1 // -------------------------------------------------------------------------
2 // @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co)
3 // -------------------------------------------------------------------------
5 #ifndef __CPPLUGINS__EXTENSIONS__ALGORITHMS__KALMANFILTER__H__
6 #define __CPPLUGINS__EXTENSIONS__ALGORITHMS__KALMANFILTER__H__
8 #include <cpPlugins/Extensions/cpPlugins_Extensions_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 );
57 * Abstract class implementing the classical kalman filter. See
58 * http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf
59 * for a description of this algorithm.
61 template< typename T >
62 class cpPlugins_Extensions_EXPORT KalmanFilter
66 typedef KalmanFilter Self;
67 typedef itk::Object Superclass;
68 typedef itk::SmartPointer< Self > Pointer;
69 typedef itk::SmartPointer< const Self > ConstPointer;
71 // Template parameters types
75 typedef vnl_matrix< TScalar > TMatrix;
76 typedef vnl_vector< TScalar > TVector;
78 // stage identificators
88 itkTypeMacro( KalmanFilter, itkObject );
91 itkGetConstMacro( StateSize, unsigned int );
92 itkGetConstMacro( InputSize, unsigned int );
93 itkGetConstMacro( MeasureSize, unsigned int );
96 kalmanGetSetMacro( TMatrix, A );
97 kalmanGetSetMacro( TMatrix, B );
98 kalmanGetSetMacro( TMatrix, H );
99 kalmanGetSetMacro( TMatrix, Q );
100 kalmanGetSetMacro( TMatrix, R );
101 kalmanGetSetMacro( TMatrix, P0 );
102 kalmanGetSetMacro( TMatrix, K );
103 kalmanGetSetMacro( TMatrix, Pm );
104 kalmanGetSetMacro( TMatrix, Pp );
107 kalmanGetSetMacro( TVector, x0 );
108 kalmanGetSetMacro( TVector, u );
109 kalmanGetSetMacro( TVector, m );
110 kalmanGetSetMacro( TVector, xm );
111 kalmanGetSetMacro( TVector, xp );
113 // Human-readable matrices
114 kalmanGetSetMatrixMacro( A, TransitionMatrix );
115 kalmanGetSetMatrixMacro( B, InputControlMatrix );
116 kalmanGetSetMatrixMacro( H, MeasureMatrix );
117 kalmanGetSetMatrixMacro( Q, ProcessNoise );
118 kalmanGetSetMatrixMacro( R, MeasureNoise );
119 kalmanGetSetMatrixMacro( P0, InitialNoise );
120 kalmanGetSetMatrixMacro( K, Gain );
121 kalmanGetSetMatrixMacro( Pm, APrioriNoise );
122 kalmanGetSetMatrixMacro( Pp, APosterioriNoise );
124 // Human-readable vectors
125 kalmanGetSetVectorMacro( x0, InitialState );
126 kalmanGetSetVectorMacro( u, Input );
127 kalmanGetSetVectorMacro( m, Measure );
128 kalmanGetSetVectorMacro( xm, APrioriState );
129 kalmanGetSetVectorMacro( xp, APosterioriState );
133 void Configure( unsigned int s, unsigned int i, unsigned int m );
135 /// Iteration methods
136 virtual void Initialize( );
137 virtual void Predict( );
138 virtual void Innovate( );
139 virtual void Filtrate( );
140 virtual void OneStep( )
146 virtual void NSteps( unsigned int n )
148 for( unsigned int i = 0; i < n; i++ )
151 unsigned int CurrentIteration( ) const
152 { return( this->m_I ); }
153 unsigned char CurrentStep( ) const
154 { return( this->m_Step ); }
158 virtual ~KalmanFilter( );
161 // Purposely not implemented
162 KalmanFilter( const Self& );
163 void operator=( const Self& );
167 unsigned int m_StateSize;
168 unsigned int m_InputSize;
169 unsigned int m_MeasureSize;
171 // Transition matrices
172 TMatrix m_A; /// Transition
173 TMatrix m_B; /// Input control
174 TMatrix m_H; /// Measure
175 TMatrix m_Id; /// Identity matrix
178 TMatrix m_Q; /// Process noise covariance
179 TMatrix m_R; /// Measure noise covariance
182 TVector m_x0; /// Initial state
183 TMatrix m_P0; /// Initial error covariance
186 TVector m_u; /// Last real input
187 TVector m_m; /// Last real measure
188 TVector m_xm; /// A priori state
189 TVector m_xp; /// A posteriori state
192 TMatrix m_K; /// kalman gain
193 TMatrix m_Pm; /// A priori error
194 TMatrix m_Pp; /// A posteriori error
197 unsigned int m_I; /// Current iteration
198 unsigned char m_Step; /// Current step within current iteration
200 // -----------------------------------------------------------------
201 // Classic kronecker product operator
202 // -----------------------------------------------------------------
204 static void Kronecker( M& AkB, const M& A, const M& B );
213 #include <cpPlugins/Extensions/Algorithms/KalmanFilter.hxx>
215 #endif // __CPPLUGINS__EXTENSIONS__ALGORITHMS__KALMANFILTER__H__