lib/cpExtensions/Algorithms/KalmanFilter.h

   1 // -------------------------------------------------------------------------
   2 // @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co)
   3 // -------------------------------------------------------------------------
   4
   5 #ifndef __CPEXTENSIONS__ALGORITHMS__KALMANFILTER__H__
   6 #define __CPEXTENSIONS__ALGORITHMS__KALMANFILTER__H__
   7
   8 #include <cpExtensions/Config.h>
   9
  10 #include <itkObject.h>
  11 #include <itkObjectFactory.h>
  12 #include <vnl/vnl_matrix.h>
  13 #include <vnl/vnl_vector.h>
  14
  15 // -------------------------------------------------------------------------
  16 #define kalmanGetSetMacro( type, name )         \
  17   itkGetConstMacro( name, type );               \
  18   itkSetMacro( name, type );
  19
  20 // -------------------------------------------------------------------------
  21 #define kalmanGetMatrixMacro( var, name )       \
  22   virtual const TMatrix& Get##name( ) const     \
  23   { return( this->m_##var ); }
  24
  25 // -------------------------------------------------------------------------
  26 #define kalmanSetMatrixMacro( var, name )       \
  27   virtual void Set##name( const TMatrix& m )    \
  28   { this->Set##var( m ); }
  29
  30 // -------------------------------------------------------------------------
  31 #define kalmanGetSetMatrixMacro( var, name )    \
  32   kalmanGetMatrixMacro( var, name );            \
  33   kalmanSetMatrixMacro( var, name );
  34
  35 // -------------------------------------------------------------------------
  36 #define kalmanGetVectorMacro( var, name )       \
  37   virtual const TVector& Get##name( ) const     \
  38   { return( this->m_##var ); }
  39
  40 // -------------------------------------------------------------------------
  41 #define kalmanSetVectorMacro( var, name )       \
  42   virtual void Set##name( const TVector& v )    \
  43   { this->Set##var( v ); }
  44
  45 // -------------------------------------------------------------------------
  46 #define kalmanGetSetVectorMacro( var, name )    \
  47   kalmanGetVectorMacro( var, name );            \
  48   kalmanSetVectorMacro( var, name );
  49
  50 namespace cpExtensions
  51 {
  52   namespace Algorithms
  53   {
  54     /**
  55      * Abstract class implementing the classical kalman filter. See
  56      * http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf
  57      * for a description of this algorithm.
  58      */
  59     template< typename T >
  60     class cpExtensions_EXPORT KalmanFilter
  61       : public itk::Object
  62     {
  63     public:
  64       typedef KalmanFilter                    Self;
  65       typedef itk::Object                     Superclass;
  66       typedef itk::SmartPointer< Self >       Pointer;
  67       typedef itk::SmartPointer< const Self > ConstPointer;
  68
  69       // Template parameters types
  70       typedef T TScalar;
  71
  72       // VNL types
  73       typedef vnl_matrix< TScalar > TMatrix;
  74       typedef vnl_vector< TScalar > TVector;
  75
  76       //  stage identificators
  77       enum
  78       {
  79         StPred,
  80         StInno,
  81         StFilt
  82       };
  83
  84     public:
  85       itkNewMacro( Self );
  86       itkTypeMacro( KalmanFilter, itkObject );
  87
  88       // Values
  89       itkGetConstMacro( StateSize, unsigned int );
  90       itkGetConstMacro( InputSize, unsigned int );
  91       itkGetConstMacro( MeasureSize, unsigned int );
  92
  93       // Matrices
  94       kalmanGetSetMacro( TMatrix, A );
  95       kalmanGetSetMacro( TMatrix, B );
  96       kalmanGetSetMacro( TMatrix, H );
  97       kalmanGetSetMacro( TMatrix, Q );
  98       kalmanGetSetMacro( TMatrix, R );
  99       kalmanGetSetMacro( TMatrix, P0 );
 100       kalmanGetSetMacro( TMatrix, K );
 101       kalmanGetSetMacro( TMatrix, Pm );
 102       kalmanGetSetMacro( TMatrix, Pp );
 103
 104       // Vectors
 105       kalmanGetSetMacro( TVector, x0 );
 106       kalmanGetSetMacro( TVector, u );
 107       kalmanGetSetMacro( TVector, m );
 108       kalmanGetSetMacro( TVector, xm );
 109       kalmanGetSetMacro( TVector, xp );
 110
 111       // Human-readable matrices
 112       kalmanGetSetMatrixMacro( A, TransitionMatrix );
 113       kalmanGetSetMatrixMacro( B, InputControlMatrix );
 114       kalmanGetSetMatrixMacro( H, MeasureMatrix );
 115       kalmanGetSetMatrixMacro( Q, ProcessNoise );
 116       kalmanGetSetMatrixMacro( R, MeasureNoise );
 117       kalmanGetSetMatrixMacro( P0, InitialNoise );
 118       kalmanGetSetMatrixMacro( K, Gain );
 119       kalmanGetSetMatrixMacro( Pm, APrioriNoise );
 120       kalmanGetSetMatrixMacro( Pp, APosterioriNoise );
 121
 122       // Human-readable vectors
 123       kalmanGetSetVectorMacro( x0, InitialState );
 124       kalmanGetSetVectorMacro( u, Input );
 125       kalmanGetSetVectorMacro( m, Measure );
 126       kalmanGetSetVectorMacro( xm, APrioriState );
 127       kalmanGetSetVectorMacro( xp, APosterioriState );
 128
 129     public:
 130
 131       void Configure( unsigned int s, unsigned int i, unsigned int m );
 132
 133       // Iteration methods
 134       virtual void Initialize( );
 135       virtual void Predict( );
 136       virtual void Innovate( );
 137       virtual void Filtrate( );
 138       virtual void OneStep( )
 139         {
 140           this->Predict( );
 141           this->Innovate( );
 142           this->Filtrate( );
 143         }
 144       virtual void NSteps( unsigned int n )
 145         {
 146           for( unsigned int i = 0; i < n; i++ )
 147             this->OneStep( );
 148         }
 149       unsigned int CurrentIteration( ) const
 150         { return( this->m_I ); }
 151       unsigned char CurrentStep( ) const
 152         { return( this->m_Step ); }
 153
 154     protected:
 155       KalmanFilter( );
 156       virtual ~KalmanFilter( );
 157
 158     private:
 159       // Purposely not implemented
 160       KalmanFilter( const Self& );
 161       void operator=( const Self& );
 162
 163     protected:
 164       // Filter dimensions
 165       unsigned int m_StateSize;
 166       unsigned int m_InputSize;
 167       unsigned int m_MeasureSize;
 168
 169       // Transition matrices
 170       TMatrix m_A;  // Transition
 171       TMatrix m_B;  // Input control
 172       TMatrix m_H;  // Measure
 173       TMatrix m_Id; // Identity matrix
 174
 175       // Noise matrices
 176       TMatrix m_Q; // Process noise covariance
 177       TMatrix m_R; // Measure noise covariance
 178
 179       // Initial values
 180       TVector m_x0; // Initial state
 181       TMatrix m_P0; // Initial error covariance
 182
 183       // Loop vectors
 184       TVector m_u;  // Last real input
 185       TVector m_m;  // Last real measure
 186       TVector m_xm; // A priori state
 187       TVector m_xp; // A posteriori state
 188
 189       // Loop matrices
 190       TMatrix m_K; // kalman gain
 191       TMatrix m_Pm; // A priori error
 192       TMatrix m_Pp; // A posteriori error
 193
 194       // Loop values
 195       unsigned int  m_I;    // Current iteration
 196       unsigned char m_Step; // Current step within current iteration
 197
 198       // -------------------------------------------------------------------
 199       // Classic kronecker product operator
 200       // -------------------------------------------------------------------
 201       template< class M >
 202       static void Kronecker( M& AkB, const M& A, const M& B );
 203     };
 204
 205   } // ecapseman
 206
 207 } // ecapseman
 208
 209 #include <cpExtensions/Algorithms/KalmanFilter.hxx>
 210
 211 #endif // __CPEXTENSIONS__ALGORITHMS__KALMANFILTER__H__
 212
 213 // eof - $RCSfile$