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