[66] | 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 |
|
---|
[3] | 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 | }
|
---|
[66] | 473 | } // namespace kalman
|
---|
| 474 | } // namespace filter
|
---|
| 475 |
|
---|
| 476 | #endif // __KALMAN_FILTERING__
|
---|