Kalman filter added

[cpPlugins.git] / lib / cpPlugins / Extensions / Algorithms / KalmanFilter.h

diff --git a/lib/cpPlugins/Extensions/Algorithms/KalmanFilter.h b/lib/cpPlugins/Extensions/Algorithms/KalmanFilter.h
new file mode 100644 (file)
index 0000000..ac5d9e2
--- /dev/null
+++ b/lib/cpPlugins/Extensions/Algorithms/KalmanFilter.h
@@ -0,0 +1,216 @@
+// -------------------------------------------------------------------------
+// @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co)
+// -------------------------------------------------------------------------
+
+#ifndef __CPPLUGINS__EXTENSIONS__ALGORITHMS__KALMANFILTER__H__
+#define __CPPLUGINS__EXTENSIONS__ALGORITHMS__KALMANFILTER__H__
+
+#include <vnl/vnl_matrix.h>
+#include <vnl/vnl_vector.h>
+
+// -------------------------------------------------------------------------
+#define kalmanGetMacro( type, name )            \
+  virtual type Get##name( ) const               \
+  { return( this->m_##name ); }
+
+// -------------------------------------------------------------------------
+#define kalmanSetMacro( type, name )            \
+  virtual void Set##name( const type& m )       \
+  { this->m_##name = m; }
+
+// -------------------------------------------------------------------------
+#define kalmanGetSetMacro( type, name )         \
+  kalmanGetMacro( type, name );                 \
+  kalmanSetMacro( type, name );
+
+// -------------------------------------------------------------------------
+#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->m_##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->m_##var = v; }
+
+// -------------------------------------------------------------------------
+#define kalmanGetSetVectorMacro( var, name )    \
+  kalmanGetVectorMacro( var, name );            \
+  kalmanSetVectorMacro( var, name );
+
+namespace cpPlugins
+{
+  namespace Extensions
+  {
+    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 KalmanFilter
+      {
+      public:
+        typedef KalmanFilter Self;
+
+        // 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:
+        // Values
+        kalmanGetMacro( unsigned int, StateSize );
+        kalmanGetMacro( unsigned int, InputSize );
+        kalmanGetMacro( unsigned int, MeasureSize );
+
+        // 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:
+        KalmanFilter(
+          unsigned int s = 1, unsigned int i = 1, unsigned int m = 1
+          );
+        virtual ~KalmanFilter( );
+
+        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 ); }
+
+      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
+
+} // ecapseman
+
+#include <cpPlugins/Extensions/Algorithms/KalmanFilter.hxx>
+
+#endif // __CPPLUGINS__EXTENSIONS__ALGORITHMS__KALMANFILTER__H__
+
+// eof - $RCSfile$