1 // -------------------------------------------------------------------------
2 // @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co)
3 // -------------------------------------------------------------------------
5 #include <cpExtensions/Algorithms/KalmanVelocityFilter.h>
8 // -------------------------------------------------------------------------
10 void cpExtensions::Algorithms::KalmanVelocityFilter< T >::
11 Configure( unsigned int m )
13 this->Superclass::Configure( m << 1, 1, m );
15 this->m_TimeOffset.set_size( 2, 2 );
16 this->SetTimeOffset( TScalar( 1 ) );
18 this->m_Sigma.set_size( this->m_MeasureSize, this->m_MeasureSize );
19 this->m_Sigma.set_identity( );
22 // -------------------------------------------------------------------------
23 template< typename T >
24 typename cpExtensions::Algorithms::KalmanVelocityFilter< T >::
25 TScalar cpExtensions::Algorithms::KalmanVelocityFilter< T >::
26 GetTimeOffset( ) const
28 return( TScalar( std::sqrt( double( this->m_TimeOffset[ 1 ][ 1 ] ) ) ) );
31 // -------------------------------------------------------------------------
32 template< typename T >
33 void cpExtensions::Algorithms::KalmanVelocityFilter< T >::
34 SetTimeOffset( TScalar t )
39 this->m_TimeOffset[ 0 ][ 0 ] = t4 / TScalar( 4 );
40 this->m_TimeOffset[ 1 ][ 1 ] = t2;
42 this->m_TimeOffset[ 0 ][ 1 ] = t3 / TScalar( 2 );
43 this->m_TimeOffset[ 1 ][ 0 ] = this->m_TimeOffset[ 0 ][ 1 ];
46 // -------------------------------------------------------------------------
47 template< typename T >
48 void cpExtensions::Algorithms::KalmanVelocityFilter< T >::
51 this->Superclass::Initialize( );
53 Self::Kronecker( this->m_Q, this->m_Sigma, this->m_TimeOffset );
55 this->m_A.set_identity( );
56 this->m_B.fill( TScalar( 0 ) );
57 this->m_H.fill( TScalar( 0 ) );
59 TScalar tOff = this->GetTimeOffset( );
60 for( unsigned int i = 0; i < this->m_MeasureSize; i++ )
62 this->m_A[ i ][ this->m_MeasureSize + i ] = tOff;
63 this->m_H[ i ][ i ] = TScalar( 1 );
68 // -------------------------------------------------------------------------
69 template< typename T >
70 cpExtensions::Algorithms::KalmanVelocityFilter< T >::
71 KalmanVelocityFilter( )
77 // -------------------------------------------------------------------------
78 template< typename T >
79 cpExtensions::Algorithms::KalmanVelocityFilter< T >::
80 ~KalmanVelocityFilter( )
84 // -------------------------------------------------------------------------
85 // Explicit instantiations
87 using namespace cpExtensions::Algorithms;
89 template class KalmanVelocityFilter< float >;
90 template class KalmanVelocityFilter< double >;