[89] | 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__
|
---|