lib/cpPlugins/Extensions/Algorithms/KalmanFilter.cxx

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