- // 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 );