X-Git-Url: https://git.creatis.insa-lyon.fr/pubgit/?a=blobdiff_plain;f=lib%2FcpExtensions%2FAlgorithms%2FKalmanFilter.h;h=bd0c17a701a5505bbc7d93c43459ee916a44ae46;hb=a3cae0375976d524c957493a3cc565b9f5b20bce;hp=a96c6208c116e741bfc4d7c51670459bf66fb729;hpb=506b27e2ea07cb34ba230b02eb6857b20cbf78b1;p=cpPlugins.git diff --git a/lib/cpExtensions/Algorithms/KalmanFilter.h b/lib/cpExtensions/Algorithms/KalmanFilter.h index a96c620..bd0c17a 100644 --- a/lib/cpExtensions/Algorithms/KalmanFilter.h +++ b/lib/cpExtensions/Algorithms/KalmanFilter.h @@ -130,83 +130,85 @@ namespace cpExtensions void Configure( unsigned int s, unsigned int i, unsigned int m ); - /// Iteration methods - virtual void Initialize( ); - virtual void Predict( ); - virtual void Innovate( ); - virtual void Filtrate( ); - virtual void OneStep( ) + // Iteration methods + virtual void Initialize( ); + virtual void Predict( ); + virtual void Innovate( ); + virtual void Filtrate( ); + virtual void OneStep( ) { this->Predict( ); this->Innovate( ); this->Filtrate( ); } - virtual void NSteps( unsigned int n ) + virtual void NSteps( unsigned int n ) { for( unsigned int i = 0; i < n; i++ ) this->OneStep( ); } - unsigned int CurrentIteration( ) const + unsigned int CurrentIteration( ) const { return( this->m_I ); } - unsigned char CurrentStep( ) const + unsigned char CurrentStep( ) const { return( this->m_Step ); } protected: - KalmanFilter( ); - virtual ~KalmanFilter( ); + KalmanFilter( ); + virtual ~KalmanFilter( ); private: - // Purposely not implemented - KalmanFilter( const Self& ); - void operator=( const Self& ); + // Purposely not implemented + KalmanFilter( const Self& ); + void operator=( const Self& ); protected: - // Filter dimensions - unsigned int m_StateSize; - unsigned int m_InputSize; - unsigned int m_MeasureSize; - - // Transition matrices - TMatrix m_A; /// Transition - TMatrix m_B; /// Input control - TMatrix m_H; /// Measure - TMatrix m_Id; /// Identity matrix - - // Noise matrices - TMatrix m_Q; /// Process noise covariance - TMatrix m_R; /// Measure noise covariance - - // Initial values - TVector m_x0; /// Initial state - TMatrix m_P0; /// Initial error covariance - - // Loop vectors - TVector m_u; /// Last real input - TVector m_m; /// Last real measure - TVector m_xm; /// A priori state - TVector m_xp; /// A posteriori state - - // Loop matrices - TMatrix m_K; /// kalman gain - TMatrix m_Pm; /// A priori error - TMatrix m_Pp; /// A posteriori error - - // Loop values - unsigned int m_I; /// Current iteration - unsigned char m_Step; /// Current step within current iteration - - // ----------------------------------------------------------------- - // Classic kronecker product operator - // ----------------------------------------------------------------- - template< class M > - static void Kronecker( M& AkB, const M& A, const M& B ); + // Filter dimensions + unsigned int m_StateSize; + unsigned int m_InputSize; + unsigned int m_MeasureSize; + + // Transition matrices + TMatrix m_A; // Transition + TMatrix m_B; // Input control + TMatrix m_H; // Measure + TMatrix m_Id; // Identity matrix + + // Noise matrices + TMatrix m_Q; // Process noise covariance + TMatrix m_R; // Measure noise covariance + + // Initial values + TVector m_x0; // Initial state + TMatrix m_P0; // Initial error covariance + + // Loop vectors + TVector m_u; // Last real input + TVector m_m; // Last real measure + TVector m_xm; // A priori state + TVector m_xp; // A posteriori state + + // Loop matrices + TMatrix m_K; // kalman gain + TMatrix m_Pm; // A priori error + TMatrix m_Pp; // A posteriori error + + // Loop values + unsigned int m_I; // Current iteration + unsigned char m_Step; // Current step within current iteration + + // ------------------------------------------------------------------- + // Classic kronecker product operator + // ------------------------------------------------------------------- + template< class M > + static void Kronecker( M& AkB, const M& A, const M& B ); }; } // ecapseman } // ecapseman +#ifndef ITK_MANUAL_INSTANTIATION #include +#endif // ITK_MANUAL_INSTANTIATION #endif // __CPEXTENSIONS__ALGORITHMS__KALMANFILTER__H__