lib/cpExtensions/Algorithms/KalmanVelocityFilter.cxx

   1 // -------------------------------------------------------------------------
   2 // @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co)
   3 // -------------------------------------------------------------------------
   4
   5 #include <cpExtensions/Algorithms/KalmanVelocityFilter.h>
   6 #include <cmath>
   7
   8 // -------------------------------------------------------------------------
   9 template< typename T >
  10 void cpExtensions::Algorithms::KalmanVelocityFilter< T >::
  11 Configure( unsigned int m )
  12 {
  13   this->Superclass::Configure( m << 1, 1, m );
  14
  15   this->m_TimeOffset.set_size( 2, 2 );
  16   this->SetTimeOffset( TScalar( 1 ) );
  17
  18   this->m_Sigma.set_size( this->m_MeasureSize, this->m_MeasureSize );
  19   this->m_Sigma.set_identity( );
  20 }
  21
  22 // -------------------------------------------------------------------------
  23 template< typename T >
  24 typename cpExtensions::Algorithms::KalmanVelocityFilter< T >::
  25 TScalar cpExtensions::Algorithms::KalmanVelocityFilter< T >::
  26 GetTimeOffset( ) const
  27 {
  28   return( TScalar( std::sqrt( double( this->m_TimeOffset[ 1 ][ 1 ] ) ) ) );
  29 }
  30
  31 // -------------------------------------------------------------------------
  32 template< typename T >
  33 void cpExtensions::Algorithms::KalmanVelocityFilter< T >::
  34 SetTimeOffset( TScalar t )
  35 {
  36   TScalar t2 = t * t;
  37   TScalar t3 = t2 * t;
  38   TScalar t4 = t3 * t;
  39   this->m_TimeOffset[ 0 ][ 0 ] = t4 / TScalar( 4 );
  40   this->m_TimeOffset[ 1 ][ 1 ] = t2;
  41
  42   this->m_TimeOffset[ 0 ][ 1 ] = t3 / TScalar( 2 );
  43   this->m_TimeOffset[ 1 ][ 0 ] = this->m_TimeOffset[ 0 ][ 1 ];
  44 }
  45
  46 // -------------------------------------------------------------------------
  47 template< typename T >
  48 void cpExtensions::Algorithms::KalmanVelocityFilter< T >::
  49 Initialize( )
  50 {
  51   this->Superclass::Initialize( );
  52
  53   Self::Kronecker( this->m_Q, this->m_Sigma, this->m_TimeOffset );
  54
  55   this->m_A.set_identity( );
  56   this->m_B.fill( TScalar( 0 ) );
  57   this->m_H.fill( TScalar( 0 ) );
  58
  59   TScalar tOff = this->GetTimeOffset( );
  60   for( unsigned int i = 0; i < this->m_MeasureSize; i++ )
  61   {
  62     this->m_A[ i ][ this->m_MeasureSize + i ] = tOff;
  63     this->m_H[ i ][ i ] = TScalar( 1 );
  64
  65   } // rof
  66 }
  67
  68 // -------------------------------------------------------------------------
  69 template< typename T >
  70 cpExtensions::Algorithms::KalmanVelocityFilter< T >::
  71 KalmanVelocityFilter( )
  72   : Superclass( )
  73 {
  74   this->Configure( 1 );
  75 }
  76
  77 // -------------------------------------------------------------------------
  78 template< typename T >
  79 cpExtensions::Algorithms::KalmanVelocityFilter< T >::
  80 ~KalmanVelocityFilter( )
  81 {
  82 }
  83
  84 // -------------------------------------------------------------------------
  85 // Explicit instantiations
  86
  87 template class cpExtensions::Algorithms::KalmanVelocityFilter< float >;
  88 template class cpExtensions::Algorithms::KalmanVelocityFilter< double >;
  89
  90 // eof - $RCSfile$