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