www.pudn.com > 人体步态跟踪识别bate版.rar > KalmanFilter.h

// KalmanFilter.h: interface for the CKalmanFilter class. 
// 
////////////////////////////////////////////////////////////////////// 
 
#if !defined(AFX_KALMANFILTER_H__B171A58C_CD61_47C1_9C80_20925D1C3368__INCLUDED_) 
#define AFX_KALMANFILTER_H__B171A58C_CD61_47C1_9C80_20925D1C3368__INCLUDED_ 
 
#if _MSC_VER > 1000 
#pragma once 
#endif // _MSC_VER > 1000 
/* 
*	kalman滤波器模型为 
*  X'=AX+W      W ~ Q    X[x,y,z,Vx,Vy,Vz,ax,ay,az,ex,ey,ez] 
*  Z =HX+V      V ~ R    Z[x,y,z] 
*  y[k]=y[k-1]+Vy[k-1]+1/2ay[k-1] 
*  Vy[k]=Vy[k-1]+ay[k-1] 
*  ay=ay=const 
*  y的曲线为y=k^2 
*  ay=2 
*  ey=const 
*  Usage: CKFToolBox *kf=new CKFToolBox; 
*         kf->SetInitialXP(); 
*         kf->GetFilteredState();   
*/ 
 
class CKalmanFilter   
{ 
public: 
 
	void ImageMatch(double* XOUT);//图像匹配找到精确位置点 
 
 
	void InitialXP(double *X0, double *P0); 
	//得到状态估计值 
	void GetFilteredState(double *XOUT,double *ZIN); 
	//估计均方误差 
	void GetP(); 
    //一步预测均方误差 
	void GetPPRE(); 
	//得到滤波器增益 
	void GetGainK(); 
    //得到预测值供外部使用 
    //得到下一个状态的预测值 
	void GetNextState(double *XOUT); 
 
	//矩阵复制运算 
	void MatrixCpy(double *dest, double *src, int count); 
	//矩阵赋值 
	void MatrixSet(double *dest, double c, int count); 
	 
	CKalmanFilter(); 
	virtual ~CKalmanFilter(); 
 
private: 
	//矩阵求转置 
	void MatrixInversion(double * Matrix,int iDim); 
	//矩阵相乘运算 
	void MatrixMultiplication(double *Matrix1,int iRow1,int iCol1, double *Matrix2,int iRow2,int iCol2,double *MatrixOut); 
	//矩阵相减 
	void MatrixSubtraction(double *Matrix1,double *Matrix2,int iRow,int iCol); 
	//矩阵相加 
	void MatrixAddition(double *Matrix1,double *Matrix2,int iRow,int iCol); 
	//矩阵求逆 
	void MatrixTranversion(double *Matrix,int iRow,int iCol,double *MatrixOut); 
	//初始化观测矩阵G 
	void InitializeMatricG(); 
    //初始化状态转移矩阵F 
	void InitializeMatricF(double timeinterval); 
	//初始化增益K 
	void InitializeGainK(); 
    //初始化X,P的预测值 
	void InitializeXPPre(); 
	//初始化噪声协方差 
	void InitializeNoiseCorr(); 
	//初始化观测矩阵H 
	void InitializeH(); 
	 
private: 
	double X[DIMENSION];                         //n维状态矢量 估计值 
    double XPRE[DIMENSION];                      //n维状态矢量 预测值 
 
    double P[DIMENSION*DIMENSION];               //n*n维协方差阵 估计值 
	double PPRE[DIMENSION*DIMENSION];            //n*n维协方差阵 预测值 
 
	double K[DIMENSION*DIMENSIONOUT];            //n*m维滤波器增益矩阵 
 
	double Q[DIMENSIONOUT*DIMENSIONOUT];               //n*n维状态噪声矩阵 
	double R[DIMENSIONOUT*DIMENSIONOUT];         //m*m维观测噪声矩阵 
 
    double I[DIMENSION*DIMENSION];               //单位阵 
 
	double m_G_Matric[DIMENSION*DIMENSIONOUT];    //m*n维观测矩阵 
	double m_F_Matric[DIMENSION*DIMENSION];       //n*n维状态转移矩阵 
	double m_H_Matric[DIMENSIONOUT*DIMENSION];    //m*n维观测矩阵 
	 
	double  m_TimeInterval;                       //时间间隔 
	 
	CPoint  m_CurrentPoint;                      //当前要预测的点坐标 
}; 
 
#endif // !defined(AFX_KALMANFILTER_H__B171A58C_CD61_47C1_9C80_20925D1C3368__INCLUDED_)