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

// KalmanFilter.cpp: implementation of the CKalmanFilter class. 
// 
////////////////////////////////////////////////////////////////////// 
 
#include "stdafx.h" 
#include "humantrack.h" 
#include "KalmanFilter.h" 
 
#ifdef _DEBUG 
#undef THIS_FILE 
static char THIS_FILE[]=__FILE__; 
#define new DEBUG_NEW 
#endif 
////////////////////////////////////////////////////////////////////// 
// Construction/Destruction 
////////////////////////////////////////////////////////////////////// 
 
CKalmanFilter::CKalmanFilter() 
{ 
	m_TimeInterval=1.0/25.0 ;           //初始化时间间隔为1.0/25.0 
	InitializeH();                      //初始化H矩阵 
	InitializeMatricF(m_TimeInterval);  //初始化F矩阵 
	InitializeMatricG();                //初始化G矩阵 
	InitializeNoiseCorr();              //初始化噪声方差矩阵R和Q 
	InitializeXPPre();                  //初始化X值和p值 
	InitializeGainK();                  //初始化增益K矩阵 
} 
 
CKalmanFilter::~CKalmanFilter() 
{ 
 
} 
///////////////////////////////////////////////////////////////////////// 
//初始化X0和P0 InitialXP(double *X0, double *P0) 
//初始化H      InitialzeH() 
//初始化F      InitializeMatricF() 
//初始化G      InitializeMatricG() 
//初始化R和Q   InitializeNoiseCorr() 
//初始化增益K  InitializeGainK() 
////////////////////////////////////////////////////////////////////////// 
 
//初始化状态矢量X和状态矢量协方差P 
void CKalmanFilter::InitialXP(double *X0, double *P0) 
{ 
	int i,j; 
	for(i=0;i