X-Git-Url: https://git.creatis.insa-lyon.fr/pubgit/?a=blobdiff_plain;f=lib%2FcpExtensions%2FAlgorithms%2FKalmanFilter.h;h=003962087b696126875ad45ebe0bbdae0ed6ab7e;hb=8eaa2cf759c39233848cdd5b8346faccbea4c172;hp=de6090f98c72353c031905547cae6f7bf8a95eab;hpb=2361f4f97631e09d88d8a5510a369817dcaa19db;p=cpPlugins.git diff --git a/lib/cpExtensions/Algorithms/KalmanFilter.h b/lib/cpExtensions/Algorithms/KalmanFilter.h index de6090f..0039620 100644 --- a/lib/cpExtensions/Algorithms/KalmanFilter.h +++ b/lib/cpExtensions/Algorithms/KalmanFilter.h @@ -5,7 +5,7 @@ #ifndef __CPEXTENSIONS__ALGORITHMS__KALMANFILTER__H__ #define __CPEXTENSIONS__ALGORITHMS__KALMANFILTER__H__ -#include +#include #include #include @@ -13,8 +13,8 @@ #include // ------------------------------------------------------------------------- -#define kalmanGetSetMacro( type, name ) \ - itkGetConstMacro( name, type ); \ +#define kalmanGetSetMacro( type, name ) \ + itkGetConstMacro( name, type ); \ itkSetMacro( name, type ); // ------------------------------------------------------------------------- @@ -48,161 +48,161 @@ kalmanSetVectorMacro( var, name ); namespace cpExtensions +{ + namespace Algorithms { - namespace Algorithms + /** + * Abstract class implementing the classical kalman filter. See + * http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf + * for a description of this algorithm. + */ + template< typename T > + class cpExtensions_EXPORT KalmanFilter + : public itk::Object { - /** - * Abstract class implementing the classical kalman filter. See - * http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf - * for a description of this algorithm. - */ - template< typename T > - class cpExtensions_EXPORT KalmanFilter - : public itk::Object - { - public: - typedef KalmanFilter Self; - typedef itk::Object Superclass; - typedef itk::SmartPointer< Self > Pointer; - typedef itk::SmartPointer< const Self > ConstPointer; + public: + typedef KalmanFilter Self; + typedef itk::Object Superclass; + typedef itk::SmartPointer< Self > Pointer; + typedef itk::SmartPointer< const Self > ConstPointer; - // Template parameters types - typedef T TScalar; + // Template parameters types + typedef T TScalar; - // VNL types - typedef vnl_matrix< TScalar > TMatrix; - typedef vnl_vector< TScalar > TVector; + // VNL types + typedef vnl_matrix< TScalar > TMatrix; + typedef vnl_vector< TScalar > TVector; - // stage identificators - enum - { - StPred, - StInno, - StFilt - }; - - public: - itkNewMacro( Self ); - itkTypeMacro( KalmanFilter, itkObject ); - - // Values - itkGetConstMacro( StateSize, unsigned int ); - itkGetConstMacro( InputSize, unsigned int ); - itkGetConstMacro( MeasureSize, unsigned int ); - - // Matrices - kalmanGetSetMacro( TMatrix, A ); - kalmanGetSetMacro( TMatrix, B ); - kalmanGetSetMacro( TMatrix, H ); - kalmanGetSetMacro( TMatrix, Q ); - kalmanGetSetMacro( TMatrix, R ); - kalmanGetSetMacro( TMatrix, P0 ); - kalmanGetSetMacro( TMatrix, K ); - kalmanGetSetMacro( TMatrix, Pm ); - kalmanGetSetMacro( TMatrix, Pp ); - - // Vectors - kalmanGetSetMacro( TVector, x0 ); - kalmanGetSetMacro( TVector, u ); - kalmanGetSetMacro( TVector, m ); - kalmanGetSetMacro( TVector, xm ); - kalmanGetSetMacro( TVector, xp ); - - // Human-readable matrices - kalmanGetSetMatrixMacro( A, TransitionMatrix ); - kalmanGetSetMatrixMacro( B, InputControlMatrix ); - kalmanGetSetMatrixMacro( H, MeasureMatrix ); - kalmanGetSetMatrixMacro( Q, ProcessNoise ); - kalmanGetSetMatrixMacro( R, MeasureNoise ); - kalmanGetSetMatrixMacro( P0, InitialNoise ); - kalmanGetSetMatrixMacro( K, Gain ); - kalmanGetSetMatrixMacro( Pm, APrioriNoise ); - kalmanGetSetMatrixMacro( Pp, APosterioriNoise ); - - // Human-readable vectors - kalmanGetSetVectorMacro( x0, InitialState ); - kalmanGetSetVectorMacro( u, Input ); - kalmanGetSetVectorMacro( m, Measure ); - kalmanGetSetVectorMacro( xm, APrioriState ); - kalmanGetSetVectorMacro( xp, APosterioriState ); - - public: - - 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( ) - { - this->Predict( ); - this->Innovate( ); - this->Filtrate( ); - } - virtual void NSteps( unsigned int n ) - { - for( unsigned int i = 0; i < n; i++ ) - this->OneStep( ); - } - unsigned int CurrentIteration( ) const - { return( this->m_I ); } - unsigned char CurrentStep( ) const - { return( this->m_Step ); } - - protected: - KalmanFilter( ); - virtual ~KalmanFilter( ); - - private: - // 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 ); + // stage identificators + enum + { + StPred, + StInno, + StFilt }; - } // ecapseman + public: + itkNewMacro( Self ); + itkTypeMacro( KalmanFilter, itkObject ); + + // Values + itkGetConstMacro( StateSize, unsigned int ); + itkGetConstMacro( InputSize, unsigned int ); + itkGetConstMacro( MeasureSize, unsigned int ); + + // Matrices + kalmanGetSetMacro( TMatrix, A ); + kalmanGetSetMacro( TMatrix, B ); + kalmanGetSetMacro( TMatrix, H ); + kalmanGetSetMacro( TMatrix, Q ); + kalmanGetSetMacro( TMatrix, R ); + kalmanGetSetMacro( TMatrix, P0 ); + kalmanGetSetMacro( TMatrix, K ); + kalmanGetSetMacro( TMatrix, Pm ); + kalmanGetSetMacro( TMatrix, Pp ); + + // Vectors + kalmanGetSetMacro( TVector, x0 ); + kalmanGetSetMacro( TVector, u ); + kalmanGetSetMacro( TVector, m ); + kalmanGetSetMacro( TVector, xm ); + kalmanGetSetMacro( TVector, xp ); + + // Human-readable matrices + kalmanGetSetMatrixMacro( A, TransitionMatrix ); + kalmanGetSetMatrixMacro( B, InputControlMatrix ); + kalmanGetSetMatrixMacro( H, MeasureMatrix ); + kalmanGetSetMatrixMacro( Q, ProcessNoise ); + kalmanGetSetMatrixMacro( R, MeasureNoise ); + kalmanGetSetMatrixMacro( P0, InitialNoise ); + kalmanGetSetMatrixMacro( K, Gain ); + kalmanGetSetMatrixMacro( Pm, APrioriNoise ); + kalmanGetSetMatrixMacro( Pp, APosterioriNoise ); + + // Human-readable vectors + kalmanGetSetVectorMacro( x0, InitialState ); + kalmanGetSetVectorMacro( u, Input ); + kalmanGetSetVectorMacro( m, Measure ); + kalmanGetSetVectorMacro( xm, APrioriState ); + kalmanGetSetVectorMacro( xp, APosterioriState ); + + public: + + 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( ) + { + this->Predict( ); + this->Innovate( ); + this->Filtrate( ); + } + virtual void NSteps( unsigned int n ) + { + for( unsigned int i = 0; i < n; i++ ) + this->OneStep( ); + } + unsigned int CurrentIteration( ) const + { return( this->m_I ); } + unsigned char CurrentStep( ) const + { return( this->m_Step ); } + + protected: + KalmanFilter( ); + virtual ~KalmanFilter( ); + + private: + // 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 ); + }; + + } // ecapseman } // ecapseman