kalmanfiltertwod.cc

来自「VC视频对象的跟踪提取原代码（vc视频监控源码）」· CC 代码 · 共 292 行

/***************************************************************
 * C++ source
 *
 * File : 	KalmanFilter.cc
 *
 * Module :	KalmanFilter
 *
 * Author : 	A M Baumberg
 *
 * Creation Date : Mon Dec  6 12:53:14 1993 
 *
 * Comments : 	various Kalman Filter classes
 *
 ***************************************************************/


/* include files */

#include <cstdio>

#include "KalmanFilterTwoD.h"
#include "NagVector.h"
#include "NagMatrix.h"
#include "text_output.h"

namespace ReadingPeopleTracker
{


StaticKalmanTwoD::StaticKalmanTwoD(realno Qxx, realno Qxy, realno Qyy, 
				   realno bx_0, realno by_0,
				   realno pxx_0, realno pxy_0, 
				   realno pyy_0) :
    Q(Qxx, Qxy, Qyy),
    b(bx_0, by_0),
    old_b(bx_0, by_0),
    Cov(pxx_0, pxy_0, pyy_0),
    old_Cov(pxx_0, pxy_0, pyy_0)
{
    // nothing
}


realno StaticKalmanTwoD::delta_update(realno dbx_obs, realno dby_obs,  
				      realno rxx, realno rxy, realno ryy)
{  
    old_Cov = Cov;
    old_b = b;
    
    realno det1 =  Cov.determinant();
    if (det1 < 0)
    {
	cerror << " StaticKalmanTwoD::delta_update(): det Cov < 0, aborting..." << endl;
	abort(); 
    }
    
    // if det1 too small, measurement has no effect so return
    if (det1 < 1e-30)
	return 0;
    
    Matrix2by2 R_inv(rxx,rxy,ryy);
    Matrix2by2 tmp_m = Cov.inverse() + R_inv;
    
    realno det2 = tmp_m.determinant();
    if (det2 < 0)
    {
	cerror << " StaticKalmanTwoD::delta_update(): det2 < 0, aborting..." << endl;
	abort();
    }
    
    Cov = tmp_m.inverse();
    Point2 db_obs(dbx_obs, dby_obs);
    
    b += Cov.multiply(db_obs);
    
    Matrix2by2 K_gain = Cov * R_inv;
    // return maximum eigenvalue as upper bound on gain
    return K_gain.get_max_real_eigenvalue();
}

Point2 StaticKalmanTwoD::add_observation(Point2 b_obs, Matrix2by2 R)
{
    Matrix2by2 tmp((Cov + R).inverse());
    Matrix2by2 K = Cov.multiply(tmp);
    
    Point2 db(b_obs - b);
    
    old_b = b;
    
    b += K.multiply(db);
    
    old_Cov = Cov;
    
    Cov -= K.multiply(old_Cov);
    
    return b;
}

void StaticKalmanTwoD::update_state()
{
    Cov = Cov + Q;
    old_Cov = Cov;
}


void StaticKalmanTwoD::remove_last_observation()
{
    Cov = old_Cov;
    b = old_b;
}

void StaticKalmanTwoD::set_polar_noise_cov(realno sv, realno tv)
{
    realno bx2 = b.x * b.x;
    realno by2 = b.y * b.y;
    realno bxy = b.x * b.y;
    Q = Matrix2by2((bx2 * sv + by2 * tv),
		   (bxy * (sv - tv)),
		   (by2 * sv + bx2 * tv));
}


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

void StaticKalmanTwoD::add_noise(realno var_x, realno var_y)
{
    b.x += sqrt(var_x) * NagVector::normal_random();
    b.y += sqrt(var_y) * NagVector::normal_random();
    Cov.val00 += var_x;
    Cov.val11 += var_y;
}
#endif

realno StaticKalmanTwoD::get_uncertainty()
{ 
    // return maximum eigenvalue of the covariance matrix
    // which is a bound on the expected errors
    return Cov.get_max_real_eigenvalue();
    
}

// 2D constant velocity discrete Kalman Filter
// 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::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 del_t) :
    Q00(q_x, q_y), Q11(q_dx, q_dy),
    C00(var_x, var_y), C11(var_dx, var_dy),
    old_C00(var_x, var_y), old_C11(var_dx, var_dy),
    pos(pos_0), velocity(vel_0), old_pos(pos_0), old_velocity(vel_0)
{
    
    t = 0;
    dt = del_t;
    // nothing left to initialise    
}

//#ifndef WIN32
//inline
//#endif
void DynamicKalmanTwoD::reset_noise_covariance(realno q_x, realno q_y,
					       realno q_dx, realno q_dy)
{
    Q00 = Matrix2by2(q_x, q_y);
    Q11 = Matrix2by2(q_dx, q_dy);
}


//
// add an observation  ...
// pos_obs  	observed value
// sensor covariance [ rxx rxy ]
//                   [ rxy ryy ]

Point2 DynamicKalmanTwoD::add_observation(Point2 pos_obs, realno rxx, 
					  realno rxy, realno ryy)
{
    Matrix2by2 R(rxx, rxy, ryy);
    Matrix2by2 tmp((C00 + R).inverse());
    Matrix2by2 K_pos = C00.multiply(tmp);
    Matrix2by2 K_vel = C01.multiply(tmp);
    
    Point2 dp(pos_obs - pos);
    
    old_pos = pos;
    old_velocity = velocity;
    
    pos += K_pos.multiply(dp);
    velocity += K_vel.multiply(dp);
    
    old_C00 = C00; old_C01 = C01; old_C11 = C11;
    
    C00 -= K_pos.multiply(old_C00);
    C01 -= K_pos.multiply(old_C01);
    C11 -= K_vel.multiply(old_C01);
    
    return dp;   // FIXME: added by nts 31/10/00  ---  correct???
}

// delta_update ...
// add an observation 
// with 
//	delta_pos = ( R^{-1} (pos_obs - pos) ) 
// and measurement inverse covariance R^{-1} given by
//
//	R^{-1} =	[ rxx rxy ]
//			[ rxy ryy ]	
// 
// return the Kalman gain

realno DynamicKalmanTwoD::delta_update(Point2 delta_pos, realno rxx, 
				       realno rxy, realno ryy)
{
    
    
    Matrix2by2 R_inv(rxx, rxy, ryy);
    if (fabs(R_inv.determinant()) < 1e-30) return 0.0;
    
    Matrix2by2 R(R_inv.inverse());
    
    Point2 dp(R.multiply(delta_pos));
    
    Matrix2by2 tmp((C00 + R).inverse());
    Matrix2by2 K_pos = C00.multiply(tmp);
    Matrix2by2 K_vel = C01.multiply(tmp);
    
    old_pos = pos;
    old_velocity = velocity;
    
    pos += K_pos.multiply(dp);
    velocity += K_vel.multiply(dp);
    
    old_C00 = C00; old_C01 = C01; old_C11 = C11;
    
    C00 -= K_pos.multiply(old_C00);
    C01 -= K_pos.multiply(old_C01);
    C11 -= K_vel.multiply(old_C01);
    
    return K_pos.get_max_real_eigenvalue();
}



//
// get_prediction and update estimates ...
// delta_t 	time elapsed since last update
//

Point2 DynamicKalmanTwoD::get_prediction(realno delta_t)
{
    t += delta_t;
    
    pos += delta_t * velocity;
    old_pos = pos;
    
    C00 += C01.scale(2 * delta_t) + C11.scale(delta_t * delta_t)
	+ Q00.scale(delta_t);
    C01 += C11.scale(delta_t);
    C11 += Q11.scale(delta_t);
    
    old_C00 = C00; old_C01 = C01; old_C11 = C11;
    
    return pos;
}



void DynamicKalmanTwoD::remove_last_observation()
{
    C00 = old_C00; C01 = old_C01; C11 = old_C11;
    pos = old_pos; velocity = old_velocity;
}


realno DynamicKalmanTwoD::get_uncertainty()
{
    return C00.get_max_real_eigenvalue();
}

} // namespace ReadingPeopleTracker