sot-core  4.11.4
Hierarchical task solver plug-in for dynamic-graph.
kalman.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2010, 2011, 2012
3  * Nicolas Mansard,
4  * François Bleibel,
5  * Olivier Stasse,
6  * Florent Lamiraux
7  *
8  * CNRS/AIST
9  *
10  */
11 
12 #ifndef __SOT_KALMAN_H
13 #define __SOT_KALMAN_H
14 
15 /* -------------------------------------------------------------------------- */
16 /* --- INCLUDE -------------------------------------------------------------- */
17 /* -------------------------------------------------------------------------- */
18 
19 #include <Eigen/LU>
20 #include <dynamic-graph/all-signals.h>
21 #include <dynamic-graph/entity.h>
22 #include <dynamic-graph/linear-algebra.h>
23 
24 /* -------------------------------------------------------------------------- */
25 /* --- API ------------------------------------------------------------------ */
26 /* -------------------------------------------------------------------------- */
27 
28 #if defined(WIN32)
29 #if defined(kalman_EXPORTS)
30 #define SOT_KALMAN_EXPORT __declspec(dllexport)
31 #else
32 #define SOT_KALMAN_EXPORT __declspec(dllimport)
33 #endif
34 #else
35 #define SOT_KALMAN_EXPORT
36 #endif
37 
38 /* -------------------------------------------------------------------------- */
39 /* --- CLASSE --------------------------------------------------------------- */
40 /* -------------------------------------------------------------------------- */
41 
42 namespace dynamicgraph {
43 namespace sot {
44 
45 class SOT_KALMAN_EXPORT Kalman : public Entity {
46 public:
47  static const std::string CLASS_NAME;
48  virtual const std::string &getClassName(void) const { return CLASS_NAME; }
49 
50 protected:
51  unsigned int size_state;
52  unsigned int size_measure;
53  double dt;
54 
55 public:
56  SignalPtr<Vector, int> measureSIN; // y
57  SignalPtr<Matrix, int> modelTransitionSIN; // F
58  SignalPtr<Matrix, int> modelMeasureSIN; // H
59  SignalPtr<Matrix, int> noiseTransitionSIN; // Q
60  SignalPtr<Matrix, int> noiseMeasureSIN; // R
61 
62  SignalPtr<Vector, int> statePredictedSIN; // x_{k|k-1}
63  SignalPtr<Vector, int> observationPredictedSIN; // y_pred = h (x_{k|k-1})
64  SignalTimeDependent<Matrix, int> varianceUpdateSOUT; // P
65  SignalTimeDependent<Vector, int> stateUpdateSOUT; // X_est
66 
67  SignalTimeDependent<Matrix, int> gainSINTERN; // K
68  SignalTimeDependent<Matrix, int> innovationSINTERN; // S
69 
70 public:
71  virtual std::string getDocString() const {
72  return "Implementation of extended Kalman filter \n"
73  "\n"
74  " Dynamics of the system: \n"
75  "\n"
76  " x = f (x , u ) + w (state) \n"
77  " k k-1 k-1 k-1 \n"
78  "\n"
79  " y = h (x ) + v (observation)\n"
80  " k k k \n"
81  "\n"
82  " Prediction:\n"
83  "\n"
84  " ^ ^ \n"
85  " x = f (x , u ) (state) \n"
86  " k|k-1 k-1|k-1 k-1 \n"
87  "\n"
88  " T \n"
89  " P = F P F + Q (covariance)\n"
90  " k|k-1 k-1 k-1|k-1 k-1 \n"
91  "\n"
92  " with\n"
93  " \\ \n"
94  " d f ^ \n"
95  " F = --- (x , u ) \n"
96  " k-1 \\ k-1|k-1 k-1 \n"
97  " d x \n"
98  "\n"
99  " \\ \n"
100  " d h ^ \n"
101  " H = --- (x ) \n"
102  " k \\ k-1|k-1 \n"
103  " d x \n"
104 
105  " Update:\n"
106  "\n"
107  " ^ \n"
108  " z = y - h (x ) (innovation)\n"
109  " k k k|k-1 \n"
110  " T \n"
111  " S = H P H + R (innovation covariance)\n"
112  " k k k|k-1 k \n"
113  " T -1 \n"
114  " K = P H S (Kalman gain)\n"
115  " k k|k-1 k k \n"
116  " ^ ^ \n"
117  " x = x + K z (state) \n"
118  " k|k k|k-1 k k \n"
119  "\n"
120  " P =(I - K H ) P \n"
121  " k|k k k k|k-1 \n"
122  "\n"
123  " Signals\n"
124  " - input(vector)::x_pred: state prediction\n"
125  " ^\n"
126  " - input(vector)::y_pred: observation prediction: h (x )\n"
127  " k|k-1\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"
132  " k-1\n"
133  " - input(matrix)::R: variance of noise v\n"
134  " k\n"
135  " - output(matrix)::P_pred: variance of prediction\n"
136  " ^\n"
137  " - output(vector)::x_est: state estimation x\n"
138  " k|k\n";
139  }
140 
141 protected:
142  Matrix &computeVarianceUpdate(Matrix &P_k_k, const int &time);
143  Vector &computeStateUpdate(Vector &x_est, const int &time);
144 
145  void setStateEstimation(const Vector &x0) {
146  stateEstimation_ = x0;
147  stateUpdateSOUT.recompute(0);
148  }
149 
150  void setStateVariance(const Matrix &P0) {
151  stateVariance_ = P0;
152  varianceUpdateSOUT.recompute(0);
153  }
154  // Current state estimation
155  // ^
156  // x
157  // k-1|k-1
159  // Variance of current state estimation
160  // P
161  // k-1|k-1
163 
164  // ^
165  // Innovation: z = y - H x
166  // k k k k|k-1
167  Vector z_;
168 
169  // F P
170  // k-1 k-1|k-1
171  Matrix FP_;
172 
173  // Variance prediction
174  // P
175  // k|k-1
176  Matrix Pk_k_1_;
177 
178  // Innovation covariance
179  Matrix S_;
180 
181  // Kalman Gain
182  Matrix K_;
183 
184 public:
185  Kalman(const std::string &name);
186  /* --- Entity --- */
187  void display(std::ostream &os) const;
188 };
189 
190 } // namespace sot
191 } // namespace dynamicgraph
192 
198 #endif
dynamicgraph::sot::Kalman
Definition: kalman.hh:45
dynamicgraph::sot::Kalman::stateVariance_
Matrix stateVariance_
Definition: kalman.hh:162
dynamicgraph::sot::Kalman::statePredictedSIN
SignalPtr< Vector, int > statePredictedSIN
Definition: kalman.hh:62
dynamicgraph::sot::Kalman::setStateVariance
void setStateVariance(const Matrix &P0)
Definition: kalman.hh:150
dynamicgraph
Definition: abstract-sot-external-interface.hh:17
dynamicgraph::sot::Kalman::modelMeasureSIN
SignalPtr< Matrix, int > modelMeasureSIN
Definition: kalman.hh:58
dynamicgraph::sot::Kalman::dt
double dt
Definition: kalman.hh:53
dynamicgraph::sot::Kalman::K_
Matrix K_
Definition: kalman.hh:182
dynamicgraph::sot::Kalman::noiseTransitionSIN
SignalPtr< Matrix, int > noiseTransitionSIN
Definition: kalman.hh:59
dynamicgraph::sot::Kalman::S_
Matrix S_
Definition: kalman.hh:179
dynamicgraph::sot::Kalman::getClassName
virtual const std::string & getClassName(void) const
Definition: kalman.hh:48
dynamicgraph::sot::Kalman::gainSINTERN
SignalTimeDependent< Matrix, int > gainSINTERN
Definition: kalman.hh:67
SOT_KALMAN_EXPORT
#define SOT_KALMAN_EXPORT
Definition: kalman.hh:35
dynamicgraph::sot::Kalman::varianceUpdateSOUT
SignalTimeDependent< Matrix, int > varianceUpdateSOUT
Definition: kalman.hh:64
dynamicgraph::sot::Kalman::Pk_k_1_
Matrix Pk_k_1_
Definition: kalman.hh:176
dynamicgraph::sot::Kalman::modelTransitionSIN
SignalPtr< Matrix, int > modelTransitionSIN
Definition: kalman.hh:57
dynamicgraph::sot::Kalman::stateUpdateSOUT
SignalTimeDependent< Vector, int > stateUpdateSOUT
Definition: kalman.hh:65
dynamicgraph::sot::Kalman::CLASS_NAME
static const std::string CLASS_NAME
Definition: kalman.hh:47
dynamicgraph::sot::Kalman::stateEstimation_
Vector stateEstimation_
Definition: kalman.hh:158
dynamicgraph::sot::Kalman::observationPredictedSIN
SignalPtr< Vector, int > observationPredictedSIN
Definition: kalman.hh:63
dynamicgraph::sot::Kalman::size_measure
unsigned int size_measure
Definition: kalman.hh:52
dynamicgraph::sot::Kalman::size_state
unsigned int size_state
Definition: kalman.hh:51
dynamicgraph::sot::Kalman::FP_
Matrix FP_
Definition: kalman.hh:171
dynamicgraph::sot::Kalman::getDocString
virtual std::string getDocString() const
Definition: kalman.hh:71
dynamicgraph::sot::Kalman::setStateEstimation
void setStateEstimation(const Vector &x0)
Definition: kalman.hh:145
dynamicgraph::sot::Kalman::innovationSINTERN
SignalTimeDependent< Matrix, int > innovationSINTERN
Definition: kalman.hh:68
dynamicgraph::sot::Kalman::measureSIN
SignalPtr< Vector, int > measureSIN
Definition: kalman.hh:56
dynamicgraph::sot::Kalman::noiseMeasureSIN
SignalPtr< Matrix, int > noiseMeasureSIN
Definition: kalman.hh:60
dynamicgraph::sot::Kalman::z_
Vector z_
Definition: kalman.hh:167