kalmanfilteroned.cc

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

/***************************************************************
 * 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 <cstdio>

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

namespace ReadingPeopleTracker
{

//#define USE_OVERWEIGHTING1
#define USE_OVERWEIGHTING2
//#define USE_OVERWEIGHTING3

StaticKalmanOneD::StaticKalmanOneD(realno Q, realno r_0, realno b_0,
                                   realno p11_0, realno eps)
{
    
    q = Q;
    old_p11 = p11 = p11_0;
    old_b = b = b_0;
    def_r = r_0;
    epsilon = eps;
    
}


realno StaticKalmanOneD::update_state()
{
    realno new_p11 =  p11 + q;
    old_p11 = p11 = new_p11;
    old_b = b;
    return b;
}

realno StaticKalmanOneD::add_observation(realno b_obs, realno r)
{
    old_b = b; old_p11 = p11;
    
    b_obs -= b;
    realno denom = p11 + r;
    
#ifdef USE_OVERWEIGHTING2
    b += ((p11 + epsilon * epsilon * r) / denom) * b_obs;
    p11 = ((p11 + epsilon * epsilon * r)  * r) / denom;
    
//   realno gain1 = p11 / denom;
//   realno gain2 = q  / (q + r);
//   realno gain = gain1 + epsilon * gain2;
//   b += gain * b_obs;
//   p11 = gain * r;
    
#else
    b += (1.0 + epsilon) * (p11  / denom) * b_obs;
    p11 = (1.0 + epsilon) * (p11 * r) / denom;
#endif
    return b;
}


realno StaticKalmanOneD::delta_update(realno delta_b, realno inv_r)
{
    if (inv_r == 0) return 0;
    
    old_b = b; old_p11 = p11;
    realno denom = 1 + p11 * inv_r;
    
#ifdef USE_OVERWEIGHTING2
    p11 = ((p11 + (epsilon * epsilon / inv_r))) / denom;
    
//   realno method1 = p11  / (1.0 + p11 * inv_r);
//   realno method2 = q / (1.0 + q * inv_r);
    
//   p11 = (1 /* - epsilon */) * method1 + epsilon * method2;
    
#else
    p11 = (p11 / denom);
#endif
    
    b += p11 * delta_b;
    return p11 * inv_r;
    
}

void StaticKalmanOneD::remove_last_observation()
{
    b = old_b;
    p11 = old_p11;
}

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

void StaticKalmanOneD::add_noise(realno var)
{
    b += sqrt(var) * NagVector::normal_random();
    p11 += var;
}
#endif



DynamicKalmanOneD::DynamicKalmanOneD(realno Q11, realno Q12, realno Q22,
				     realno r_0, realno b_0, realno
				     db_0, realno p11_0, realno p12_0, realno
				     p22_0 , realno delta_t, realno eps)
{
    
    q11 = Q11; q12 = Q12; q22 = Q22;
    old_p11 = p11 = p11_0;
    old_p12 = p12 = p12_0;
    old_p22 = p22 = p22_0;
    dt = delta_t;
    old_b = b = b_0;
    old_db = db = db_0;
    def_r = r_0;
    t = 0;
    epsilon = eps;
    
}


realno DynamicKalmanOneD::get_prediction(realno delta_t)
{
    b += delta_t * db;
    t += delta_t;
    realno new_p11 =  p11 + delta_t * (2 * p12 + delta_t * p22) +
	delta_t * q11;
    realno new_p12 =  p12 + (delta_t * p22) + delta_t * q12;
    realno new_p22 = p22 + (delta_t * q22);
    old_p11 = p11 = new_p11;
    old_p12 = p12 = new_p12;
    old_p22 = p22 = new_p22;
    old_b = b;
    old_db = db;
    return b;
}

realno DynamicKalmanOneD::add_observation(realno b_obs, realno r)
{
    old_b = b;
    old_db =db;
    old_p11 = p11;
    old_p12 = p12;
    old_p22 = p22;
    
    b_obs -= b;
    realno denom = p11 + r;
    
#ifdef USE_OVERWEIGHTING1
    
    b += ((p11 + epsilon * epsilon * r) / denom) * b_obs;
    
    b += (p11  / denom) * b_obs;
    db += (p12 / denom) * b_obs;
    realno new_p11 = ((p11 + epsilon * epsilon * r)  * r) / denom;
    
    
//   realno gain1 = p11 / denom;
//   realno q_pos = 0.5 * q22 * dt * dt * dt;
//   realno gain2 = q_pos / (q_pos + r);
//   realno gain = gain1 + epsilon * gain2;
//   b += gain * b_obs;
//   db += (p12 / denom) * b_obs;
    
//   realno new_p11 = r * gain;
    
#else
    
    b += (1.0 + epsilon) * (p11 / denom) * b_obs;
    db += (1.0 + epsilon) * (p12 / denom) * b_obs;
    realno new_p11 = (p11 * r) / denom;
    
#endif
    
    
    realno new_p12 = (p12 * r) / denom;
    realno new_p22 = p22 - ((p12 * p12) / denom);
    
    
    p11 = new_p11;
    p12 = new_p12;
    p22 = new_p22;
    return b;
}

realno DynamicKalmanOneD::delta_update(realno delta_b, realno inv_r)
{
    if (inv_r == 0) return 0;
    old_b = b;
    old_db = db;
    old_p11 = p11;
    old_p12 = p12;
    old_p22 = p22;
    
    realno denom = 1 + p11 * inv_r;
    
    p22 -= (p12 * p12 * inv_r) / denom;
    p12 /= denom;
    
#ifdef USE_OVERWEIGHTING1
    p11 = (p11 / denom) + (epsilon * epsilon) / (inv_r * denom);
//   realno method1 = p11  / (1.0 + p11 * inv_r);
//   realno q_pos = 0.5 * q22 * dt * dt * dt;
//   realno method2 = q_pos / (1.0 + q_pos * inv_r);
    
//   p11 = (1 /* - epsilon */) * method1 + epsilon * method2;
    
#else
    p11 = (p11 / denom);
#endif
    
    
    b += p11 * delta_b;
    db += p12 * delta_b;
    
    // return the Kalman gain for parameter 'b'
    return p11 * inv_r;
    
}

void DynamicKalmanOneD::remove_last_observation()
{
    b = old_b;
    db = old_db;
    p11 = old_p11;
    p12 = old_p12;
    p22 = old_p22;
}


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

void DynamicKalmanOneD::add_noise(realno var)
{
    b += sqrt(var) * NagVector::normal_random();
    p11 += var;
}
#endif

VibrationFilter::VibrationFilter(realno Q11, realno Q12, realno Q22,
				 realno pass_a, realno pass_b,
				 realno r_0, realno x_0, realno dx_0,
				 realno p11_0, realno p12_0, 
				 realno p22_0, realno delta_t)
{
    q11 = Q11; q12 = Q12; q22 = Q22;
    a = pass_a; b = pass_b;
    p11 = p11_0;
    p12 = p12_0;
    p22 = p22_0;
    dt = delta_t;
    x = x_0;
    bias = x_p = 0;
    dx = dx_0;
    def_r = r_0;
    t = 0;
    
}

// assume delta_t is small
// the exact solution exists in terms of exponentials
// too difficult for the moment!

realno VibrationFilter::get_prediction(realno delta_t)
{
//  bias += 0.1 * (x - x_p);
    NagMatrix thi(2,2); 
    NagVector new_x(2);
    NagVector old_x(2);
    
    thi.set(0,0,1); thi.set(0,1,delta_t);
    thi.set(1,0, a * delta_t); thi.set(1,1,1 + b * delta_t);
    old_x[0] = x; old_x[1] = dx;
    thi.multiply(old_x, new_x);
    x = new_x[0]; dx = new_x[1] - thi.read(1,0) * bias;
    
    NagMatrix tmp_P(2,2); NagMatrix tmp_Q(2,2);
    NagMatrix new_P;
    
    tmp_P.set(0,0,p11); tmp_P.set(0,1,p12);
    tmp_P.set(1,0,p12); tmp_P.set(1,1,p22);
    tmp_Q.set(0,0,q11); tmp_Q.set(0,1,q12);
    tmp_Q.set(1,0,q12); tmp_Q.set(1,1,q22);
    
    tmp_P.ortho_transform(thi, new_P, false);
    tmp_Q.scale(delta_t, tmp_Q);
    new_P.add(tmp_Q, new_P);
    
    p11 = new_P.read(0,0); p12 = new_P.read(0,1);
    p22 = new_P.read(1,1);
    
    t += delta_t;
    x_p = x;
    return x;
}

realno VibrationFilter::add_observation(realno x_obs, realno r)
{
    
    x_obs -= x;
    realno denom = p11 + r;
    
    x += (p11 / denom) * x_obs;
    dx += (p12 / denom) * x_obs;
    realno new_p11 = (p11 * r) / denom;
    realno new_p12 = (p12 * r) / denom;
    realno new_p22 = p22 - ((p12 * p12) / denom);
    
    
    p11 = new_p11;
    p12 = new_p12;
    p22 = new_p22;
    return x;
}

realno VibrationFilter::delta_update(realno delta_x, realno inv_r)
{
    if (inv_r == 0)
	return 0;
    
    realno denom = 1 + p11 * inv_r;
    
    p22 -= (p12 * p12 * inv_r) / denom;
    p12 /= denom;
    p11 = (p11 / denom);
    x += p11 * delta_x;
    dx += p12 * delta_x;
    
    // return the Kalman gain for 'x'
    return p11 * inv_r;    
}

} // namespace ReadingPeopleTracker