kalmanfilteroned.h

来自「VC视频对象的跟踪提取原代码（vc视频监控源码）」· C头文件 代码 · 共 391 行

//////////////////////////////////////////////////////////////////////
//                                                                  //
//  File:    KalmanFilterOneD.h                                     //
//                                                                  //
//  Classes: FilterOneD and derived 1D Kalman filter classes        //
//                                                                  //
//  Author : 	A M Baumberg                                        //
//                                                                  //
//  Creation Date: Mon Dec  6 10:35:28 1993                         //
//                                                                  //
//////////////////////////////////////////////////////////////////////


#ifndef __KALMAN_FILTER_ONE_D_H__
#define __KALMAN_FILTER_ONE_D_H__

#include "Matrix2by2.h"
#include "text_output.h"

namespace ReadingPeopleTracker
{


//////////////////////////////////////////////////////////////////////
//                                                                  //
//  FilterOneD - base class for 1D Kalman Filters                   //
//                                                                  //
//////////////////////////////////////////////////////////////////////

class FilterOneD
{
public:
    enum FilterType { BASE_FILT, KALM_FILT1, KALM_FILT2, VIBE_FILT };

    virtual realno add_observation(realno, realno )
	{
	    return 0.0;
	}
    virtual realno delta_update(realno, realno ) 
	{
	    return 0.0;
	}
    virtual realno get_update_gain(realno ) 
	{
	    return 0.0;
	}
    
    virtual realno add_observation(realno )  = 0;

    virtual void remove_last_observation() = 0;

    virtual realno update_state() = 0;
    
    virtual realno &get_state() = 0;
    
    virtual realno get_p11 () const = 0;

    virtual FilterType get_type() 
	{
	    return BASE_FILT;
	}
    
    // clone --   returns a dynamically allocated copy of
    //		the derived class
    
    virtual FilterOneD *clone() const = 0;
    
#ifdef _SVID_SOURCE
// the following uses SVID 48-bit random numbers...

    // corrupts the filter state and increases the
    // covariance -- for simluated annealing
    virtual void add_noise(realno variance)
	{
	    // nothing
	}
#endif
    
};


//////////////////////////////////////////////////////////////////////
//                                                                  //
//  StaticKalmanOneD - 1D discrete Kalman Filter                    //
//                                                                  //
//////////////////////////////////////////////////////////////////////

class StaticKalmanOneD : public FilterOneD
{
    
public:
    
    realno p11;   realno old_p11;   // covariance matrix
    realno b;     realno old_b;     // current estimates
    realno q;                       // variance of system noise
    realno def_r;                   // default sensor variance
    realno epsilon;                 // weight current measurement
    
    StaticKalmanOneD()
	{
	    p11 = b = q = def_r = epsilon = 0;
	}
    
    StaticKalmanOneD (realno Q, realno r_0, realno b_0 = 0,
		      realno p11_0 = 1e12, realno eps = 0);
    
    inline realno &get_state()
	{
	    return b; 
	}
    inline realno get_p11() const
	{
	    return p11; 
	}
    
    realno add_observation(realno b_obs, realno r);
    inline realno add_observation(realno b_obs)
	{
	    return add_observation(b_obs, def_r);
	}
    realno delta_update(realno delta_b, realno inv_r);
    inline realno get_update_gain(realno inv_r)
	{
	    return (p11 * inv_r) / (1 + p11 * inv_r); 
	}
    inline realno state_change()
	{
	    return b - old_b;
	}
    realno update_state(); 
    void remove_last_observation();
    inline FilterType get_type()
	{ 
	    return KALM_FILT2; 
	}
    inline FilterOneD* clone() const
	{ 
	    StaticKalmanOneD* res = new StaticKalmanOneD();
	    *res = *this; 
	    return res; 
	}

#ifdef _SVID_SOURCE
// the following uses SVID 48-bit random numbers...

    void add_noise(realno variance);

#endif
    
    inline void set_variance(realno v)
	{ 
	    p11 = old_p11 = v;
	}
    inline void set_state(realno x)
	{
	    b = old_b = x;
	}
    
    
};

//////////////////////////////////////////////////////////////////////
//                                                                  //
//  DynamicKalmanOneD - 1D constant velocity discrete Kalman Filter //
//                                                                  //
//  default initial conditions                                      //
//    p11 = infinity (b unknown)                                    //
//    p12 = 0                                                       //
//    p22 = 0, db/dt = 0 (stationary)                               //
//                                                                  //
//////////////////////////////////////////////////////////////////////

class DynamicKalmanOneD : public FilterOneD
{
    
public:
    
    realno p11, p12, p22; // covariance matrix
    realno old_p11, old_p12, old_p22, old_b, old_db; // temporary p11, b
    realno epsilon;	// set to zero for standard Kalman
    realno b, db; 	// current estimates
    realno q11, q12, q22;	// system noise covariance
    realno def_r; 	// default sensor variance
    realno dt;  		// time interval between measurements
    realno t; 		// time elapsed
    
    inline FilterOneD *clone() const
	{
	    DynamicKalmanOneD* res = new DynamicKalmanOneD();
	    *res = *this;
	    return res; 
	}
    
    
    inline DynamicKalmanOneD() 
	{ p11 = p12 = p22 = epsilon = 
	      b = db = q11 = q12 = q22 = 
	      def_r = dt = t = 0;
	}
    
    DynamicKalmanOneD(realno Q11, realno Q12, realno Q22,
		      realno r_0, realno b_0 = 0, realno db_0
		      = 0, realno p11_0 = 1e12, realno p12_0 = 0, realno p22_0
		      = 0, realno dt = (1.0/15.0), realno eps = 0.0);
    
    //
    // add an observation  ...
    // b_obs  	observed value
    // r     	sensor variance
    //
    
    realno add_observation(realno b_obs, realno r);
    
    // delta_update ...
    // add an observation 
    // with (b_obs - b_est) * inv_r = delta_b
    // and measurement inverse covariance = inv_r
    // 
    // return the Kalman gain
    
    realno delta_update(realno delta_b, realno inv_r);
    inline realno get_update_gain(realno inv_r)
	{
	    return (p11 * inv_r) / (1 + p11 * inv_r);
	}
    
    
    inline realno add_observation(realno b_obs)
	{
	    return add_observation(b_obs, def_r);
	}
    
    //
    // get_prediction and update estimates ...
    // delta_t 	time elapsed since last update
    //
    
    realno get_prediction(realno delta_t);
    inline realno get_prediction()
	{
	    return get_prediction(dt);
	}
    
    inline realno update_state()
	{
	    return get_prediction(dt); 
	}
    
    inline realno state_change_b()
	{
	    return b - old_b;
	}
    
    inline realno state_change_db()
	{
	    return db - old_db;
	}
    
    void remove_last_observation();
    
    inline realno& get_velocity() 
	{
	    return db;
	}
    inline realno &get_state()
	{ 
	    return b;
	}
    inline realno get_p11() const
	{
	    return p11;
	}

#ifdef _SVID_SOURCE
// the following uses SVID 48-bit random numbers...
    void add_noise(realno variance);
#endif
    
    inline FilterType get_type() 
	{
	    return KALM_FILT1;
	}
    
};


//////////////////////////////////////////////////////////////////////
//                                                                  //
//  VibrationFilter - 1D dynamic Kalman filter (state x, dx/dt)     //
//                                                                  //
//  with 2nd order model                                            //
//    d  [x ] = [0 1][x - bias ] + noise                            //
//    dt [dx]   [a b][dx]                                           //
//                                                                  //
//  noise has covariance Q                                          //
//                                                                  //
//////////////////////////////////////////////////////////////////////

class VibrationFilter : public FilterOneD
{
public:
    
    realno p11, p12, p22; // covariance matrix
    realno x, dx;         // current estimates
    realno x_p;	 	  // predicted value of x
    realno bias;          // set to x(t-1)
    realno a,b;		  // 2nd order model 
    realno q11, q12, q22; // system noise covariance
    realno def_r; 	  // default sensor variance
    realno dt;            // time interval between measurements
    realno t; 		  // time elapsed
    
    VibrationFilter()
	{
	    p11 = p12 = p22 = a = b = 
		x = dx = bias = q11 = q12 = q22 = 
		def_r = dt = t = 0; 
	}
    
    VibrationFilter(realno Q11, realno Q12, realno Q22,
		    realno a, realno b,
		    realno r_0, realno x_0 = 0, realno dx_0 = 0,
		    realno p11_0 = 1e12, realno p12_0 = 0, 
		    realno p22_0 = 0, realno dt = 1.0) ;
    
    //
    // add an observation  ...
    // x_obs  	observed value
    // r     	sensor variance
    //
    
    realno add_observation(realno x_obs, realno r);
    realno delta_update(realno delta_x, realno inv_r);
    
    inline realno add_observation(realno x_obs)
	{
	    return add_observation(x_obs, def_r);
	}
    
    virtual void remove_last_observation()
	{ 
	    cerror << "VibrationFilter::remove_last_observation: not implemented." << endl;
	    exit (1);
	}
    //
    // get_prediction and update estimates ...
    // delta_t 	time elapsed since last update
    //
    
    realno get_prediction(realno delta_t);
    inline realno get_prediction()
	{
	    return get_prediction(dt);
	}
    inline realno update_state() 
	{ 
	    return get_prediction(dt); 
	}
    
    inline realno& get_velocity() 
	{ 
	    return dx;
	}
    inline realno &get_state()
	{
	    return x;
	}
    
    inline realno get_p11() const
	{
	    return p11; 
	}
    
    inline FilterType get_type()
	{
	    return VIBE_FILT;
	}
    
    inline FilterOneD *clone()  const
	{
	    VibrationFilter* res = new VibrationFilter();
	    *res = *this;
	    return res;
	}
};


} // namespace ReadingPeopleTracker

#endif  // ifndef __KALMAN_FILTER_ONE_D_H__