1 | // %pacpus:license{
|
---|
2 | // This file is part of the PACPUS framework distributed under the
|
---|
3 | // CECILL-C License, Version 1.0.
|
---|
4 | // %pacpus:license}
|
---|
5 | /// @file
|
---|
6 | /// @author Firstname Surname <firstname.surname@utc.fr>
|
---|
7 | /// @date Month, Year
|
---|
8 | /// @version $Id: kalman_filtering.hpp 76 2013-01-10 17:05:10Z kurdejma $
|
---|
9 | /// @copyright Copyright (c) UTC/CNRS Heudiasyc 2006 - 2013. All rights reserved.
|
---|
10 | /// @brief Brief description.
|
---|
11 | ///
|
---|
12 | /// Detailed description.
|
---|
13 |
|
---|
14 | #ifndef __KALMAN_FILTERING__
|
---|
15 | #define __KALMAN_FILTERING__
|
---|
16 |
|
---|
17 | #include "bayes_filtering.hpp"
|
---|
18 | #include "../math/ublas.hpp"
|
---|
19 |
|
---|
20 | namespace filter{
|
---|
21 | namespace kalman{
|
---|
22 | using namespace math::ublas;
|
---|
23 |
|
---|
24 |
|
---|
25 | /*!
|
---|
26 | * \class State
|
---|
27 | * \brief This class describes a basic Kalman state
|
---|
28 | */
|
---|
29 | class State {
|
---|
30 | public :
|
---|
31 |
|
---|
32 | Vector X; /*!< State vector */
|
---|
33 |
|
---|
34 | Matrix P; /*!< Covariance matrix*/
|
---|
35 |
|
---|
36 | /*!
|
---|
37 | * \brief method where the state vector and covariance matrix are allocated
|
---|
38 | * \param state_size : the size of the state vector
|
---|
39 | */
|
---|
40 | void Allocator(const size_t &state_size){
|
---|
41 | X=ZeroVector(state_size);
|
---|
42 | P=ZeroMatrix(state_size,state_size);
|
---|
43 | }
|
---|
44 |
|
---|
45 | };
|
---|
46 |
|
---|
47 |
|
---|
48 | /*!
|
---|
49 | * \class DynamicEquation
|
---|
50 | * \brief This class describes a basic Kalman dynamic equation
|
---|
51 | */
|
---|
52 | template <class S=State> class DynamicEquation: public BayesDynamicEquation<S,S>{
|
---|
53 | public :
|
---|
54 |
|
---|
55 | /*!
|
---|
56 | * \brief virtual method where matrices of state system must be allocated
|
---|
57 | * \param state_size : the size of the state vector
|
---|
58 | * \param data_size : the size of the input vector
|
---|
59 | */
|
---|
60 | virtual void Allocator(const size_t &state_size,const size_t &data_size)=0;
|
---|
61 |
|
---|
62 | /*!
|
---|
63 | * \brief virtual method where parameters of the dynamic equation must be evaluated
|
---|
64 | * \param s : the state vector at time k-1
|
---|
65 | */
|
---|
66 | virtual void EvaluateParameters(S *s)=0;
|
---|
67 |
|
---|
68 | /*!
|
---|
69 | * \brief virtual method where the a priori state vector must be computed
|
---|
70 | * \param in the state vector at time k-1
|
---|
71 | * \param out the state vector at time k
|
---|
72 | */
|
---|
73 | virtual void Predict(S *in,S *out)=0;
|
---|
74 |
|
---|
75 | /*!
|
---|
76 | * \brief Destructor
|
---|
77 | */
|
---|
78 | virtual ~DynamicEquation(){}
|
---|
79 |
|
---|
80 | /*!
|
---|
81 | * \brief Get/Set data in input vector U
|
---|
82 | * \return vector U
|
---|
83 | */
|
---|
84 | double & U(const int &row){return _U(row);}
|
---|
85 |
|
---|
86 | /*!
|
---|
87 | * \brief Get/Set data in covariance matrix Q
|
---|
88 | * return matrix Q
|
---|
89 | */
|
---|
90 | double & Q(const int &row, const int &column){return _Q(row,column);}
|
---|
91 |
|
---|
92 | protected :
|
---|
93 | Vector _U; /*!< Input vector */
|
---|
94 |
|
---|
95 | Matrix _Q; /*!< Input covariance matrix */
|
---|
96 | };
|
---|
97 |
|
---|
98 | /*!
|
---|
99 | * \class LinearDynamicEquation
|
---|
100 | * \brief This clas dscribes a linear dynamic equation
|
---|
101 | * \n
|
---|
102 | * X(k+1|k) =A*X(k)+B*U(k) \n
|
---|
103 | * P(k+1|k) = A*P(k+1|k)*A'+B*Q(k)*B' \n
|
---|
104 | * A = state matrix \n
|
---|
105 | * B = input matrix \n
|
---|
106 | * U = input of dynamic system \n
|
---|
107 | * Q = input covariance matrix \n
|
---|
108 | */
|
---|
109 | template <class S=State> class LinearDynamicEquation : public DynamicEquation<S>{
|
---|
110 | public :
|
---|
111 | /*!
|
---|
112 | * \brief virtual method where matrices of state system must be allocated
|
---|
113 | * \param state_size : the size of the state vector
|
---|
114 | * \param data_size : the size of the input vector
|
---|
115 | */
|
---|
116 | virtual void Allocator(const size_t &state_size,const size_t &data_size);
|
---|
117 | /*!
|
---|
118 | * \brief virtual method where parameters of the dynamic equation must be evaluated
|
---|
119 | * \param s : the state vector at time k-1
|
---|
120 | */
|
---|
121 | virtual void EvaluateParameters(S *s)=0;
|
---|
122 |
|
---|
123 | /*!
|
---|
124 | * \brief virtual method where the a priori state vector must be computed
|
---|
125 | * \param in : the state vector at time k-1
|
---|
126 | * \param out : the state vector at time k
|
---|
127 | */
|
---|
128 | virtual void Predict(S *in,S *out);
|
---|
129 |
|
---|
130 | /*!
|
---|
131 | * \brief Destructor
|
---|
132 | */
|
---|
133 | virtual ~LinearDynamicEquation(){}
|
---|
134 |
|
---|
135 | /*!
|
---|
136 | * \brief Get/Set an constant variable in A matrix
|
---|
137 | * \return A(row,column)
|
---|
138 | */
|
---|
139 | double & A(const int &row, const int &column){return _A(row,column);}
|
---|
140 |
|
---|
141 | /*!
|
---|
142 | * \brief Get/Set an constant variable in B matrix
|
---|
143 | * \return B(row,column)
|
---|
144 | */
|
---|
145 | double & B(const int &row, const int &column){return _B(row,column);}
|
---|
146 |
|
---|
147 | protected :
|
---|
148 | Matrix _A; /*!< A matrix */
|
---|
149 |
|
---|
150 | Matrix _B; /*!< B matrix */
|
---|
151 |
|
---|
152 |
|
---|
153 | };
|
---|
154 |
|
---|
155 |
|
---|
156 | // Kalman linear dynamic equation member functions
|
---|
157 | template <class S> void LinearDynamicEquation<S>::Allocator(const size_t &state_size,const size_t &data_size){
|
---|
158 | DynamicEquation<S>::_U=ZeroVector(data_size);
|
---|
159 | DynamicEquation<S>::_Q=ZeroMatrix(data_size,data_size);
|
---|
160 | _A=ZeroMatrix(state_size,state_size);
|
---|
161 | _B=ZeroMatrix(state_size,data_size);
|
---|
162 |
|
---|
163 | }
|
---|
164 |
|
---|
165 | template <class S> void LinearDynamicEquation<S>::Predict(S *in,S *out){
|
---|
166 | EvaluateParameters(in);
|
---|
167 | out->P=_A*in->P*Trans(_A) +_B*DynamicEquation<S>::_Q*Trans(_B);
|
---|
168 | out->X=_A*in->X+_B*DynamicEquation<S>::_U;
|
---|
169 | }
|
---|
170 |
|
---|
171 |
|
---|
172 |
|
---|
173 | /*!
|
---|
174 | * \class NonLinearDynamicEquation
|
---|
175 | * \brief this class describes a non linear dynamic equation
|
---|
176 | * \n
|
---|
177 | * X(k+1|k) =f(X(k)+U(k)) \n
|
---|
178 | * P(k+1|k) = F*P(k+1|k)*F'+G*Q(k)*G' \n
|
---|
179 | * F= Jacobian of dynamic equation with respect of the state vector \n
|
---|
180 | * G= Jacobian of dynamic equation with respect of the entrie vector \n
|
---|
181 | * U = entrie of dynamic system \n
|
---|
182 | * Q = covariance matrix associated with U \n
|
---|
183 | */
|
---|
184 | template <class S=State> class NonLinearDynamicEquation : public DynamicEquation<S>{
|
---|
185 | public :
|
---|
186 | /*!
|
---|
187 | * \brief virtual method where matrices of state system must be allocated
|
---|
188 | * \param state_size : the size of the state vector
|
---|
189 | * \param data_size : the size of the input vector
|
---|
190 | */
|
---|
191 | virtual void Allocator(const size_t &state_size,const size_t &data_size);
|
---|
192 |
|
---|
193 | /*!
|
---|
194 | * \brief virtual method where parameters of the dynamic equation must be evaluated
|
---|
195 | * \param s : the state vector at time k-1
|
---|
196 | * f=
|
---|
197 | * F=
|
---|
198 | * G=
|
---|
199 | */
|
---|
200 |
|
---|
201 | virtual void EvaluateParameters(S *s)=0;
|
---|
202 |
|
---|
203 |
|
---|
204 | /*!
|
---|
205 | * \brief virtual method where the a priori state vector must be computed
|
---|
206 | * \param in : the state vector at time k-1
|
---|
207 | * \param out : the state vector at time k
|
---|
208 | */
|
---|
209 | virtual void Predict(S *in,S *out);
|
---|
210 |
|
---|
211 | /*!
|
---|
212 | * \brief Destructor
|
---|
213 | */
|
---|
214 | virtual ~NonLinearDynamicEquation(){}
|
---|
215 |
|
---|
216 | /*!
|
---|
217 | * \brief Get/Set a constant variable in F matrix
|
---|
218 | * return : F(row,column)
|
---|
219 | */
|
---|
220 | double & F(const int &row, const int &column){return _F(row,column);}
|
---|
221 |
|
---|
222 | /*!
|
---|
223 | * \brief Get/Set a constant variable in G matrix
|
---|
224 | * return : G(row,column)
|
---|
225 | */
|
---|
226 | double & G(const int &row, const int &column){return _G(row,column);}
|
---|
227 |
|
---|
228 |
|
---|
229 | protected:
|
---|
230 |
|
---|
231 | Vector _f; /*!< Vector f where f(X,U) is stored */
|
---|
232 |
|
---|
233 | Matrix _F; /*!< Jacobian of dynamic equation with respect of the state vector */
|
---|
234 |
|
---|
235 | Matrix _G; /*!< Jacobian of dynamic equation with respect of the input vector*/
|
---|
236 |
|
---|
237 | };
|
---|
238 |
|
---|
239 |
|
---|
240 | // Kalman non linear dynamic equation member functions
|
---|
241 | template <class S> void NonLinearDynamicEquation<S>::Allocator(const size_t &state_size,const size_t &data_size){
|
---|
242 | DynamicEquation<S>::_U=ZeroVector(data_size);
|
---|
243 | DynamicEquation<S>::_Q=ZeroMatrix(data_size,data_size);
|
---|
244 | _f=ZeroVector(state_size);
|
---|
245 | _F=ZeroMatrix(state_size,state_size);
|
---|
246 | _G=ZeroMatrix(state_size,data_size);
|
---|
247 | }
|
---|
248 |
|
---|
249 | template <class S> void NonLinearDynamicEquation<S>::Predict(S *in,S *out){
|
---|
250 | EvaluateParameters(in);
|
---|
251 | out->X=_f;
|
---|
252 | out->P=_F*in->P*Trans(_F) + _G*DynamicEquation<S>::_Q*Trans(_G);
|
---|
253 | }
|
---|
254 |
|
---|
255 |
|
---|
256 |
|
---|
257 | /*!
|
---|
258 | * \class MeasureEquation
|
---|
259 | * \brief This class describes a basic Kalman measure equation
|
---|
260 | */
|
---|
261 | template <class S=State> class MeasureEquation: public BayesMeasureEquation<S,S>{
|
---|
262 | public :
|
---|
263 |
|
---|
264 |
|
---|
265 | /*!
|
---|
266 | * \brief virtual method where matrices of state system must be allocated
|
---|
267 | * \param state_size : the size of the state vector
|
---|
268 | * \param data_size : the size of the observation vector
|
---|
269 | */
|
---|
270 | virtual void Allocator(const size_t &state_size,const size_t &data_size)=0;
|
---|
271 |
|
---|
272 | /*!
|
---|
273 | * \brief virtual method where parameters of the dynamic equation must be evaluated
|
---|
274 | * \param s the state vector at time k
|
---|
275 | */
|
---|
276 | virtual void EvaluateParameters(S *s)=0;
|
---|
277 |
|
---|
278 | /*!
|
---|
279 | * \brief virtual method where the a posteriori state vector must be computed
|
---|
280 | * \param in : the a priori state vector at time k
|
---|
281 | * \param out : the a posteriori state vector at time k
|
---|
282 | */
|
---|
283 | virtual void Update(S *in,S *out)=0;
|
---|
284 |
|
---|
285 | /*!
|
---|
286 | * \brief Destructor
|
---|
287 | */
|
---|
288 | virtual ~MeasureEquation(){};
|
---|
289 |
|
---|
290 | /*!
|
---|
291 | * \brief Get/Set data in observation vector Z
|
---|
292 | * \return Z(row,column)
|
---|
293 | */
|
---|
294 | double & Z(const int &row){return _Z(row);}
|
---|
295 |
|
---|
296 | /*!
|
---|
297 | * \brief Get/Set data in observation covariane matrix R
|
---|
298 | * return R(row,column)
|
---|
299 | */
|
---|
300 | double & R(const int &row,const int &column){return _R(row,column);}
|
---|
301 |
|
---|
302 | /*!
|
---|
303 | * \brief Get/Set observation vector Z
|
---|
304 | * return vector Z
|
---|
305 | */
|
---|
306 | Vector & Z(){return _Z;}
|
---|
307 | /*!
|
---|
308 | * \brief Get/Set observation covariance matrix R
|
---|
309 | * return matrix R
|
---|
310 | */
|
---|
311 | Matrix & R(){return _R;}
|
---|
312 |
|
---|
313 |
|
---|
314 | protected :
|
---|
315 |
|
---|
316 | Vector _Z; /*!< Observation vector */
|
---|
317 |
|
---|
318 | Matrix _R; /*!< Observation covariance matrix */
|
---|
319 |
|
---|
320 | Matrix _K; /*!< Kalman gain matrix */
|
---|
321 |
|
---|
322 |
|
---|
323 | /*!
|
---|
324 | * \brief Coherency of observation data \n
|
---|
325 | * This parameter must be computed in the EvaluateParameters method \n
|
---|
326 | * For kalman filtering the mahalanobis distance is usually used \n
|
---|
327 | * By default the coherency value is true \n
|
---|
328 | */
|
---|
329 | bool _coherency;
|
---|
330 | };
|
---|
331 |
|
---|
332 |
|
---|
333 |
|
---|
334 | /*!
|
---|
335 | * \class LinearMeasureEquation
|
---|
336 | * \brief This class describes a linear measure equation \n
|
---|
337 | * X(k+1|k+1)=X(k+1|k)+K(Z(k)-H*X(k+1|k)) \n
|
---|
338 | * P(k+1|k+1) = (I-K*H)P(k+1|k) \n
|
---|
339 | * K=P(k+1|k)*H'/(H*P(k+1|k)*H'+R) \n
|
---|
340 | * H = observation matrix \n
|
---|
341 | * Z = observation data \n
|
---|
342 | * Q = observation covariance matrix \n
|
---|
343 | */
|
---|
344 | template<class S=State> class LinearMeasureEquation : public MeasureEquation<S>{
|
---|
345 | public :
|
---|
346 | /*!
|
---|
347 | * \brief virtual method where matrices of state system must be allocated
|
---|
348 | * \param state_size : the size of the state vector
|
---|
349 | * \param data_size : the size of the observation vector
|
---|
350 | */
|
---|
351 | virtual void Allocator(const size_t &state_size,const size_t &data_size);
|
---|
352 |
|
---|
353 | /*!
|
---|
354 | * \brief virtual method where parameters of the dynamic equation must be evaluated
|
---|
355 | * \param s : the state vector at time k
|
---|
356 | */
|
---|
357 | virtual void EvaluateParameters(S *s)=0;
|
---|
358 |
|
---|
359 | /*!
|
---|
360 | * \brief virtual method where the a posteriori state vector must be computed
|
---|
361 | * \param in : the a priori state vector at time k
|
---|
362 | * \param out : the a posteriori state vector at time k
|
---|
363 | */
|
---|
364 | virtual void Update(S *in,S *out);
|
---|
365 |
|
---|
366 | /*!
|
---|
367 | * \brief Destructor
|
---|
368 | */
|
---|
369 | virtual ~LinearMeasureEquation(){}
|
---|
370 |
|
---|
371 | /*!
|
---|
372 | * \brief Get/Set an constant variable in observation matrix H
|
---|
373 | */
|
---|
374 | double & H(int row, int column){return _H(row,column);}
|
---|
375 |
|
---|
376 | protected:
|
---|
377 |
|
---|
378 | Matrix _H; /*!< Observation matrix */
|
---|
379 |
|
---|
380 | };
|
---|
381 |
|
---|
382 |
|
---|
383 | // Kalman linear measure equation member functions
|
---|
384 | template <class S> void LinearMeasureEquation<S>::Allocator(const size_t &state_size,const size_t &data_size){
|
---|
385 | MeasureEquation<S>::_Z=ZeroVector(data_size);
|
---|
386 | MeasureEquation<S>::_R=ZeroMatrix(data_size,data_size);
|
---|
387 | _H=ZeroMatrix(data_size,state_size);
|
---|
388 | MeasureEquation<S>::_coherency=true;
|
---|
389 | }
|
---|
390 |
|
---|
391 | template <class S> void LinearMeasureEquation<S>::Update(S *in,S *out){
|
---|
392 | MeasureEquation<S>::_coherency=true;
|
---|
393 | EvaluateParameters(in);
|
---|
394 |
|
---|
395 | if(MeasureEquation<S>::_coherency){
|
---|
396 | MeasureEquation<S>::_K=in->P*Trans(_H) *InvQR( _H*in->P*Trans(_H) +MeasureEquation<S>::_R );
|
---|
397 | out->X=in->X + MeasureEquation<S>::_K*(MeasureEquation<S>::_Z - _H*in->X);
|
---|
398 | out->P=in->P - MeasureEquation<S>::_K*_H*in->P;
|
---|
399 | }
|
---|
400 | }
|
---|
401 |
|
---|
402 |
|
---|
403 | /*!
|
---|
404 | * \class NonLinearMeasureEquation
|
---|
405 | * \brief This clas describes a non linear measure equation \n
|
---|
406 | * X(k+1|k+1)=X(k+1|k)+K(Z(k)-h(X(k+1|k))) \n
|
---|
407 | * P(k+1|k+1) = (I-K*H)P(k+1|k) \n
|
---|
408 | * K=P(k+1|k)*H'/(H*P(k+1|k)*H'+R) \n
|
---|
409 | * H = jacobian matrix of h() with respect of X \n
|
---|
410 | * Z = observation data \n
|
---|
411 | * Q = covariance matrix associated with Z \n
|
---|
412 | */
|
---|
413 | template <class S=State> class NonLinearMeasureEquation : public MeasureEquation<S>{
|
---|
414 | public :
|
---|
415 | /*!
|
---|
416 | * \brief virtual method where matrices of state system must be allocated
|
---|
417 | * \param state_size : the size of the state vector
|
---|
418 | * \param data_size : the size of the observation vector
|
---|
419 | */
|
---|
420 | virtual void Allocator(const size_t &state_size,const size_t &data_size);
|
---|
421 |
|
---|
422 | /*!
|
---|
423 | * \brief virtual method where parameters of the dynamic equation must be evaluated
|
---|
424 | * \param s : the state vector at time k
|
---|
425 | * h=
|
---|
426 | * H=
|
---|
427 | */
|
---|
428 | virtual void EvaluateParameters(S *s )=0;
|
---|
429 |
|
---|
430 | /*!
|
---|
431 | * \brief virtual method where the a posteriori state vector must be computed
|
---|
432 | * \param in : the a priori state vector at time k
|
---|
433 | * \param out : the a posteriori state vector at time k
|
---|
434 | */
|
---|
435 | virtual void Update(S *in,S *out);
|
---|
436 |
|
---|
437 | /** destructor */
|
---|
438 | virtual ~NonLinearMeasureEquation(){}
|
---|
439 |
|
---|
440 | /*!
|
---|
441 | * \brief Get/Set constant data in H matrix
|
---|
442 | * \return H(row,column)
|
---|
443 | */
|
---|
444 | double & H(const int &row,const int &column){return _H(row,column);}
|
---|
445 |
|
---|
446 | protected :
|
---|
447 |
|
---|
448 |
|
---|
449 |
|
---|
450 | Matrix _H; /*!< Jacobian of measure equation with respect of the state vector */
|
---|
451 |
|
---|
452 | Vector _h; /*!< Vector f where h(X) is stored */
|
---|
453 |
|
---|
454 | };
|
---|
455 |
|
---|
456 | // Kalman non linear measure equation member functions
|
---|
457 | template <class S> void NonLinearMeasureEquation<S>::Allocator(const size_t &state_size,const size_t &data_size){
|
---|
458 | MeasureEquation<S>::_Z=ZeroVector(data_size);
|
---|
459 | MeasureEquation<S>::_R=ZeroMatrix(data_size,data_size);
|
---|
460 | _H=ZeroMatrix(data_size,state_size);
|
---|
461 | _h=ZeroVector(data_size);
|
---|
462 | }
|
---|
463 |
|
---|
464 | template <class S> void NonLinearMeasureEquation<S>::Update(S *in,S *out){
|
---|
465 | MeasureEquation<S>::_coherency=true;
|
---|
466 | EvaluateParameters(in);
|
---|
467 |
|
---|
468 | if(MeasureEquation<S>::_coherency){
|
---|
469 | MeasureEquation<S>::_K=in->P*Trans(_H) *( InvLU(_H*in->P*Trans(_H) + MeasureEquation<S>::_R));
|
---|
470 | out->X=in->X+MeasureEquation<S>::_K*(MeasureEquation<S>::_Z-_h);
|
---|
471 | out->P=in->P-MeasureEquation<S>::_K*_H*in->P;
|
---|
472 | }
|
---|
473 | }
|
---|
474 | } // namespace kalman
|
---|
475 | } // namespace filter
|
---|
476 |
|
---|
477 | #endif // __KALMAN_FILTERING__
|
---|