#include <vnl/vnl_vector.h>
// -------------------------------------------------------------------------
-#define kalmanGetSetMacro( type, name ) \
- itkGetConstMacro( name, type ); \
+#define kalmanGetSetMacro( type, name ) \
+ itkGetConstMacro( name, type ); \
itkSetMacro( name, type );
// -------------------------------------------------------------------------
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
+ // 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( );
- }
+ {
+ this->Predict( );
+ this->Innovate( );
+ this->Filtrate( );
+ }
virtual void NSteps( unsigned int n )
- {
- for( unsigned int i = 0; i < n; i++ )
- this->OneStep( );
- }
+ {
+ for( unsigned int i = 0; i < n; i++ )
+ this->OneStep( );
+ }
unsigned int CurrentIteration( ) const
- { return( this->m_I ); }
+ { return( this->m_I ); }
unsigned char CurrentStep( ) const
- { return( this->m_Step ); }
+ { return( this->m_Step ); }
- protected:
+ protected:
KalmanFilter( );
virtual ~KalmanFilter( );
- private:
+ private:
// Purposely not implemented
KalmanFilter( const Self& );
void operator=( const Self& );
- protected:
+ protected:
// Filter dimensions
unsigned int m_StateSize;
unsigned int m_InputSize;
// 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
+ 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
git://git.creatis.insa-lyon.fr / cpPlugins.git / blobdiff