appli/examples/extensions/example_KalmanVelocity.cxx

   1 #include <iostream>
   2
   3 #include <cpExtensions/Algorithms/KalmanVelocityFilter.h>
   4
   5 // -------------------------------------------------------------------------
   6 const unsigned int Dim = 1;
   7 typedef double TScalar;
   8
   9 // -------------------------------------------------------------------------
  10 int main( int argc, char* argv[] )
  11 {
  12   // Kalman filters
  13   typedef
  14     cpExtensions::Algorithms::KalmanVelocityFilter< TScalar >
  15     _TKalman;
  16   _TKalman::Pointer kalman01 = _TKalman::New( );
  17   kalman01->Configure( 1 );
  18   kalman01->SetTimeOffset( TScalar( 1 ) );
  19
  20   _TKalman::TMatrix P0 = kalman01->GetP0( );
  21   P0.set_identity( );
  22   P0 *= TScalar( 10 );
  23   kalman01->SetP0( P0 );
  24
  25   _TKalman::TMatrix sigma = kalman01->GetSigma( );
  26   sigma.set_identity( );
  27   sigma *= TScalar( 1e-5 );
  28   kalman01->SetSigma( sigma );
  29
  30   _TKalman::TMatrix R = kalman01->GetR( );
  31   R.set_identity( );
  32   R *= TScalar( 0.09 );
  33   kalman01->SetR( R );
  34
  35   TScalar read;
  36   std::cin >> read;
  37
  38   _TKalman::TVector x0 = kalman01->Getx0( );
  39   x0[ 0 ] = read;
  40   kalman01->Setx0( x0 );
  41   kalman01->Initialize( );
  42
  43   while( !std::cin.eof( ) )
  44   {
  45     std::cin >> read;
  46
  47     _TKalman::TVector m = kalman01->Getm( );
  48     m[ 0 ] = read;
  49     kalman01->Setm( m );
  50     kalman01->OneStep( );
  51
  52     std::cout << m[ 0 ] << " " << kalman01->Getxp( )[ 0 ] << std::endl;
  53
  54   } // eliwh
  55
  56   return( 0 );
  57 }
  58
  59 // eof - $RCSfile$