lib/cpPlugins/Extensions/Algorithms/KalmanFilter.h

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