+++ /dev/null
-// -------------------------------------------------------------------------
-// @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co)
-// -------------------------------------------------------------------------
-
-#ifndef __CPEXTENSIONS__ALGORITHMS__KALMANFILTER__H__
-#define __CPEXTENSIONS__ALGORITHMS__KALMANFILTER__H__
-
-#include <cpExtensions/Config.h>
-
-#include <itkObject.h>
-#include <itkObjectFactory.h>
-#include <vnl/vnl_matrix.h>
-#include <vnl/vnl_vector.h>
-
-// -------------------------------------------------------------------------
-#define kalmanGetSetMacro( type, name ) \
- itkGetConstMacro( name, type ); \
- itkSetMacro( name, type );
-
-// -------------------------------------------------------------------------
-#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->Set##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->Set##var( v ); }
-
-// -------------------------------------------------------------------------
-#define kalmanGetSetVectorMacro( var, name ) \
- kalmanGetVectorMacro( var, name ); \
- kalmanSetVectorMacro( var, name );
-
-namespace cpExtensions
-{
- 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 cpExtensions_EXPORT KalmanFilter
- : public itk::Object
- {
- public:
- typedef KalmanFilter Self;
- typedef itk::Object Superclass;
- typedef itk::SmartPointer< Self > Pointer;
- typedef itk::SmartPointer< const Self > ConstPointer;
-
- // 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:
- itkNewMacro( Self );
- itkTypeMacro( KalmanFilter, itkObject );
-
- // Values
- itkGetConstMacro( StateSize, unsigned int );
- itkGetConstMacro( InputSize, unsigned int );
- itkGetConstMacro( MeasureSize, unsigned int );
-
- // 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:
-
- 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 ); }
-
- protected:
- KalmanFilter( );
- virtual ~KalmanFilter( );
-
- 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
-
-#include <cpExtensions/Algorithms/KalmanFilter.hxx>
-
-#endif // __CPEXTENSIONS__ALGORITHMS__KALMANFILTER__H__
-
-// eof - $RCSfile$
git://git.creatis.insa-lyon.fr / cpPlugins.git / blobdiff