1 #include <cpExtensions/Algorithms/KalmanFilter.h>
4 #include <vnl/algo/vnl_matrix_inverse.h>
6 // -------------------------------------------------------------------------
8 void cpExtensions::Algorithms::KalmanFilter< T >::
9 Configure( unsigned int s, unsigned int i, unsigned int m )
11 this->m_StateSize = s;
12 this->m_InputSize = i;
13 this->m_MeasureSize = m;
16 this->m_A.set_size( s, s );
17 this->m_B.set_size( s, i );
18 this->m_H.set_size( m, s );
19 this->m_Id.set_size( s, s );
20 this->m_Q.set_size( s, s );
21 this->m_R.set_size( m, m );
22 this->m_P0.set_size( s, s );
23 this->m_K.set_size( s, m );
24 this->m_Pm.set_size( s, s );
25 this->m_Pp.set_size( s, s );
28 this->m_x0.set_size( s );
29 this->m_u.set_size( i );
30 this->m_m.set_size( m );
31 this->m_xm.set_size( s );
32 this->m_xp.set_size( s );
35 this->m_Id.set_identity( );
38 // -------------------------------------------------------------------------
39 template< typename T >
40 void cpExtensions::Algorithms::KalmanFilter< T >::
43 // Set all to the first iteration
45 this->m_Step = Self::StFilt;
48 // -------------------------------------------------------------------------
49 template< typename T >
50 void cpExtensions::Algorithms::KalmanFilter< T >::
53 if( this->m_Step == Self::StFilt )
57 this->m_xp = this->m_x0;
58 this->m_Pp = this->m_P0;
63 this->m_xm = ( this->m_A * this->m_xp ) + ( this->m_B * this->m_u );
66 this->m_Pm = this->m_A * this->m_Pp * this->m_A.transpose( );
67 this->m_Pm += this->m_Q;
69 this->m_Step = Self::StPred;
74 // -------------------------------------------------------------------------
75 template< typename T >
76 void cpExtensions::Algorithms::KalmanFilter< T >::
79 typedef vnl_matrix_inverse< T > _TInv;
81 if( this->m_Step == Self::StPred )
84 this->m_K = this->m_Pm * this->m_H.transpose( );
87 ( this->m_H * this->m_Pm * this->m_H.transpose( ) ) + this->m_R
90 this->m_Step = Self::StInno;
95 // -------------------------------------------------------------------------
96 template< typename T >
97 void cpExtensions::Algorithms::KalmanFilter< T >::
100 if( this->m_Step == Self::StInno )
102 // A posteriori state
103 this->m_xp = this->m_xm;
104 this->m_xp += this->m_K * ( this->m_m - ( this->m_H * this->m_xm ) );
106 // A posteriori error
107 this->m_Pp = ( this->m_Id - ( this->m_K * this->m_H ) ) * this->m_Pm;
109 this->m_Step = Self::StFilt;
115 // -------------------------------------------------------------------------
116 template< typename T >
117 cpExtensions::Algorithms::KalmanFilter< T >::
121 this->Configure( 1, 1, 1 );
124 // -------------------------------------------------------------------------
125 template< typename T >
126 cpExtensions::Algorithms::KalmanFilter< T >::
131 // -------------------------------------------------------------------------
132 // Explicit instantiations
134 using namespace cpExtensions::Algorithms;
136 template class KalmanFilter< float >;
137 template class KalmanFilter< double >;