kalman.hpp

面向对象的卡尔曼滤波器源码

// kalman.hpp		Kalman filter class
//		this is a full blown standard Kalman filter with no shortcuts

// rcsid: @(#)kalman.hpp	1.3 16:44:46 10/6/92   EFC

#ifndef _KALMAN_HPP_
#define _KALMAN_HPP_ 1.3

#ifndef INCLUDE_VECTOR	      // include matrix/vector operations in matrix class
#define INCLUDE_VECTOR
#endif

#include <vector.hpp>
#include <matrix.hpp>

#include <dynamics.hpp>

class Kalman
{
	private:
	  int n, m;
	  Matrix gain, covariance, h, r, q, I;
	  Matrix ht, pht, denom;
	  Vector x, z, xs, innovation;
	  Dynamics& phi;
	public:
	  Matrix& k;		// the present gain matrix setting
	  Matrix& p;		// the present covariance matrix
	  Kalman(const int n_in,const int m_im,Dynamics& ph);
	 ~Kalman() {}
	  void sys_noise(Matrix& qs) { q = qs; }
	  void obs_noise(Matrix& ro) { r = ro; }
	  void obs_sys(Matrix& hin)  { h = hin; ht = transpose( h ); }
	  void initial(Vector& ic)   { x = ic; }
	  void step_x(const int steps = 1);
	  void step_p(const int steps = 1);
	  void set_gain();
	  void update_x();
	  void update_p();
	  friend Vector& operator<<(Vector&,const Kalman&);	// get filtered value
	  friend Kalman& operator<<(Kalman&,const Vector&); // incorporate observations
	  Kalman& operator++();
};

#endif