kalman.cpp

opencv实现的人体运动跟踪源码

#include "stdafx.h"
#include "Kalman.h"
#include "Matrix.h"

CKalman::CKalman( int D, int M, int C ){
	if( D <= 0 || M <= 0 ){
		AfxMessageBox("state and measurement vectors must have positive number of dimensions! ");	
	}
	if( C < 0 )
	{
        AfxMessageBox("No Control!");
	}
    DP = D;
    MP = M;
    CP = C;
	state_pre = new double[DP * 1];
	memset( state_pre ,  0 , sizeof (*state_pre) );
	state_post = new double[DP * 1];
	memset( state_post , 0 , sizeof (*state_post) );
    transition_matrix = new double[DP * DP];
	memset( transition_matrix , 0 , sizeof (*transition_matrix) );
	process_noise_cov = new double[DP * DP];
	memset( process_noise_cov , 0 , sizeof (*process_noise_cov) );
	measurement_matrix = new double[MP * DP];
	memset( measurement_matrix , 0 , sizeof (*measurement_matrix) );
	measurement_noise_cov = new double[MP * MP];
	memset( measurement_noise_cov , 0 , sizeof (*measurement_noise_cov) );
	error_cov_pre = new double[DP * DP];
	memset( error_cov_pre , 0 , sizeof (*error_cov_pre) );
	error_cov_post = new double[DP * DP];
	memset( error_cov_post , 0 , sizeof (*error_cov_post) );
	gain = new double[DP * MP];
	memset( gain , 0 , sizeof (*gain) );
	if( CP > 0 )
    {
        control_matrix = new double[ DP * CP ];
        memset( control_matrix , 0 , sizeof (*control_matrix) );
    }
}
CKalman::~CKalman(){
	delete [] state_pre ;
    delete [] state_post ;
    delete [] transition_matrix;
	if(CP >0){
      delete [] control_matrix ;
	}
    delete [] measurement_matrix;
    delete [] process_noise_cov;
    delete [] measurement_noise_cov;
    delete [] error_cov_pre;
    delete [] gain;
    delete [] error_cov_post;
}

CKalman::KalmanPredict( double * control ){

	CMatrix matrix;
	/* update the state */
    /* x'(k) = A*x(k) */
    matrix.Mul( transition_matrix, DP , DP ,state_post, DP, 1 , state_pre );

    if( control && CP > 0 ){
        /* x'(k) = x'(k) + B*u(k) */
		double * temp1 = new double [DP * 1];
        matrix.Mul( control_matrix, DP , CP , control, CP , 1 , temp1);
	    matrix.Add( state_pre, temp1 , state_pre, DP , 1);
		delete [] temp1;
		temp1 = NULL;
    }

    /* update error covariance matrices */
    /* temp2 = A*P(k) */
	double * temp2 = new double[ DP * DP ];
    matrix.Mul( transition_matrix, DP , DP , error_cov_post, DP , DP , temp2);
 
    /* P'(k) = temp2*At + Q */
	double * temp3 = new double[ DP * DP ];
	double * temp4 = new double[ DP * DP ];
	matrix.Transpose( transition_matrix , DP , DP , temp3);
    matrix.Mul(temp2 , DP ,DP , temp3 , DP , DP , temp4);
	matrix.Add(temp4 , process_noise_cov , error_cov_pre ,DP ,DP );
	delete [] temp2;  temp2 = NULL;
	delete [] temp3;  temp3 = NULL;
	delete [] temp4;  temp4 = NULL;
}

CKalman::KalmanCorrect( double * measurement ){

    if( !measurement ){
		AfxMessageBox("Measurement is Null!!!");
	}
    CMatrix matrix;
    
	/* temp1 = Ht*/
	double * temp1 = new double [ DP * MP ]; 
	matrix.Transpose( measurement_matrix , MP , DP ,temp1);
	/* temp2 = P'(k) * temp1 */
	double * temp2 = new double [DP * MP ];
    matrix.Mul( error_cov_pre, DP , DP , temp1 , DP , MP , temp2);

    /* temp3 = H*temp2 + R */
	double * temp3 = new double [ MP * MP ];
    matrix.Mul( measurement_matrix , MP , DP , temp2 , DP , MP ,temp3);
	matrix.Add( temp3 , measurement_noise_cov , temp3 , MP , MP);

    /* temp4 = inv(temp3) */
	double * temp4 = new double [ MP * MP ];
    matrix.ContraryMatrix( temp3 , temp4 ,MP);   
    
    /* K(k) = temp2 * temp4  */
    matrix.Mul( temp2 ,DP , MP , temp4 , MP ,MP ,gain);
    delete [] temp1;  temp1 = NULL;
	delete [] temp2;  temp2 = NULL;
	delete [] temp3;  temp3 = NULL;
	delete [] temp4;  temp4 = NULL;

    //update state_post
    /* temp5 = z(k) - H*x'(k) */
    double * temp5 = new double [ MP * 1 ];
	matrix.Mul( measurement_matrix , MP , DP , state_pre , DP , 1, temp5);
	matrix.Sub( measurement , temp5 , temp5, MP , 1);    
    /* x(k) = x'(k) + K(k)*temp5 */
	double * temp6 = new double [ DP * 1];
	matrix.Mul( gain ,DP , MP,  temp5 , MP , 1, temp6 );
    matrix.Add( state_pre ,temp6 ,state_post ,DP ,1);
    delete [] temp5;  temp5 = NULL;
	delete [] temp6;  temp6 = NULL;
    
	//update error_cov_post
    /* P(k) = P'(k) - K(k)*H* P'(k) */
	double * temp7 = new double [ MP * DP ];
	double * temp8 = new double [ DP * DP ];
    matrix.Mul( measurement_matrix , MP , DP ,error_cov_pre ,DP ,DP , temp7 );
	matrix.Mul( gain ,DP ,MP, temp7 ,MP ,DP ,temp8 );
	matrix.Sub( error_cov_pre , temp8 , error_cov_post, DP ,DP );
	delete [] temp7;  temp7 = NULL;
	delete [] temp8;  temp8 = NULL;
}