Moved to version 1.0

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

diff --git a/lib/cpExtensions/Algorithms/KalmanFilter.cxx b/lib/cpExtensions/Algorithms/KalmanFilter.cxx
deleted file mode 100644 (file)
index add2041..0000000
--- a/lib/cpExtensions/Algorithms/KalmanFilter.cxx
+++ /dev/null
@@ -1,137 +0,0 @@
-#include <cpExtensions/Algorithms/KalmanFilter.h>
-
-#include <cstdlib>
-#include <vnl/algo/vnl_matrix_inverse.h>
-
-// -------------------------------------------------------------------------
-template< typename T >
-void cpExtensions::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 cpExtensions::Algorithms::KalmanFilter< T >::
-Initialize( )
-{
-  // Set all to the first iteration
-  this->m_I = 0;
-  this->m_Step = Self::StFilt;
-}
-
-// -------------------------------------------------------------------------
-template< typename T >
-void cpExtensions::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 cpExtensions::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 cpExtensions::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
-}
-
-// -------------------------------------------------------------------------
-template< typename T >
-cpExtensions::Algorithms::KalmanFilter< T >::
-KalmanFilter( )
-  : Superclass( )
-{
-  this->Configure( 1, 1, 1 );
-}
-
-// -------------------------------------------------------------------------
-template< typename T >
-cpExtensions::Algorithms::KalmanFilter< T >::
-~KalmanFilter( )
-{
-}
-
-// -------------------------------------------------------------------------
-// Explicit instantiations
-
-template class cpExtensions::Algorithms::KalmanFilter< float >;
-template class cpExtensions::Algorithms::KalmanFilter< double >;
-
-// eof - $RCSfile$