kalman.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2010,
3  * Nicolas Mansard,
4  * François Bleibel,
5  * Olivier Stasse,
6  * Florent Lamiraux
7  *
8  * CNRS/AIST
9  *
10  */
11 
12 /* --- SOT --- */
14 #include <dynamic-graph/factory.h>
15 
16 #include <sot/core/debug.hh>
18 #include <sot/core/factory.hh>
19 #include <sot/core/kalman.hh> /* Header of the class implemented here. */
20 
21 namespace dynamicgraph {
22 using command::Setter;
23 namespace sot {
24 
25 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Kalman, "Kalman");
26 
27 Kalman::Kalman(const std::string &name)
28  : Entity(name),
29  measureSIN(NULL, "Kalman(" + name + ")::input(vector)::y"),
30  modelTransitionSIN(NULL, "Kalman(" + name + ")::input(matrix)::F"),
31  modelMeasureSIN(NULL, "Kalman(" + name + ")::input(matrix)::H"),
32  noiseTransitionSIN(NULL, "Kalman(" + name + ")::input(matrix)::Q"),
33  noiseMeasureSIN(NULL, "Kalman(" + name + ")::input(matrix)::R")
34 
35  ,
36  statePredictedSIN(0, "Kalman(" + name + ")::input(vector)::x_pred"),
37  observationPredictedSIN(0, "Kalman(" + name + ")::input(vector)::y_pred"),
38  varianceUpdateSOUT("Kalman(" + name + ")::output(vector)::P"),
39  stateUpdateSOUT("Kalman(" + name + ")::output(vector)::x_est"),
40  stateEstimation_(),
41  stateVariance_() {
42  sotDEBUGIN(15);
44  boost::bind(&Kalman::computeVarianceUpdate, this, _1, _2));
46  boost::bind(&Kalman::computeStateUpdate, this, _1, _2));
47 
52 
53  std::string docstring =
54  " Set initial state estimation\n"
55  "\n"
56  " input:\n"
57  " - a vector: initial state\n";
58  addCommand("setInitialState",
60  docstring));
61 
62  docstring =
63  " Set variance of initial state estimation\n"
64  "\n"
65  " input:\n"
66  " - a matrix: variance covariance matrix\n";
67  addCommand(
68  "setInitialVariance",
69  new Setter<Kalman, Matrix>(*this, &Kalman::setStateVariance, docstring));
70  sotDEBUGOUT(15);
71 }
72 
73 Matrix &Kalman::computeVarianceUpdate(Matrix &Pk_k, const int &time) {
74  sotDEBUGIN(15);
75  if (time == 0) {
76  // First time return variance initial state
77  Pk_k = stateVariance_;
78  // Set dependency to input signals for latter computations
81  } else {
82  const Matrix &Q = noiseTransitionSIN(time);
83  const Matrix &R = noiseMeasureSIN(time);
84  const Matrix &F = modelTransitionSIN(time);
85  const Matrix &H = modelMeasureSIN(time);
86  const Matrix &Pk_1_k_1 = stateVariance_;
87 
88  sotDEBUG(15) << "Q=" << Q << std::endl;
89  sotDEBUG(15) << "R=" << R << std::endl;
90  sotDEBUG(15) << "F=" << F << std::endl;
91  sotDEBUG(15) << "H=" << H << std::endl;
92  sotDEBUG(15) << "Pk_1_k_1=" << Pk_1_k_1 << std::endl;
93 
94  FP_ = F * Pk_1_k_1;
95  Pk_k_1_ = FP_ * F.transpose();
96  Pk_k_1_ += Q;
97 
98  sotDEBUG(15) << "F " << std::endl << F << std::endl;
99  sotDEBUG(15) << "P_{k-1|k-1} " << std::endl << Pk_1_k_1 << std::endl;
100  sotDEBUG(15) << "F^T " << std::endl << F.transpose() << std::endl;
101  sotDEBUG(15) << "P_{k|k-1} " << std::endl << Pk_k_1_ << std::endl;
102 
103  S_ = H * Pk_k_1_ * H.transpose() + R;
104  K_ = Pk_k_1_ * H.transpose() * S_.inverse();
105  Pk_k = Pk_k_1_ - K_ * H * Pk_k_1_;
106 
107  sotDEBUG(15) << "S_{k} " << std::endl << S_ << std::endl;
108  sotDEBUG(15) << "K_{k} " << std::endl << K_ << std::endl;
109  sotDEBUG(15) << "P_{k|k} " << std::endl << Pk_k << std::endl;
110 
111  sotDEBUGOUT(15);
112 
113  stateVariance_ = Pk_k;
114  }
115  return Pk_k;
116 }
117 
118 // ^
119 // z = y - h (x )
120 // k k k|k-1
121 //
122 // ^ ^
123 // x = x + K z
124 // k|k k|k-1 k k
125 //
126 // T
127 // S = H P H + R
128 // k k k|k-1 k
129 //
130 // T -1
131 // K = P H S
132 // k k|k-1 k k
133 //
134 // P = (I - K H ) P
135 // k|k k k k|k-1
136 
137 Vector &Kalman::computeStateUpdate(Vector &x_est, const int &time) {
138  sotDEBUGIN(15);
139  if (time == 0) {
140  // First time return variance initial state
141  x_est = stateEstimation_;
142  // Set dependency to input signals for latter computations
150  } else {
152  const Vector &x_pred = statePredictedSIN(time);
153  const Vector &y_pred = observationPredictedSIN(time);
154  const Vector &y = measureSIN(time);
155 
156  sotDEBUG(25) << "K_{k} = " << std::endl << K_ << std::endl;
157  sotDEBUG(25) << "y = " << y << std::endl;
158  sotDEBUG(25) << "h (\\hat{x}_{k|k-1}) = " << y_pred << std::endl;
159  sotDEBUG(25) << "y = " << y << std::endl;
160 
161  // Innovation: z_ = y - Hx
162  z_ = y - y_pred;
163  // x_est = x_pred + (K*(y-(H*x_pred)));
164  x_est = x_pred + K_ * z_;
165 
166  sotDEBUG(25) << "z_{k} = " << z_ << std::endl;
167  sotDEBUG(25) << "x_{k|k} = " << x_est << std::endl;
168 
169  stateEstimation_ = x_est;
170  }
171  sotDEBUGOUT(15);
172  return x_est;
173 }
174 
175 /* -------------------------------------------------------------------------- */
176 /* --- MODELE --------------------------------------------------------------- */
177 /* -------------------------------------------------------------------------- */
178 
179 void Kalman::display(std::ostream &) const {}
180 
181 } // namespace sot
182 } // namespace dynamicgraph
183 
Kalman(const std::string &name)
Definition: kalman.cpp:27
Eigen::VectorXd Vector
y
void setStateVariance(const Matrix &P0)
Definition: kalman.hh:151
SignalPtr< Vector, int > statePredictedSIN
Definition: kalman.hh:63
void signalRegistration(const SignalArray< int > &signals)
#define sotDEBUGOUT(level)
Definition: debug.hh:212
Q
SignalPtr< Matrix, int > modelMeasureSIN
Definition: kalman.hh:59
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePosture, "FeaturePosture")
#define sotDEBUGIN(level)
Definition: debug.hh:211
def H
SignalPtr< Matrix, int > noiseTransitionSIN
Definition: kalman.hh:60
void display(std::ostream &os) const
Definition: kalman.cpp:179
SignalPtr< Matrix, int > modelTransitionSIN
Definition: kalman.hh:58
virtual void addDependency(const SignalBase< Time > &signal)
SignalTimeDependent< Matrix, int > varianceUpdateSOUT
Definition: kalman.hh:65
R
SignalPtr< Vector, int > observationPredictedSIN
Definition: kalman.hh:64
#define sotDEBUG(level)
Definition: debug.hh:165
SignalTimeDependent< Vector, int > stateUpdateSOUT
Definition: kalman.hh:66
virtual void recompute(const Time &t)
Eigen::MatrixXd Matrix
Vector & computeStateUpdate(Vector &x_est, const int &time)
Definition: kalman.cpp:137
SignalPtr< Matrix, int > noiseMeasureSIN
Definition: kalman.hh:61
Matrix & computeVarianceUpdate(Matrix &P_k_k, const int &time)
Definition: kalman.cpp:73
void addCommand(const std::string &name, command::Command *command)
void setStateEstimation(const Vector &x0)
Definition: kalman.hh:146
SignalPtr< Vector, int > measureSIN
Definition: kalman.hh:57
virtual void setFunction(boost::function2< T &, T &, Time > t, Mutex *mutexref=NULL)


sot-core
Author(s): Olivier Stasse, ostasse@laas.fr
autogenerated on Wed Jun 21 2023 02:51:26