3 #include <cpExtensions/Algorithms/KalmanVelocityFilter.h>
5 // -------------------------------------------------------------------------
6 const unsigned int Dim = 1;
7 typedef double TScalar;
9 // -------------------------------------------------------------------------
10 int main( int argc, char* argv[] )
14 cpExtensions::Algorithms::KalmanVelocityFilter< TScalar >
16 _TKalman::Pointer kalman01 = _TKalman::New( );
17 kalman01->Configure( 1 );
18 kalman01->SetTimeOffset( TScalar( 1 ) );
20 _TKalman::TMatrix P0 = kalman01->GetP0( );
23 kalman01->SetP0( P0 );
25 _TKalman::TMatrix sigma = kalman01->GetSigma( );
26 sigma.set_identity( );
27 sigma *= TScalar( 1e-5 );
28 kalman01->SetSigma( sigma );
30 _TKalman::TMatrix R = kalman01->GetR( );
38 _TKalman::TVector x0 = kalman01->Getx0( );
40 kalman01->Setx0( x0 );
41 kalman01->Initialize( );
43 while( !std::cin.eof( ) )
47 _TKalman::TVector m = kalman01->Getm( );
52 std::cout << m[ 0 ] << " " << kalman01->Getxp( )[ 0 ] << std::endl;