sot-core
4.11.6
Hierarchical task solver plug-in for dynamic-graph.
|
Go to the documentation of this file.
12 #ifndef __SOT_KALMAN_H
13 #define __SOT_KALMAN_H
20 #include <dynamic-graph/all-signals.h>
21 #include <dynamic-graph/entity.h>
22 #include <dynamic-graph/linear-algebra.h>
29 #if defined(kalman_EXPORTS)
30 #define SOT_KALMAN_EXPORT __declspec(dllexport)
32 #define SOT_KALMAN_EXPORT __declspec(dllimport)
35 #define SOT_KALMAN_EXPORT
48 virtual const std::string &
getClassName(
void)
const {
return CLASS_NAME; }
72 return "Implementation of extended Kalman filter \n"
74 " Dynamics of the system: \n"
76 " x = f (x , u ) + w (state) \n"
79 " y = h (x ) + v (observation)\n"
85 " x = f (x , u ) (state) \n"
86 " k|k-1 k-1|k-1 k-1 \n"
89 " P = F P F + Q (covariance)\n"
90 " k|k-1 k-1 k-1|k-1 k-1 \n"
95 " F = --- (x , u ) \n"
96 " k-1 \\ k-1|k-1 k-1 \n"
108 " z = y - h (x ) (innovation)\n"
111 " S = H P H + R (innovation covariance)\n"
114 " K = P H S (Kalman gain)\n"
117 " x = x + K z (state) \n"
120 " P =(I - K H ) P \n"
124 " - input(vector)::x_pred: state prediction\n"
126 " - input(vector)::y_pred: observation prediction: h (x )\n"
128 " - input(matrix)::F: partial derivative wrt x of f\n"
129 " - input(vector)::y: measure \n"
130 " - input(matrix)::H: partial derivative wrt x of h\n"
131 " - input(matrix)::Q: variance of noise w\n"
133 " - input(matrix)::R: variance of noise v\n"
135 " - output(matrix)::P_pred: variance of prediction\n"
137 " - output(vector)::x_est: state estimation x\n"
142 Matrix &computeVarianceUpdate(Matrix &P_k_k,
const int &time);
143 Vector &computeStateUpdate(Vector &x_est,
const int &time);
146 stateEstimation_ = x0;
147 stateUpdateSOUT.recompute(0);
152 varianceUpdateSOUT.recompute(0);
185 Kalman(
const std::string &name);
187 void display(std::ostream &os)
const;
Matrix stateVariance_
Definition: kalman.hh:162
SignalPtr< Vector, int > statePredictedSIN
Definition: kalman.hh:62
void setStateVariance(const Matrix &P0)
Definition: kalman.hh:150
Definition: abstract-sot-external-interface.hh:17
SignalPtr< Matrix, int > modelMeasureSIN
Definition: kalman.hh:58
double dt
Definition: kalman.hh:53
Matrix K_
Definition: kalman.hh:182
SignalPtr< Matrix, int > noiseTransitionSIN
Definition: kalman.hh:59
Matrix S_
Definition: kalman.hh:179
virtual const std::string & getClassName(void) const
Definition: kalman.hh:48
SignalTimeDependent< Matrix, int > gainSINTERN
Definition: kalman.hh:67
#define SOT_KALMAN_EXPORT
Definition: kalman.hh:35
SignalTimeDependent< Matrix, int > varianceUpdateSOUT
Definition: kalman.hh:64
Matrix Pk_k_1_
Definition: kalman.hh:176
SignalPtr< Matrix, int > modelTransitionSIN
Definition: kalman.hh:57
SignalTimeDependent< Vector, int > stateUpdateSOUT
Definition: kalman.hh:65
static const std::string CLASS_NAME
Definition: kalman.hh:47
Vector stateEstimation_
Definition: kalman.hh:158
SignalPtr< Vector, int > observationPredictedSIN
Definition: kalman.hh:63
unsigned int size_measure
Definition: kalman.hh:52
unsigned int size_state
Definition: kalman.hh:51
Matrix FP_
Definition: kalman.hh:171
virtual std::string getDocString() const
Definition: kalman.hh:71
void setStateEstimation(const Vector &x0)
Definition: kalman.hh:145
SignalTimeDependent< Matrix, int > innovationSINTERN
Definition: kalman.hh:68
SignalPtr< Vector, int > measureSIN
Definition: kalman.hh:56
SignalPtr< Matrix, int > noiseMeasureSIN
Definition: kalman.hh:60
Vector z_
Definition: kalman.hh:167