--- /dev/null
+#include <cpPlugins/Extensions/Algorithms/KalmanFilter.h>
+
+#include <cstdlib>
+#include <vnl/algo/vnl_matrix_inverse.h>
+
+// -------------------------------------------------------------------------
+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$
--- /dev/null
+// -------------------------------------------------------------------------
+// @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co)
+// -------------------------------------------------------------------------
+
+#ifndef __CPPLUGINS__EXTENSIONS__ALGORITHMS__KALMANFILTER__H__
+#define __CPPLUGINS__EXTENSIONS__ALGORITHMS__KALMANFILTER__H__
+
+#include <vnl/vnl_matrix.h>
+#include <vnl/vnl_vector.h>
+
+// -------------------------------------------------------------------------
+#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 <cpPlugins/Extensions/Algorithms/KalmanFilter.hxx>
+
+#endif // __CPPLUGINS__EXTENSIONS__ALGORITHMS__KALMANFILTER__H__
+
+// eof - $RCSfile$
git://git.creatis.insa-lyon.fr / cpPlugins.git / commitdiff