#include <iostream>

#include <cpExtensions/Algorithms/KalmanVelocityFilter.h>

// -------------------------------------------------------------------------
const unsigned int Dim = 1;
typedef double TScalar;

// -------------------------------------------------------------------------
int main( int argc, char* argv[] )
{
  // Kalman filters
  typedef
    cpExtensions::Algorithms::KalmanVelocityFilter< TScalar >
    _TKalman;
  _TKalman::Pointer kalman01 = _TKalman::New( );
  kalman01->Configure( 1 );
  kalman01->SetTimeOffset( TScalar( 1 ) );

  _TKalman::TMatrix P0 = kalman01->GetP0( );
  P0.set_identity( );
  P0 *= TScalar( 10 );
  kalman01->SetP0( P0 );

  _TKalman::TMatrix sigma = kalman01->GetSigma( );
  sigma.set_identity( );
  sigma *= TScalar( 1e-5 );
  kalman01->SetSigma( sigma );

  _TKalman::TMatrix R = kalman01->GetR( );
  R.set_identity( );
  R *= TScalar( 0.09 );
  kalman01->SetR( R );

  TScalar read;
  std::cin >> read;

  _TKalman::TVector x0 = kalman01->Getx0( );
  x0[ 0 ] = read;
  kalman01->Setx0( x0 );
  kalman01->Initialize( );

  while( !std::cin.eof( ) )
  {
    std::cin >> read;

    _TKalman::TVector m = kalman01->Getm( );
    m[ 0 ] = read;
    kalman01->Setm( m );
    kalman01->OneStep( );

    std::cout << m[ 0 ] << " " << kalman01->Getxp( )[ 0 ] << std::endl;

  } // eliwh

  return( 0 );
}

// eof - $RCSfile$