1 // -------------------------------------------------------------------------
2 // @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co)
3 // -------------------------------------------------------------------------
5 #ifndef __CPEXTENSIONS__ALGORITHMS__KALMANFILTER__H__
6 #define __CPEXTENSIONS__ALGORITHMS__KALMANFILTER__H__
8 #include <cpExtensions/cpExtensions_Export.h>
10 #include <itkObject.h>
11 #include <itkObjectFactory.h>
12 #include <vnl/vnl_matrix.h>
13 #include <vnl/vnl_vector.h>
15 // -------------------------------------------------------------------------
16 #define kalmanGetSetMacro( type, name ) \
17 itkGetConstMacro( name, type ); \
18 itkSetMacro( name, type );
20 // -------------------------------------------------------------------------
21 #define kalmanGetMatrixMacro( var, name ) \
22 virtual const TMatrix& Get##name( ) const \
23 { return( this->m_##var ); }
25 // -------------------------------------------------------------------------
26 #define kalmanSetMatrixMacro( var, name ) \
27 virtual void Set##name( const TMatrix& m ) \
28 { this->Set##var( m ); }
30 // -------------------------------------------------------------------------
31 #define kalmanGetSetMatrixMacro( var, name ) \
32 kalmanGetMatrixMacro( var, name ); \
33 kalmanSetMatrixMacro( var, name );
35 // -------------------------------------------------------------------------
36 #define kalmanGetVectorMacro( var, name ) \
37 virtual const TVector& Get##name( ) const \
38 { return( this->m_##var ); }
40 // -------------------------------------------------------------------------
41 #define kalmanSetVectorMacro( var, name ) \
42 virtual void Set##name( const TVector& v ) \
43 { this->Set##var( v ); }
45 // -------------------------------------------------------------------------
46 #define kalmanGetSetVectorMacro( var, name ) \
47 kalmanGetVectorMacro( var, name ); \
48 kalmanSetVectorMacro( var, name );
50 namespace cpExtensions
55 * Abstract class implementing the classical kalman filter. See
56 * http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf
57 * for a description of this algorithm.
59 template< typename T >
60 class cpExtensions_EXPORT KalmanFilter
64 typedef KalmanFilter Self;
65 typedef itk::Object Superclass;
66 typedef itk::SmartPointer< Self > Pointer;
67 typedef itk::SmartPointer< const Self > ConstPointer;
69 // Template parameters types
73 typedef vnl_matrix< TScalar > TMatrix;
74 typedef vnl_vector< TScalar > TVector;
76 // stage identificators
86 itkTypeMacro( KalmanFilter, itkObject );
89 itkGetConstMacro( StateSize, unsigned int );
90 itkGetConstMacro( InputSize, unsigned int );
91 itkGetConstMacro( MeasureSize, unsigned int );
94 kalmanGetSetMacro( TMatrix, A );
95 kalmanGetSetMacro( TMatrix, B );
96 kalmanGetSetMacro( TMatrix, H );
97 kalmanGetSetMacro( TMatrix, Q );
98 kalmanGetSetMacro( TMatrix, R );
99 kalmanGetSetMacro( TMatrix, P0 );
100 kalmanGetSetMacro( TMatrix, K );
101 kalmanGetSetMacro( TMatrix, Pm );
102 kalmanGetSetMacro( TMatrix, Pp );
105 kalmanGetSetMacro( TVector, x0 );
106 kalmanGetSetMacro( TVector, u );
107 kalmanGetSetMacro( TVector, m );
108 kalmanGetSetMacro( TVector, xm );
109 kalmanGetSetMacro( TVector, xp );
111 // Human-readable matrices
112 kalmanGetSetMatrixMacro( A, TransitionMatrix );
113 kalmanGetSetMatrixMacro( B, InputControlMatrix );
114 kalmanGetSetMatrixMacro( H, MeasureMatrix );
115 kalmanGetSetMatrixMacro( Q, ProcessNoise );
116 kalmanGetSetMatrixMacro( R, MeasureNoise );
117 kalmanGetSetMatrixMacro( P0, InitialNoise );
118 kalmanGetSetMatrixMacro( K, Gain );
119 kalmanGetSetMatrixMacro( Pm, APrioriNoise );
120 kalmanGetSetMatrixMacro( Pp, APosterioriNoise );
122 // Human-readable vectors
123 kalmanGetSetVectorMacro( x0, InitialState );
124 kalmanGetSetVectorMacro( u, Input );
125 kalmanGetSetVectorMacro( m, Measure );
126 kalmanGetSetVectorMacro( xm, APrioriState );
127 kalmanGetSetVectorMacro( xp, APosterioriState );
131 void Configure( unsigned int s, unsigned int i, unsigned int m );
134 virtual void Initialize( );
135 virtual void Predict( );
136 virtual void Innovate( );
137 virtual void Filtrate( );
138 virtual void OneStep( )
144 virtual void NSteps( unsigned int n )
146 for( unsigned int i = 0; i < n; i++ )
149 unsigned int CurrentIteration( ) const
150 { return( this->m_I ); }
151 unsigned char CurrentStep( ) const
152 { return( this->m_Step ); }
156 virtual ~KalmanFilter( );
159 // Purposely not implemented
160 KalmanFilter( const Self& );
161 void operator=( const Self& );
165 unsigned int m_StateSize;
166 unsigned int m_InputSize;
167 unsigned int m_MeasureSize;
169 // Transition matrices
170 TMatrix m_A; // Transition
171 TMatrix m_B; // Input control
172 TMatrix m_H; // Measure
173 TMatrix m_Id; // Identity matrix
176 TMatrix m_Q; // Process noise covariance
177 TMatrix m_R; // Measure noise covariance
180 TVector m_x0; // Initial state
181 TMatrix m_P0; // Initial error covariance
184 TVector m_u; // Last real input
185 TVector m_m; // Last real measure
186 TVector m_xm; // A priori state
187 TVector m_xp; // A posteriori state
190 TMatrix m_K; // kalman gain
191 TMatrix m_Pm; // A priori error
192 TMatrix m_Pp; // A posteriori error
195 unsigned int m_I; // Current iteration
196 unsigned char m_Step; // Current step within current iteration
198 // -------------------------------------------------------------------
199 // Classic kronecker product operator
200 // -------------------------------------------------------------------
202 static void Kronecker( M& AkB, const M& A, const M& B );
209 #ifndef ITK_MANUAL_INSTANTIATION
210 #include <cpExtensions/Algorithms/KalmanFilter.hxx>
211 #endif // ITK_MANUAL_INSTANTIATION
213 #endif // __CPEXTENSIONS__ALGORITHMS__KALMANFILTER__H__