// -------------------------------------------------------------------------
// @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$