Kalman filter added

[cpPlugins.git] / lib / cpPlugins / Extensions / Algorithms / KalmanFilter.cxx

diff --git a/lib/cpPlugins/Extensions/Algorithms/KalmanFilter.cxx b/lib/cpPlugins/Extensions/Algorithms/KalmanFilter.cxx
new file mode 100644 (file)
index 0000000..9542026
--- /dev/null
+++ b/lib/cpPlugins/Extensions/Algorithms/KalmanFilter.cxx
@@ -0,0 +1,138 @@
+#include <cpPlugins/Extensions/Algorithms/KalmanFilter.h>
+
+#include <cstdlib>
+#include <vnl/algo/vnl_matrix_inverse.h>
+
+// -------------------------------------------------------------------------
+template< typename T >
+cpPlugins::Extensions::Algorithms::KalmanFilter< T >::
+KalmanFilter( unsigned int s, unsigned int i, unsigned int m )
+{
+  this->Configure( s, i, m );
+}
+
+// -------------------------------------------------------------------------
+template< typename T >
+cpPlugins::Extensions::Algorithms::KalmanFilter< T >::
+~KalmanFilter( )
+{
+}
+
+// -------------------------------------------------------------------------
+template< typename T >
+void cpPlugins::Extensions::Algorithms::KalmanFilter< T >::
+Configure( unsigned int s, unsigned int i, unsigned int m )
+{
+  this->m_StateSize   = s;
+  this->m_InputSize   = i;
+  this->m_MeasureSize = m;
+
+  // Matrices
+  this->m_A.set_size( s, s );
+  this->m_B.set_size( s, i );
+  this->m_H.set_size( m, s );
+  this->m_Id.set_size( s, s );
+  this->m_Q.set_size( s, s );
+  this->m_R.set_size( m, m );
+  this->m_P0.set_size( s, s );
+  this->m_K.set_size( s, m );
+  this->m_Pm.set_size( s, s );
+  this->m_Pp.set_size( s, s );
+
+  // Vectors
+  this->m_x0.set_size( s );
+  this->m_u.set_size( i );
+  this->m_m.set_size( m );
+  this->m_xm.set_size( s );
+  this->m_xp.set_size( s );
+
+  // Static values
+  this->m_Id.set_identity( );
+}
+
+// -------------------------------------------------------------------------
+template< typename T >
+void cpPlugins::Extensions::Algorithms::KalmanFilter< T >::
+Initialize( )
+{
+  // Set all to the first iteration
+  this->m_I = 0;
+  this->m_Step = Self::StFilt;
+}
+
+// -------------------------------------------------------------------------
+template< typename T >
+void cpPlugins::Extensions::Algorithms::KalmanFilter< T >::
+Predict( )
+{
+  if( this->m_Step == Self::StFilt )
+  {
+    if( this->m_I == 0 )
+    {
+      this->m_xp = this->m_x0;
+      this->m_Pp = this->m_P0;
+
+    } // fi
+
+    // Project the state
+    this->m_xm = ( this->m_A * this->m_xp ) + ( this->m_B * this->m_u );
+
+    // Project the error
+    this->m_Pm  = this->m_A * this->m_Pp * this->m_A.transpose( );
+    this->m_Pm += this->m_Q;
+
+    this->m_Step = Self::StPred;
+
+  } // fi
+}
+
+// -------------------------------------------------------------------------
+template< typename T >
+void cpPlugins::Extensions::Algorithms::KalmanFilter< T >::
+Innovate( )
+{
+  typedef vnl_matrix_inverse< T > _TInv;
+
+  if( this->m_Step == Self::StPred )
+  {
+    // Kalman gain
+    this->m_K  = this->m_Pm * this->m_H.transpose( );
+    this->m_K *=
+      _TInv(
+        ( this->m_H * this->m_Pm * this->m_H.transpose( ) ) + this->m_R
+        );
+
+    this->m_Step = Self::StInno;
+
+  } // fi
+}
+
+// -------------------------------------------------------------------------
+template< typename T >
+void cpPlugins::Extensions::Algorithms::KalmanFilter< T >::
+Filtrate( )
+{
+  if( this->m_Step == Self::StInno )
+  {
+    // A posteriori state
+    this->m_xp  = this->m_xm;
+    this->m_xp += this->m_K * ( this->m_m - ( this->m_H * this->m_xm ) );
+
+    // A posteriori error
+    this->m_Pp = ( this->m_Id - ( this->m_K * this->m_H ) ) * this->m_Pm;
+
+    this->m_Step = Self::StFilt;
+    this->m_I++;
+
+  } // fi
+}
+
+// -------------------------------------------------------------------------
+// Explicit instantiations
+
+using namespace cpPlugins::Extensions::Algorithms;
+
+template class KalmanFilter< float >;
+template class KalmanFilter< double >;
+
+// eof - $RCSfile$