kalmanfiltertwod.h

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

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


#ifndef __KALMAN_FILTER_TWO_D_H__
#define __KALMAN_FILTER_TWO_D_H__

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

namespace ReadingPeopleTracker
{

//////////////////////////////////////////////////////////////////////
//                                                                  //
//  FilterTwoD - base class for 2D Kalman Filters                   //
//                                                                  //
//////////////////////////////////////////////////////////////////////

class FilterTwoD
{
public:
    
    virtual FilterTwoD* clone() const = 0;
    
    // return 2D static state
    virtual const Point2 &get_state() const = 0;
    
    // return 2x2 covaraince of static part of state
    virtual Matrix2by2& get_cov()
	{ 
	    cerror << "FilterTwoD::get_cov: not implemented." << endl;
	    exit (1);
	}
    
    // return the variance in the most uncertain direction
    // (i.e. the principal eigenvalue of the covariance matrix)
    virtual realno get_uncertainty()
	{ 
	    cerror << "FilterTwoD::get_uncertainty: not implemented." << endl;
	    exit (1);
	}
    
    // delta_update(dp, M): 
    // add an observation with ...
    // measurement covariance R = M^{-1}
    // and p_observed - p_estimate = R dp
    //
    // returns Kalman gain
    virtual realno delta_update(Point2 /*dp*/, Matrix2by2 /*M*/)
	{
	    cerror << "FilterTwoD::delta_update: not implemented." << endl;
	    exit (1);
	}
    
    // add an observation 
    // (pass observed,  measurement covariance)
    // returns new state
    virtual Point2 add_observation(Point2 /*pos_obs*/, Matrix2by2 /*R*/)
	{
	    cerror << "FilterTwoD::add_observation: not implemented." << endl;
	    exit (1);
	}
    
    // the change in state after last observation
    virtual Point2 state_change()
	{
	    cerror << "FilterTwoD::state_change: not implemented." << endl;
	    exit (1);
	}
};


//////////////////////////////////////////////////////////////////////
//                                                                  //
//  StaticKalmanTwoD - 2D discrete Kalman filter                    //
//                                                                  //
//////////////////////////////////////////////////////////////////////

class StaticKalmanTwoD : public FilterTwoD
{
    
public:
    
    Matrix2by2 Cov;	// 2 x 2 covariance
    Matrix2by2 old_Cov;
    
    Point2 b;				// current estimates
    Point2 old_b;
    
    Matrix2by2 Q;			       	// system noise matrix
    
    StaticKalmanTwoD() : Cov(), old_Cov(), b(), old_b(), Q()
	{
	    // nothing
	}
    
    StaticKalmanTwoD(realno Qxx, realno Qxy, realno Qyy , 
		     realno bx_0 = 0, realno by_0 = 0,
		     realno pxx_0 = 1e12, realno pxy_0 = 0, 
		     realno pyy_0 = 1e12);
    
    inline FilterTwoD *clone() const
	{ 
	    StaticKalmanTwoD* res = new StaticKalmanTwoD();
	    *res = *this;
	    return res;
	}
    
    inline realno& get_xstate()
	{
	    return b.x; 
	}
    inline realno& get_ystate()
	{
	    return b.y;
	}
    inline const Point2 &get_state() const
	{
	    return b; 
	}
    
    realno get_uncertainty();
    
    inline realno state_change_x()
	{
	    return b.x - old_b.x; 
	}
    
    inline realno state_change_y()
	{
	    return b.y - old_b.y;
	}
    
    inline Point2 state_change()
	{
	    return b - old_b; 
	}
    
    // update state (as before) and return maximum 
    // eigenvalue of gain matrix
    realno delta_update(realno dbx_obs, realno dby_obs,  
			realno rxx, realno rxy, realno ryy);
    
    inline realno delta_update(Point2 db , Matrix2by2 R_inv)
	{
	    return delta_update(db.x, db.y, R_inv.val00, R_inv.val01, R_inv.val11);
	}
    
    Point2 add_observation(Point2 b_obs, Matrix2by2 R);
    
    // set_polar_noise_cov
    // sets a polar noise model covariance by linearising
    // about current estimates
    // -- input variance of relative scale noise term
    // -- and variance of angular noise term
    void set_polar_noise_cov(realno scale_var, realno theta_var);
    
    void update_state(); 
    void remove_last_observation();
    
#ifdef _SVID_SOURCE
// the following uses SVID 48-bit random numbers...

    void add_noise(realno var_x, realno var_y);
    
#endif
    
    inline void set_covariance(Matrix2by2 new_cov) 
	{
	    Cov = old_Cov = new_cov; 
	}
    inline Matrix2by2& get_cov()
	{
	    return Cov;
	}
    inline void set_state(Point2 x)
	{
	    b = old_b = x; 
	}
    
};


//////////////////////////////////////////////////////////////////////
//                                                                  //
//  DynamicKalmanTwoD - 2D constant velocity discrete Kalman Filter //
//                                                                  //
//////////////////////////////////////////////////////////////////////

class DynamicKalmanTwoD : public FilterTwoD
{
public:  /// FIXME: should make these protected and have inline fn's to access

    // covariance matrix in 2 by 2 blocks
    Matrix2by2 C00;
    Matrix2by2 C01;
    Matrix2by2 C11;
    
    // old covariance matrix in 2 by 2 blocks
    Matrix2by2 old_C00;
    Matrix2by2 old_C01;
    Matrix2by2 old_C11;
    
    
    Point2 pos;		// current estimate of position
    Point2 velocity;	// current estimate of velocity
    Point2 old_pos;
    Point2 old_velocity;
    
    // system noise in 2 by 2 blocks 
    Matrix2by2 Q00;
    Matrix2by2 Q01;
    Matrix2by2 Q11;
    
    realno dt;  		// time interval between measurements
    realno t; 		// time elapsed
   
    // null constructor
    DynamicKalmanTwoD() : 
	pos(0,0), velocity(0,0), old_pos(0,0), old_velocity(0,0)
	{
	    dt = t = 0; 
	}
    
    // constructor :
    // diagonal of system noise passed as (q_x, q_y, q_dx, q_dy)
    // diagonal of initial covariance passed as 
    //	(var_x, var_y, var_dx, var_dy)
    // assume all other covariance terms are zero
    // initial position pos_0
    // initial velocity vel_0
    DynamicKalmanTwoD(realno q_x, realno q_y, realno q_dx, realno q_dy,
		      Point2 pos_0, Point2 vel_0,
		      realno var_x, realno var_y, realno var_dx, realno var_dy,
		      realno dt = (1.0/15.0));
    
    inline FilterTwoD *clone() const
	{
	    DynamicKalmanTwoD* res = new DynamicKalmanTwoD();
	    *res = *this;
	    return res; 
	}
    
    
    // reset diagonal of system noise
    void reset_noise_covariance(realno q_x, realno q_y, 
				realno q_dx, realno q_dy);
    
    
    //
    // add an observation  ...
    // pos_obs  	observed value
    // sensor covariance  [ rxx rxy ]
    //			  [ rxy ryy ]
    
    Point2 add_observation(Point2 pos_obs, realno rxx, realno rxy, realno ryy);
    
    inline Point2 add_observation(Point2 pos_obs, Matrix2by2 R)
	{
	    return add_observation(pos_obs, R.val00, R.val01, R.val11);
	}
    
    realno delta_update(Point2 delta_pos, realno rxx, realno rxy, realno ryy);
    
    inline realno delta_update(Point2 delta_pos, Matrix2by2 R_i)
	{
	    return delta_update(delta_pos, R_i.val00, R_i.val01, R_i.val11);
	}
    //
    // get_prediction and update estimates ...
    // delta_t 	time elapsed since last update
    //
    
    Point2 get_prediction(realno delta_t);
    inline Point2 get_prediction()
	{
	    return get_prediction(dt);
	}
    
    inline Point2 update_state()
	{
	    return get_prediction(dt); 
	}
    
    inline Point2 state_change()
	{
	    return pos - old_pos; 
	}
    
    void remove_last_observation();
    
    inline Point2& get_velocity() 
	{
	    return velocity; 
	}
    
    inline const Point2 &get_state() const
	{
	    return pos; 
	}
    
    inline void get_positional_covariance(realno& pxx, realno& pxy,
					  realno& pyy)
	{
	    pxx = C00.val00;
	    pxy = C00.val01;
	    pyy = C00.val11;
	}
    
    realno get_uncertainty();
    
    inline Matrix2by2& get_cov() 
	{
	    return C00; 
	}
};

} // namespace ReadingPeopleTracker

#endif  // #ifndef __KALMAN_FILTER_TWO_D_H__