Kalman completely ported

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

diff --git a/lib/cpPlugins/Extensions/Algorithms/KalmanVelocityFilter.cxx b/lib/cpPlugins/Extensions/Algorithms/KalmanVelocityFilter.cxx
new file mode 100644 (file)
index 0000000..6ec3295
--- /dev/null
+++ b/lib/cpPlugins/Extensions/Algorithms/KalmanVelocityFilter.cxx
@@ -0,0 +1,92 @@
+// -------------------------------------------------------------------------
+// @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co)
+// -------------------------------------------------------------------------
+
+#include <cpPlugins/Extensions/Algorithms/KalmanVelocityFilter.h>
+#include <cmath>
+
+// -------------------------------------------------------------------------
+template< typename T >
+void cpPlugins::Extensions::Algorithms::KalmanVelocityFilter< T >::
+Configure( unsigned int m )
+{
+  this->Superclass::Configure( m << 1, 1, m );
+
+  this->m_TimeOffset.set_size( 2, 2 );
+  this->SetTimeOffset( TScalar( 1 ) );
+
+  this->m_Sigma.set_size( this->m_MeasureSize, this->m_MeasureSize );
+  this->m_Sigma.set_identity( );
+}
+
+// -------------------------------------------------------------------------
+template< typename T >
+typename cpPlugins::Extensions::Algorithms::KalmanVelocityFilter< T >::
+TScalar cpPlugins::Extensions::Algorithms::KalmanVelocityFilter< T >::
+GetTimeOffset( ) const
+{
+  return( TScalar( std::sqrt( double( this->m_TimeOffset[ 1 ][ 1 ] ) ) ) );
+}
+
+// -------------------------------------------------------------------------
+template< typename T >
+void cpPlugins::Extensions::Algorithms::KalmanVelocityFilter< T >::
+SetTimeOffset( TScalar t )
+{
+  TScalar t2 = t * t;
+  TScalar t3 = t2 * t;
+  TScalar t4 = t3 * t;
+  this->m_TimeOffset[ 0 ][ 0 ] = t4 / TScalar( 4 );
+  this->m_TimeOffset[ 1 ][ 1 ] = t2;
+
+  this->m_TimeOffset[ 0 ][ 1 ] = t3 / TScalar( 2 );
+  this->m_TimeOffset[ 1 ][ 0 ] = this->m_TimeOffset[ 0 ][ 1 ];
+}
+
+// -------------------------------------------------------------------------
+template< typename T >
+void cpPlugins::Extensions::Algorithms::KalmanVelocityFilter< T >::
+Initialize( )
+{
+  this->Superclass::Initialize( );
+
+  Self::Kronecker( this->m_Q, this->m_Sigma, this->m_TimeOffset );
+
+  this->m_A.set_identity( );
+  this->m_B.fill( TScalar( 0 ) );
+  this->m_H.fill( TScalar( 0 ) );
+
+  TScalar tOff = this->GetTimeOffset( );
+  for( unsigned int i = 0; i < this->m_MeasureSize; i++ )
+  {
+    this->m_A[ i ][ this->m_MeasureSize + i ] = tOff;
+    this->m_H[ i ][ i ] = TScalar( 1 );
+
+  } // rof
+}
+
+// -------------------------------------------------------------------------
+template< typename T >
+cpPlugins::Extensions::Algorithms::KalmanVelocityFilter< T >::
+KalmanVelocityFilter( )
+  : Superclass( )
+{
+  this->Configure( 1 );
+}
+
+// -------------------------------------------------------------------------
+template< typename T >
+cpPlugins::Extensions::Algorithms::KalmanVelocityFilter< T >::
+~KalmanVelocityFilter( )
+{
+}
+
+// -------------------------------------------------------------------------
+// Explicit instantiations
+
+using namespace cpPlugins::Extensions::Algorithms;
+
+template class KalmanVelocityFilter< float >;
+template class KalmanVelocityFilter< double >;
+
+// eof - $RCSfile$