kalmantrackor.cpp

通过设置物体的位置和速度信息

// KalmanTrackor.cpp : implementation file
//

#include "stdafx.h"
#include "humancounter.h"
#include "KalmanTrackor.h"
#include "matrix.h"
#include <stdlib.h>
#include "time.h"

#ifdef _DEBUG
#define new DEBUG_NEW 

#define d 4 
#define dv 0.5 

#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif

/////////////////////////////////////////////////////////////////////////////
// KalmanTrackor

KalmanTrackor::KalmanTrackor()
{
	Inibool=false;
   

}

KalmanTrackor::~KalmanTrackor()
{
     clearup(); 
} 

/* 类入后， 把当前度量值给kalman 程序，然后进行跟踪处理        */ 
//函数出后，其他都设成私有函数

//首次看一下独自用kalman的算法效果 
CPoint KalmanTrackor::TrackProcess_NewFrame(int mark,CPoint meansurpoint)
{ 
	if(!Inibool)
	{ // 初始化 			
		
		CreateKalman(2,4,0,4.0,meansurpoint,2,2); // vx ,vy的初值设定，
		//这里还有问题， 如何对目标的初速度进行设定呢  //先统计目标的移动方向
		//然后给一个统计值， 就是根据点的坐标的对比 //先放这儿，流程的时候再编写 
	}  
 CPoint p;
  if(mark==0) //有当前度量值 
  {
   measurement_vector.Init((double)meansurpoint.x,(double)meansurpoint.y); 
   KalmanPredict();
   KalmanCorrect();

   p.x=(int)state_post(1,1);
   p.y=(int)state_post(2,1);
//    return p; 
  }

  if(mark==1) //返回预测的值 OK 
  {

//   measurement_vector.Init(state_post(1,1),state_post(2,1));  
      KalmanPredict();
//    KalmanCorrect();
//  
     p.x=(int)state_pre(1,1);
     p.y=(int)state_pre(2,1);  //返回预测的值  

   
  }
  return p; 
}

//kalman预测函数
//得到预测状态与预测状态的偏差 
//传入的是前一时刻的最优值呢  也就是当前没有修改前的后验状态 
//同理 也要用的没有修改后的最优值的偏差 
 
bool KalmanTrackor:: KalmanPredict() /*预测k时刻状态*/	
{
     matrix tempmatrix;

     tempmatrix.Mult(transition_matrix, state_post) ;  //(4*4) *(4*1)= 4*1 oK ;

     state_pre=tempmatrix; //得到预测的状态 A*S(k-1)

	 //完成    P'(k)=A*P(k-1)*At + Q)*/ 
     tempmatrix.Mult(transition_matrix,error_cov_post); //A*P(k-1) (4*4)*(4*4)=(4*4)
     
	  matrix transpose_transition_matrix;
	  transpose_transition_matrix.Turn(transition_matrix); //得到At 
      
	  matrix tempmatrix1;
	  tempmatrix1.Mult(tempmatrix,transpose_transition_matrix);//A*P(k-1)*At　	  　　
//　　　 time_t t;
//		srand((unsigned) time(&t));
//
//		for(int i=1;i<=D_P;i++)
//		{   double data;
//		data=rand()%100;
//		data=data/20;
//		process_noise_cov(i,i)=data;  //Q  0-5之间的随机数 
//
//　　　　}

      CreateRanddom_ProcessCov();

	  error_cov_pre.Plus(tempmatrix1,process_noise_cov);//P'(k)=A*P(k-1)*At + Q)*/ 
	  

	  tempmatrix.Destroy(); 
	  transpose_transition_matrix.Destroy();
	  tempmatrix1.Destroy(); 

      return true; 

}



/*  创建各个矩阵对象以及进行赋给初值   */
//需要考虑的问题，关于速度的初值问题 
void  KalmanTrackor::CreateKalman(   int MP,              /* 测量向量维数 2*/
				                     int DP,              /* 状态向量维数 4*/
				                     int CP,                 /* 控制向量维数 0 */
				                     double T ,               /* 转移矩阵中的T 2.0*/
									 CPoint Inipoint,       //初始化的检测值 
									 int vx,
									 int vy)   /*第一个初始值 初始化的速度*/
{   
	D_P=DP;
    M_P=MP;
	C_P=CP; 
	state_pre.Set(DP,1); //DP*1的矩阵 实在上是向量 4*1
    state_post.Set(DP,1);  //初始化为 4*1
    transition_matrix.Set(DP,DP);  // 4*4
	
    measurement_vector.Set(MP,1);  //度量向量  2*1
	measurement_matrix.Set(MP,DP); //2*4
    measurement_noise_cov.Set(MP,MP); //度量偏差矩阵 2*2的
	process_noise_cov.Set(DP,DP); //系统噪声 4*4 
	error_cov_post.Set(DP,DP);  //后验偏差 4*4
	error_cov_pre.Set(DP,DP);   //先验预测偏差 首先赋值一个非零的值 
	
	gain.Set(DP,MP); //4*2 矩阵  //随时更新


	double x=(double)Inipoint.x;
	double y=(double)Inipoint.y;
	state_pre.Init(x,y,(double)vx,(double)vy);
	state_post=state_pre;  //先初始化 先验后验相同值 开始设置


	transition_matrix.Init(1.0, 0.0, T, 0.0,
		                   0.0, 1.0, 0.0, T,
						   0.0, 0.0, 1.0, 0.0,
						   0.0, 0.0, 0.0, 1.0); //这个是不变的 
   

	measurement_matrix.Init(1.0, 0.0, 0.0, 0.0,
		                    0.0, 1.0, 0.0, 0.0); //度量矩阵初始化完毕2*4 


   


	//随机数 
    //系统噪声方差矩阵 
	error_cov_post.Unit(); //4*4 
	for(int i=1;i<=D_P;i++)
	{  
		error_cov_post(i,i)=2.0;  //P 的初值 
		
	}
	
    

	process_noise_cov.Unit();//单位阵   //这个还要进一步初始化 我把系统噪声设了1 

	time_t t;
	srand((unsigned) time(&t));
	
	for( i=1;i<=D_P;i++)
	{   double data;
	    data=rand()%100;
	    data=data/20;
        process_noise_cov(i,i)=data;  //Q  0-5之间的随机数 

	}
	measurement_noise_cov.Unit();  //R 
	for( i=1;i<=M_P;i++)
	{    double data;
    	data=rand()%100;
    	data=data/50;
		measurement_noise_cov(i,i)=data; //0-2之间的随机数 
		
	}
// 


//状态偏差赋值   //这个不是随机数， 需要给初值 

//dv dy是什么

	error_cov_pre.Unit(); //给它赋一个单位阵，相当于偏差我们取得为1个像素 //P 
    error_cov_pre(1,1)=d;
	error_cov_pre(2,2)=d;
	error_cov_pre(3,3)=dv;
	error_cov_pre(4,4)=dv;  //方差有d,dv来控制，这里我们却非零值 


	Inibool=true; 


//	 process_noise_cov=new matrix(DP,DP) /*process noise covariance matrix (Q) */

  
    //设定其值  //系统来讲都是固定的 
    // 
 //    control_matrix=NULL; // 没有设置为空

}


/******************************
//更新状态 /*x(k)=x'(k)+K(k)*(z(k)-H*x'(k)) */
/***********************************/
//
void KalmanTrackor::KalmanCorrect_state_post()
{

   matrix tempmult; //2*1
   tempmult.Mult(measurement_matrix,state_pre); //H*x'(k)  (2*4) *(4*1)=(2*1)
   
   matrix tempsub;
   tempsub.Sub(measurement_vector,tempmult);     //(z(k)-H*x'(k)) 2*1
  
   tempmult.Mult(gain,tempsub)    ;              //K(k)*(z(k)-H*x'(k)
   
   state_post.Plus(state_pre,tempmult);   //*x(k)=x'(k)+K(k)*(z(k)-H*x'(k))
  
   tempmult.Destroy();
   tempsub.Destroy(); 
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                             

}

//*******************************************************
//更新状态偏差 P(k)=(I-K(k)*H)*P'(k) */
//*******************************************************
void KalmanTrackor::KalmanCorrect_error_cov_post()
{
    matrix tempmult; //2*1
	tempmult.Mult(gain,measurement_matrix); //K(k)*H) (4*2)*(2*4)=4*4 维数
	matrix I(D_P,D_P);
	I.Unit(); //单位化 
	matrix tempsub;
	tempsub.Sub(I,tempmult); //(I-K(k)*H) 4*4 维数
	error_cov_post.Mult(tempsub,error_cov_pre); //(I-K(k)*H)*P'(k)

    tempmult.Destroy();
    I.Destroy();
    tempsub.Destroy(); 
}
//*********************************************************
//得到增益    K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)*/
//*********************************************************
//这里有矩阵不匹配的地方 
void KalmanTrackor::Kalman_get_gain()
{
	matrix tempmult; //2*1
	
	//K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)*/
	tempmult.Mult(error_cov_pre,measurement_matrix,0,1);//P'(k)*Ht   (4*4)*(4*2)=(4*2) ,Ht 为H的转置
	
	matrix temp_H_P;
	temp_H_P.Mult(measurement_matrix,error_cov_pre);        //H*P'(k) (2*4)*(4*4)=(2*4) 
	matrix temp_H_P_HT;
	/////////////////////////////////wenti 
	temp_H_P_HT.Mult(temp_H_P,measurement_matrix,0,1);  //H*P'(k)*Ht (2*4)*(4*2)=2*2 
    


	matrix tempplus;
	CreateRanddom_MeasureCov();
	tempplus.Plus(temp_H_P_HT,measurement_noise_cov);    //(2*2)+(2*2) //(H*P'(k)*Ht+R)
	

	matrix tempaddend;
	tempaddend.Invers(tempplus);                     // inv(H*P'(k)*Ht+R)*/
	
	gain.Mult(tempmult,tempaddend);  //(4*2) *(2*2)  //K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)*/
   
	tempaddend.Destroy(); 
	temp_H_P.Destroy();
	tempmult.Destroy();
	temp_H_P_HT.Destroy(); 
	tempplus.Destroy();
}

//**********************************************************
//更新状态 /*x(k)=x'(k)+K(k)*(z(k)-H*x'(k)) */
//得到增益    K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)*/
//更新状态偏差 P(k)=(I-K(k)*H)*P'(k) */


/***********************************************************/
void  KalmanTrackor:: KalmanCorrect()
{ 


  
  //K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)*/
    Kalman_get_gain(); 
//**************************************************
//更新状态 /*x(k)=x'(k)+K(k)*(z(k)-H*x'(k)) */
 
   KalmanCorrect_state_post(); 
  ///////////////////////////////////////////////////////
  //P(k)=(I-K(k)*H)*P'(k) */ 计算后验概率的偏差

   KalmanCorrect_error_cov_post(); 
 

}



void  KalmanTrackor:: clearup()
{
	 state_pre.Destroy();
	 state_post.Destroy();
	 transition_matrix.Destroy();
	 process_noise_cov.Destroy();
	//meansurement_vector.Destroy();
	 measurement_noise_cov.Destroy();
	 measurement_matrix.Destroy();
	 measurement_vector.Destroy(); 
 
	 gain.Destroy();
	 error_cov_post.Destroy();
	 error_cov_pre.Destroy();
	 control_matrix.Destroy();
 

}

void KalmanTrackor::CreateRanddom_MeasureCov() //R
{
	time_t t;
	srand((unsigned) time(&t));
	
	for(int i=1;i<=M_P;i++)
	{    double data;
	data=rand()%100;
	data=data/50;
	measurement_noise_cov(i,i)=data; //0-2之间的随机数 
	
	}

}
void KalmanTrackor::CreateRanddom_ProcessCov() //Q 
{
	time_t t;
	srand((unsigned) time(&t));
	
	for(int i=1;i<=D_P;i++)
	{  
		double data;
		data=rand()%100;
		data=data/50;
		process_noise_cov(i,i)=data; //0-2之间的随机数 
	
	}

}


BEGIN_MESSAGE_MAP(KalmanTrackor, CEdit)
	//{{AFX_MSG_MAP(KalmanTrackor)
		// NOTE - the ClassWizard will add and remove mapping macros here.
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// KalmanTrackor message handlers