ukalmanfilter.h

路径规划源程序

#pragma once
#include "UncentedScheml.h"
#include "UKF_COMMAN_HEAD.h"
#include "..\ObjectAngleCheck.h"
#include <boost/numeric/ublas/vector_proxy.hpp>

/********************************************************************
	created:	2008/04/25
	created:	25:4:2008   15:47
	filename: 	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005\PathPlan\UncentedKalmanFilt\UKalmanFilter.h
	file path:	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005\PathPlan\UncentedKalmanFilt
	file base:	UKalmanFilter
	file ext:	h
	author:		GaoYang
	
	purpose:	Kalman滤波器类,本类从UT变换,等继承来,形成UKF
*********************************************************************/
class CUKalmanFilter :public CUncentedScheml,virtual public BOOST_DEBUG_TOOL,public ObjectAngleCheck
{
public:
	/**
	*构造函数，默认4维状态
	*/
	CUKalmanFilter(void);
	/**
	*构造函数，输入扩充前的状态维数，默认4维
	*AugmenteSize为扩充的维数
	*/
	CUKalmanFilter(size_t StateSize,size_t ObserveDataSize,size_t AugmenteSize);
	const size_t m_uStateSize;
	void Predict(STATE_STYLE& PreStateMean,VARIANCE& PreStateCov,CPredictModel& PredictObj,
		VARIANCE& ControlNoise, VARIANCE& ModelNoise,double dl,double dr,double TimeStep=0);
public:
	virtual~CUKalmanFilter(void);
	/**
	*初始化函数,设置均值以及方差阵的均值
	*/
	void initFilter(STATE_STYLE PreStateMean,VARIANCE PreStateCov);
protected:
	STATE_STYLE m_mAugmantedState;///<扩充后的状态均值
	VARIANCE m_mAugmantedCov;///<扩充后的方差
	PARM m_dAlpha,m_dBeta,m_dKapa;///<系统UT变换参数，有默认值
	matrix_type& CalulateKalmanGain(TRANSTATE_MAT_TYPE& TranslateMatrix,VARIANCE MeasureNoise);
public:
	matrix_type m_mKalmanGain;
#ifdef USE_TRADITIONAL_AKF
	VARIANCE m_mPredictStateVar;///<状态预测后的方差阵
#endif
	
public:
	VARIANCE m_Covariance_S;///<协方差S,更新时用到
	


public:
	void Update(STATE_STYLE& PreStateMean,VARIANCE& PreStateCov,CObservModel& ObserveObj,
		OBSERVE_DATA_STYLE& ObserveData,VARIANCE& ObserveNoiseCov,TRANSTATE_MAT_TYPE& TranslateMatrix);


	//STATE_STYLE
	OBSERVE_DATA_STYLE m_mResidual;
};