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/cpExtensions_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 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$