yet another refactoring

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

diff --git a/lib/cpExtensions/Algorithms/KalmanFilter.h b/lib/cpExtensions/Algorithms/KalmanFilter.h
new file mode 100644 (file)
index 0000000..0039620
--- /dev/null
+++ b/lib/cpExtensions/Algorithms/KalmanFilter.h
@@ -0,0 +1,213 @@
+// -------------------------------------------------------------------------
+// @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co)
+// -------------------------------------------------------------------------
+
+#ifndef __CPEXTENSIONS__ALGORITHMS__KALMANFILTER__H__
+#define __CPEXTENSIONS__ALGORITHMS__KALMANFILTER__H__
+
+#include <cpExtensions/Config.h>
+
+#include <itkObject.h>
+#include <itkObjectFactory.h>
+#include <vnl/vnl_matrix.h>
+#include <vnl/vnl_vector.h>
+
+// -------------------------------------------------------------------------
+#define kalmanGetSetMacro( type, name )         \
+  itkGetConstMacro( name, type );               \
+  itkSetMacro( name, type );
+
+// -------------------------------------------------------------------------
+#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->Set##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->Set##var( v ); }
+
+// -------------------------------------------------------------------------
+#define kalmanGetSetVectorMacro( var, name )    \
+  kalmanGetVectorMacro( var, name );            \
+  kalmanSetVectorMacro( var, name );
+
+namespace cpExtensions
+{
+  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 cpExtensions_EXPORT KalmanFilter
+      : public itk::Object
+    {
+    public:
+      typedef KalmanFilter                    Self;
+      typedef itk::Object                     Superclass;
+      typedef itk::SmartPointer< Self >       Pointer;
+      typedef itk::SmartPointer< const Self > ConstPointer;
+
+      // 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:
+      itkNewMacro( Self );
+      itkTypeMacro( KalmanFilter, itkObject );
+
+      // Values
+      itkGetConstMacro( StateSize, unsigned int );
+      itkGetConstMacro( InputSize, unsigned int );
+      itkGetConstMacro( MeasureSize, unsigned int );
+
+      // 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:
+
+      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 ); }
+
+    protected:
+      KalmanFilter( );
+      virtual ~KalmanFilter( );
+
+    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
+
+#include <cpExtensions/Algorithms/KalmanFilter.hxx>
+
+#endif // __CPEXTENSIONS__ALGORITHMS__KALMANFILTER__H__
+
+// eof - $RCSfile$