From: Leonardo Florez-Valencia Date: Fri, 21 Aug 2015 23:06:18 +0000 (-0500) Subject: Kalman filter added X-Git-Tag: v0.1~392 X-Git-Url: https://git.creatis.insa-lyon.fr/pubgit/?a=commitdiff_plain;h=8075b3c18618cd48cd2ae539d237a13768081489;p=cpPlugins.git Kalman filter added --- diff --git a/lib/cpPlugins/Extensions/Algorithms/KalmanFilter.cxx b/lib/cpPlugins/Extensions/Algorithms/KalmanFilter.cxx new file mode 100644 index 0000000..9542026 --- /dev/null +++ b/lib/cpPlugins/Extensions/Algorithms/KalmanFilter.cxx @@ -0,0 +1,138 @@ +#include + +#include +#include + +// ------------------------------------------------------------------------- +template< typename T > +cpPlugins::Extensions::Algorithms::KalmanFilter< T >:: +KalmanFilter( unsigned int s, unsigned int i, unsigned int m ) +{ + this->Configure( s, i, m ); +} + +// ------------------------------------------------------------------------- +template< typename T > +cpPlugins::Extensions::Algorithms::KalmanFilter< T >:: +~KalmanFilter( ) +{ +} + +// ------------------------------------------------------------------------- +template< typename T > +void cpPlugins::Extensions::Algorithms::KalmanFilter< T >:: +Configure( unsigned int s, unsigned int i, unsigned int m ) +{ + this->m_StateSize = s; + this->m_InputSize = i; + this->m_MeasureSize = m; + + // Matrices + this->m_A.set_size( s, s ); + this->m_B.set_size( s, i ); + this->m_H.set_size( m, s ); + this->m_Id.set_size( s, s ); + this->m_Q.set_size( s, s ); + this->m_R.set_size( m, m ); + this->m_P0.set_size( s, s ); + this->m_K.set_size( s, m ); + this->m_Pm.set_size( s, s ); + this->m_Pp.set_size( s, s ); + + // Vectors + this->m_x0.set_size( s ); + this->m_u.set_size( i ); + this->m_m.set_size( m ); + this->m_xm.set_size( s ); + this->m_xp.set_size( s ); + + // Static values + this->m_Id.set_identity( ); +} + +// ------------------------------------------------------------------------- +template< typename T > +void cpPlugins::Extensions::Algorithms::KalmanFilter< T >:: +Initialize( ) +{ + // Set all to the first iteration + this->m_I = 0; + this->m_Step = Self::StFilt; +} + +// ------------------------------------------------------------------------- +template< typename T > +void cpPlugins::Extensions::Algorithms::KalmanFilter< T >:: +Predict( ) +{ + if( this->m_Step == Self::StFilt ) + { + if( this->m_I == 0 ) + { + this->m_xp = this->m_x0; + this->m_Pp = this->m_P0; + + } // fi + + // Project the state + this->m_xm = ( this->m_A * this->m_xp ) + ( this->m_B * this->m_u ); + + // Project the error + this->m_Pm = this->m_A * this->m_Pp * this->m_A.transpose( ); + this->m_Pm += this->m_Q; + + this->m_Step = Self::StPred; + + } // fi +} + +// ------------------------------------------------------------------------- +template< typename T > +void cpPlugins::Extensions::Algorithms::KalmanFilter< T >:: +Innovate( ) +{ + typedef vnl_matrix_inverse< T > _TInv; + + if( this->m_Step == Self::StPred ) + { + // Kalman gain + this->m_K = this->m_Pm * this->m_H.transpose( ); + this->m_K *= + _TInv( + ( this->m_H * this->m_Pm * this->m_H.transpose( ) ) + this->m_R + ); + + this->m_Step = Self::StInno; + + } // fi +} + +// ------------------------------------------------------------------------- +template< typename T > +void cpPlugins::Extensions::Algorithms::KalmanFilter< T >:: +Filtrate( ) +{ + if( this->m_Step == Self::StInno ) + { + // A posteriori state + this->m_xp = this->m_xm; + this->m_xp += this->m_K * ( this->m_m - ( this->m_H * this->m_xm ) ); + + // A posteriori error + this->m_Pp = ( this->m_Id - ( this->m_K * this->m_H ) ) * this->m_Pm; + + this->m_Step = Self::StFilt; + this->m_I++; + + } // fi +} + +// ------------------------------------------------------------------------- +// Explicit instantiations + +using namespace cpPlugins::Extensions::Algorithms; + +template class KalmanFilter< float >; +template class KalmanFilter< double >; + +// eof - $RCSfile$ diff --git a/lib/cpPlugins/Extensions/Algorithms/KalmanFilter.h b/lib/cpPlugins/Extensions/Algorithms/KalmanFilter.h new file mode 100644 index 0000000..ac5d9e2 --- /dev/null +++ b/lib/cpPlugins/Extensions/Algorithms/KalmanFilter.h @@ -0,0 +1,216 @@ +// ------------------------------------------------------------------------- +// @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co) +// ------------------------------------------------------------------------- + +#ifndef __CPPLUGINS__EXTENSIONS__ALGORITHMS__KALMANFILTER__H__ +#define __CPPLUGINS__EXTENSIONS__ALGORITHMS__KALMANFILTER__H__ + +#include +#include + +// ------------------------------------------------------------------------- +#define kalmanGetMacro( type, name ) \ + virtual type Get##name( ) const \ + { return( this->m_##name ); } + +// ------------------------------------------------------------------------- +#define kalmanSetMacro( type, name ) \ + virtual void Set##name( const type& m ) \ + { this->m_##name = m; } + +// ------------------------------------------------------------------------- +#define kalmanGetSetMacro( type, name ) \ + kalmanGetMacro( type, name ); \ + kalmanSetMacro( type, name ); + +// ------------------------------------------------------------------------- +#define kalmanGetMatrixMacro( var, name ) \ + virtual const TMatrix& Get##name( ) const \ + { return( this->m_##var ); } + +// ------------------------------------------------------------------------- +#define kalmanSetMatrixMacro( var, name ) \ + virtual void Set##name( const TMatrix& m ) \ + { this->m_##var = m; } + +// ------------------------------------------------------------------------- +#define kalmanGetSetMatrixMacro( var, name ) \ + kalmanGetMatrixMacro( var, name ); \ + kalmanSetMatrixMacro( var, name ); + +// ------------------------------------------------------------------------- +#define kalmanGetVectorMacro( var, name ) \ + virtual const TVector& Get##name( ) const \ + { return( this->m_##var ); } + +// ------------------------------------------------------------------------- +#define kalmanSetVectorMacro( var, name ) \ + virtual void Set##name( const TVector& v ) \ + { this->m_##var = v; } + +// ------------------------------------------------------------------------- +#define kalmanGetSetVectorMacro( var, name ) \ + kalmanGetVectorMacro( var, name ); \ + kalmanSetVectorMacro( var, name ); + +namespace cpPlugins +{ + namespace Extensions + { + namespace Algorithms + { + /** + * Abstract class implementing the classical kalman filter. See + * http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf + * for a description of this algorithm. + */ + template< typename T > + class KalmanFilter + { + public: + typedef KalmanFilter Self; + + // Template parameters types + typedef T TScalar; + + // VNL types + typedef vnl_matrix< TScalar > TMatrix; + typedef vnl_vector< TScalar > TVector; + + // stage identificators + enum + { + StPred, + StInno, + StFilt + }; + + public: + // Values + kalmanGetMacro( unsigned int, StateSize ); + kalmanGetMacro( unsigned int, InputSize ); + kalmanGetMacro( unsigned int, MeasureSize ); + + // Matrices + kalmanGetSetMacro( TMatrix, A ); + kalmanGetSetMacro( TMatrix, B ); + kalmanGetSetMacro( TMatrix, H ); + kalmanGetSetMacro( TMatrix, Q ); + kalmanGetSetMacro( TMatrix, R ); + kalmanGetSetMacro( TMatrix, P0 ); + kalmanGetSetMacro( TMatrix, K ); + kalmanGetSetMacro( TMatrix, Pm ); + kalmanGetSetMacro( TMatrix, Pp ); + + // Vectors + kalmanGetSetMacro( TVector, x0 ); + kalmanGetSetMacro( TVector, u ); + kalmanGetSetMacro( TVector, m ); + kalmanGetSetMacro( TVector, xm ); + kalmanGetSetMacro( TVector, xp ); + + // Human-readable matrices + kalmanGetSetMatrixMacro( A, TransitionMatrix ); + kalmanGetSetMatrixMacro( B, InputControlMatrix ); + kalmanGetSetMatrixMacro( H, MeasureMatrix ); + kalmanGetSetMatrixMacro( Q, ProcessNoise ); + kalmanGetSetMatrixMacro( R, MeasureNoise ); + kalmanGetSetMatrixMacro( P0, InitialNoise ); + kalmanGetSetMatrixMacro( K, Gain ); + kalmanGetSetMatrixMacro( Pm, APrioriNoise ); + kalmanGetSetMatrixMacro( Pp, APosterioriNoise ); + + // Human-readable vectors + kalmanGetSetVectorMacro( x0, InitialState ); + kalmanGetSetVectorMacro( u, Input ); + kalmanGetSetVectorMacro( m, Measure ); + kalmanGetSetVectorMacro( xm, APrioriState ); + kalmanGetSetVectorMacro( xp, APosterioriState ); + + public: + KalmanFilter( + unsigned int s = 1, unsigned int i = 1, unsigned int m = 1 + ); + virtual ~KalmanFilter( ); + + void Configure( unsigned int s, unsigned int i, unsigned int m ); + + /// Iteration methods + virtual void Initialize( ); + virtual void Predict( ); + virtual void Innovate( ); + virtual void Filtrate( ); + virtual void OneStep( ) + { + this->Predict( ); + this->Innovate( ); + this->Filtrate( ); + } + virtual void NSteps( unsigned int n ) + { + for( unsigned int i = 0; i < n; i++ ) + this->OneStep( ); + } + unsigned int CurrentIteration( ) const + { return( this->m_I ); } + unsigned char CurrentStep( ) const + { return( this->m_Step ); } + + private: + // Purposely not implemented + KalmanFilter( const Self& ); + void operator=( const Self& ); + + protected: + // Filter dimensions + unsigned int m_StateSize; + unsigned int m_InputSize; + unsigned int m_MeasureSize; + + // Transition matrices + TMatrix m_A; /// Transition + TMatrix m_B; /// Input control + TMatrix m_H; /// Measure + TMatrix m_Id; /// Identity matrix + + // Noise matrices + TMatrix m_Q; /// Process noise covariance + TMatrix m_R; /// Measure noise covariance + + // Initial values + TVector m_x0; /// Initial state + TMatrix m_P0; /// Initial error covariance + + // Loop vectors + TVector m_u; /// Last real input + TVector m_m; /// Last real measure + TVector m_xm; /// A priori state + TVector m_xp; /// A posteriori state + + // Loop matrices + TMatrix m_K; /// kalman gain + TMatrix m_Pm; /// A priori error + TMatrix m_Pp; /// A posteriori error + + // Loop values + unsigned int m_I; /// Current iteration + unsigned char m_Step; /// Current step within current iteration + + // ----------------------------------------------------------------- + // Classic kronecker product operator + // ----------------------------------------------------------------- + template< class M > + static void Kronecker( M& AkB, const M& A, const M& B ); + }; + + } // ecapseman + + } // ecapseman + +} // ecapseman + +#include + +#endif // __CPPLUGINS__EXTENSIONS__ALGORITHMS__KALMANFILTER__H__ + +// eof - $RCSfile$ diff --git a/lib/cpPlugins/Extensions/Algorithms/KalmanFilter.hxx b/lib/cpPlugins/Extensions/Algorithms/KalmanFilter.hxx new file mode 100644 index 0000000..0e4f106 --- /dev/null +++ b/lib/cpPlugins/Extensions/Algorithms/KalmanFilter.hxx @@ -0,0 +1,34 @@ +#ifndef __CPPLUGINS__EXTENSIONS__ALGORITHMS__KALMANFILTER__HXX__ +#define __CPPLUGINS__EXTENSIONS__ALGORITHMS__KALMANFILTER__HXX__ + +// ------------------------------------------------------------------------- +template< typename T > +template< class M > +void cpPlugins::Extensions::Algorithms::KalmanFilter< T >:: +Kronecker( M& AkB, const M& A, const M& B ) +{ + unsigned int Ar = A.rows( ); unsigned int Ac = A.cols( ); + unsigned int Br = B.rows( ); unsigned int Bc = B.cols( ); + AkB.set_size( Ar * Br, Ac * Bc ); + + for( unsigned int ar = 0; ar < Ar; ar++ ) + { + for( unsigned int ac = 0; ac < Ac; ac++ ) + { + typename M::element_type vA = A[ ar ][ ac ]; + for( unsigned int br = 0; br < Br; br++ ) + { + unsigned int rOff = ar + ( br * ( Br - 1 ) ); + for( unsigned int bc = 0; bc < Bc; bc++ ) + AkB[ rOff ][ ac + ( bc * ( Bc - 1 ) ) ] = vA * B[ br ][ bc ]; + + } // rof + + } // rof + + } // rof +} + +#endif // __CPPLUGINS__EXTENSIONS__ALGORITHMS__KALMANFILTER__HXX__ + +// eof - $RCSfile$