lib/cpExtensions/Algorithms/KalmanFilter.cxx

   1 #include <cpExtensions/Algorithms/KalmanFilter.h>
   2
   3 #include <cstdlib>
   4 #include <vnl/algo/vnl_matrix_inverse.h>
   5
   6 // -------------------------------------------------------------------------
   7 template< typename T >
   8 void cpExtensions::Algorithms::KalmanFilter< T >::
   9 Configure( unsigned int s, unsigned int i, unsigned int m )
  10 {
  11   this->m_StateSize   = s;
  12   this->m_InputSize   = i;
  13   this->m_MeasureSize = m;
  14
  15   // Matrices
  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 );
  26
  27   // Vectors
  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 );
  33
  34   // Static values
  35   this->m_Id.set_identity( );
  36 }
  37
  38 // -------------------------------------------------------------------------
  39 template< typename T >
  40 void cpExtensions::Algorithms::KalmanFilter< T >::
  41 Initialize( )
  42 {
  43   // Set all to the first iteration
  44   this->m_I = 0;
  45   this->m_Step = Self::StFilt;
  46 }
  47
  48 // -------------------------------------------------------------------------
  49 template< typename T >
  50 void cpExtensions::Algorithms::KalmanFilter< T >::
  51 Predict( )
  52 {
  53   if( this->m_Step == Self::StFilt )
  54   {
  55     if( this->m_I == 0 )
  56     {
  57       this->m_xp = this->m_x0;
  58       this->m_Pp = this->m_P0;
  59
  60     } // fi
  61
  62     // Project the state
  63     this->m_xm = ( this->m_A * this->m_xp ) + ( this->m_B * this->m_u );
  64
  65     // Project the error
  66     this->m_Pm  = this->m_A * this->m_Pp * this->m_A.transpose( );
  67     this->m_Pm += this->m_Q;
  68
  69     this->m_Step = Self::StPred;
  70
  71   } // fi
  72 }
  73
  74 // -------------------------------------------------------------------------
  75 template< typename T >
  76 void cpExtensions::Algorithms::KalmanFilter< T >::
  77 Innovate( )
  78 {
  79   typedef vnl_matrix_inverse< T > _TInv;
  80
  81   if( this->m_Step == Self::StPred )
  82   {
  83     // Kalman gain
  84     this->m_K  = this->m_Pm * this->m_H.transpose( );
  85     this->m_K *=
  86       _TInv(
  87         ( this->m_H * this->m_Pm * this->m_H.transpose( ) ) + this->m_R
  88         );
  89
  90     this->m_Step = Self::StInno;
  91
  92   } // fi
  93 }
  94
  95 // -------------------------------------------------------------------------
  96 template< typename T >
  97 void cpExtensions::Algorithms::KalmanFilter< T >::
  98 Filtrate( )
  99 {
 100   if( this->m_Step == Self::StInno )
 101   {
 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 ) );
 105
 106     // A posteriori error
 107     this->m_Pp = ( this->m_Id - ( this->m_K * this->m_H ) ) * this->m_Pm;
 108
 109     this->m_Step = Self::StFilt;
 110     this->m_I++;
 111
 112   } // fi
 113 }
 114
 115 // -------------------------------------------------------------------------
 116 template< typename T >
 117 cpExtensions::Algorithms::KalmanFilter< T >::
 118 KalmanFilter( )
 119   : Superclass( )
 120 {
 121   this->Configure( 1, 1, 1 );
 122 }
 123
 124 // -------------------------------------------------------------------------
 125 template< typename T >
 126 cpExtensions::Algorithms::KalmanFilter< T >::
 127 ~KalmanFilter( )
 128 {
 129 }
 130
 131 // -------------------------------------------------------------------------
 132 // Explicit instantiations
 133
 134 template class cpExtensions::Algorithms::KalmanFilter< float >;
 135 template class cpExtensions::Algorithms::KalmanFilter< double >;
 136
 137 // eof - $RCSfile$