source: pacpusframework/branches/2.0-beta1/include/Pacpus/PacpusTools/filtering/kalman_filtering.hpp@ 89

Last change on this file since 89 was 89, checked in by morasjul, 11 years ago

PACPUS 2.0 Beta deployed in new branch

Major changes:
-Add communication interface between components
-Add examples for communications interface (TestComponents)
-Move to Qt5 support

  • Property svn:executable set to *
File size: 15.4 KB
Line 
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
20namespace filter{
21namespace 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__
Note: See TracBrowser for help on using the repository browser.