plKalmanFilter.h

/*=============================================================================
 * Product        : OpenPL 
 * File           : plKalmanFilter.h
 * Author         : Christophe Coue
 * Creation       : Tue Mar 27 2001
 *
 *=============================================================================
 *     (c) Copyright 2000, Centre National de la Recherche Scientifique,
 *                              all rights reserved
 *=============================================================================
 *
 *------------------------- Description ---------------------------------------
 *   
 *   
 *-----------------------------------------------------------------------------
*/

#include <plObject.h>
#include <plMath.h>
#include <plFloatVector.h>
#include <plFloatMatrix.h>
#include <plKernel.h>
#include <plValues.h>



/*****************************************************************************
 *                           Classes Definitions
 *****************************************************************************/


#ifndef __LINEAR_KALMAN_FILTER_H
#define __LINEAR_KALMAN_FILTER_H

// pour algo d'inversion de Greville
#ifndef INVERSE_SEUIL
#define INVERSE_SEUIL 1.0e-38
#endif

class plKalmanFilter : public plObject
{
protected:
  // Identity Matrice
  plFloatMatrix I;
  
  unsigned int state_size;
  unsigned int obs_size;
  unsigned int command_size;


  // redimensionnement des vecteurs et matrices ci dessus
  void resize( unsigned int s, unsigned int o, unsigned int c);

  // mise a jour du filtre (interne)
  void internal_prediction();
  void internal_update();

 public:
  plFloatMatrix A, At, B, H, Ht, Q, R, K, P_update, P_predict;
  plFloatVector x_update, x_predict, z, u;

  /* constructeurs */
  plKalmanFilter() {};
  plKalmanFilter(unsigned int n_in, unsigned int m_im,  unsigned int c_in=1);

  /* destructeur */
  virtual ~plKalmanFilter() {};

  void update_matrix(const plFloatMatrix& Ain)
    { 
      A = Ain;
      At = Ain.transpose();
    }
  void control_matrix(const plFloatMatrix& Bin) { B = Bin; }
  void sys_noise(const plFloatMatrix& QS) { Q = QS; }
  void obs_noise(const plFloatMatrix& Ro) { R = Ro; }
  void obs_sys(const plFloatMatrix& Hin)  
    { 
      H = Hin; 
      Ht = Hin.transpose();
    }  
  void initial_prediction(const plFloatVector& Xo,
              const plFloatMatrix& Po)
    {
      x_predict = Xo;
      P_predict = Po;
    }

  void initial_update(const plFloatVector& Xo, 
              const plFloatMatrix& Po)
    {
      x_update = Xo;
      P_update = Po;
    }

  // incorporate new command */
  void commande(const plFloatVector& Uin)
    {
      u = Uin;
    }

  //incorporate new observation
  void observation(const plFloatVector& Zin)
    { 
      z = Zin;
    }

  void prediction();
  
  void update();

  // METHODES D'ACCES
  plFloatVector& mean() { return x_update;}
  plFloatMatrix& covariance() { return P_update;}

};

#endif

---