+++ /dev/null
-// -------------------------------------------------------------------------
-// @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co)
-// -------------------------------------------------------------------------
-
-#include <cpExtensions/Algorithms/KalmanVelocityFilter.h>
-#include <cmath>
-
-// -------------------------------------------------------------------------
-template< typename T >
-void cpExtensions::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 cpExtensions::Algorithms::KalmanVelocityFilter< T >::
-TScalar cpExtensions::Algorithms::KalmanVelocityFilter< T >::
-GetTimeOffset( ) const
-{
- return( TScalar( std::sqrt( double( this->m_TimeOffset[ 1 ][ 1 ] ) ) ) );
-}
-
-// -------------------------------------------------------------------------
-template< typename T >
-void cpExtensions::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 cpExtensions::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 >
-cpExtensions::Algorithms::KalmanVelocityFilter< T >::
-KalmanVelocityFilter( )
- : Superclass( )
-{
- this->Configure( 1 );
-}
-
-// -------------------------------------------------------------------------
-template< typename T >
-cpExtensions::Algorithms::KalmanVelocityFilter< T >::
-~KalmanVelocityFilter( )
-{
-}
-
-// -------------------------------------------------------------------------
-// Explicit instantiations
-
-template class cpExtensions::Algorithms::KalmanVelocityFilter< float >;
-template class cpExtensions::Algorithms::KalmanVelocityFilter< double >;
-
-// eof - $RCSfile$