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 <itkObject.h>
   9 #include <itkObjectFactory.h>
  10 #include <vnl/vnl_matrix.h>
  11 #include <vnl/vnl_vector.h>
  12
  13 // -------------------------------------------------------------------------
  14 #define kalmanGetSetMacro( type, name )    \
  15   itkGetConstMacro( name, type );          \
  16   itkSetMacro( name, type );
  17
  18 // -------------------------------------------------------------------------
  19 #define kalmanGetMatrixMacro( var, name )       \
  20   virtual const TMatrix& Get##name( ) const     \
  21   { return( this->m_##var ); }
  22
  23 // -------------------------------------------------------------------------
  24 #define kalmanSetMatrixMacro( var, name )       \
  25   virtual void Set##name( const TMatrix& m )    \
  26   { this->Set##var( m ); }
  27
  28 // -------------------------------------------------------------------------
  29 #define kalmanGetSetMatrixMacro( var, name )    \
  30   kalmanGetMatrixMacro( var, name );            \
  31   kalmanSetMatrixMacro( var, name );
  32
  33 // -------------------------------------------------------------------------
  34 #define kalmanGetVectorMacro( var, name )       \
  35   virtual const TVector& Get##name( ) const     \
  36   { return( this->m_##var ); }
  37
  38 // -------------------------------------------------------------------------
  39 #define kalmanSetVectorMacro( var, name )       \
  40   virtual void Set##name( const TVector& v )    \
  41   { this->Set##var( v ); }
  42
  43 // -------------------------------------------------------------------------
  44 #define kalmanGetSetVectorMacro( var, name )    \
  45   kalmanGetVectorMacro( var, name );            \
  46   kalmanSetVectorMacro( var, name );
  47
  48 namespace cpPlugins
  49 {
  50   namespace Extensions
  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 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 } // ecapseman
 210
 211 #include <cpPlugins/Extensions/Algorithms/KalmanFilter.hxx>
 212
 213 #endif // __CPPLUGINS__EXTENSIONS__ALGORITHMS__KALMANFILTER__H__
 214
 215 // eof - $RCSfile$