ukalmanfilter.cpp

InnovLabSimu在vc++下实现

//#include "StdAfx.h"
#include "UKalmanFilter.h"

CUKalmanFilter::CUKalmanFilter(void):CUncentedScheml(4,2),m_mAugmantedState(6),m_uStateSize(4)
{
	m_dAlpha=0.5;
	m_dBeta=2;
	m_dKapa=3-4;
}
CUKalmanFilter::CUKalmanFilter(size_t StateSize,size_t ObserveDataSize,size_t AugmenteSize):CUncentedScheml(StateSize,AugmenteSize),
m_mAugmantedState(StateSize+AugmenteSize),m_mAugmantedCov(StateSize+AugmenteSize,StateSize+AugmenteSize),m_uStateSize(StateSize),
m_mResidual(ObserveDataSize)

{
	m_dAlpha=0.5;
	m_dBeta=2;
	m_dKapa=static_cast<double>(3-(int)StateSize-(int)AugmenteSize);
}
CUKalmanFilter::~CUKalmanFilter(void)
{
}
void CUKalmanFilter::Predict(STATE_STYLE& PreStateMean,VARIANCE& PreStateCov,CPredictModel& PredictObj,
							VARIANCE& ControlNoise, VARIANCE& ModelNoise,double dl,double dr,
							double TimeStep)
{
	m_mAugmantedCov.clear();//BOOST中clear()是清0还是清除??这里要的是清0,达到了效果,但文档中是清除????

#ifdef  DEBUG_USE_FLAG
	ViewVector(PreStateMean);
	m_mSigma_Viewer;
#endif
#ifdef  DEBUG_USE_FLAG
	ViewMatrix(PreStateCov);
	m_mSigma_Viewer;
#endif
#ifdef  DEBUG_USE_FLAG
	ViewMatrix(ControlNoise);
	m_mSigma_Viewer;
#endif
#ifdef  DEBUG_USE_FLAG
	ViewMatrix(ModelNoise);
	m_mSigma_Viewer;
#endif
	/////////////////////扩充状态变量/////////////////////////////////////////////////////
	subrange(m_mAugmantedState,0,m_uStateSize)=PreStateMean;
	//m_mAugmantedState(m_uStateSize)=dl;
	//m_mAugmantedState(m_uStateSize+1)=dr;
	m_mAugmantedState(m_uStateSize)=0;
	m_mAugmantedState(m_uStateSize+1)=0;
	//////////////////////将控制噪声扩充方差阵////////////////////////////////////////////////////
	//range LineRang(0,m_uStateSize);
	//range RowRang(0,m_uStateSize);
	subrange(m_mAugmantedCov,0,m_uStateSize,0,m_uStateSize)=PreStateCov;
	/*range LineRang2(m_uStateSize,m_uStateSize+2);
	range RowRang2(m_uStateSize,m_uStateSize+2);*/
	subrange(m_mAugmantedCov,m_uStateSize,m_uStateSize+2,m_uStateSize,m_uStateSize+2)=ControlNoise;

	///////////////////////调用 UT变换，进行预测///////////////////////////////////////////////////
#ifdef CONSIDER_TIME_IN_VEL

	ut_transform(m_mAugmantedState,m_mAugmantedCov,PredictObj,dl,dr,m_dAlpha,m_dBeta,m_dKapa,TimeStep);
#else
	ut_transform(m_mAugmantedState,m_mAugmantedCov,PredictObj,dl,dr,m_dAlpha,m_dBeta,m_dKapa);
#endif

#ifdef  DEBUG_USE_FLAG
	ViewVector(m_mStateMeanResult);
	m_mSigma_Viewer;
#endif
#ifdef USE_TRADITIONAL_AKF
	m_mPredictStateVar=m_mStateCov;//保存状态预测后的方差阵
#endif
	
	m_mStateCov+=ModelNoise;//增加模型噪声	
#ifdef  DEBUG_USE_FLAG
	ViewMatrix(m_mStateCov);
	m_mSigma_Viewer;
#endif

}
matrix_type& CUKalmanFilter::CalulateKalmanGain(TRANSTATE_MAT_TYPE& TranslateMatrix,VARIANCE MeasureNoise)
{
	m_Covariance_S=prod( matrix_type(prod(TranslateMatrix,m_mStateCov)),trans(TranslateMatrix))+MeasureNoise;//Sk = HPH' + Rk
#ifdef  DEBUG_USE_FLAG
	ViewMatrix(m_Covariance_S);
	m_mSigma_Viewer;
#endif

	matrix_type tmp(m_Covariance_S.size1(),m_Covariance_S.size2());
	CalMatrixReverse(m_Covariance_S,tmp);
#ifdef  DEBUG_USE_FLAG

	ViewMatrix(tmp);
	m_mSigma_Viewer;
#endif
	m_mKalmanGain=prod(matrix_type(prod(m_mStateCov,trans(TranslateMatrix))),tmp);
	return m_mKalmanGain;
}
void CUKalmanFilter::Update(STATE_STYLE& PreStateMean,VARIANCE& PreStateCov,CObservModel& ObserveObj,
							OBSERVE_DATA_STYLE& ObserveData,VARIANCE& ObserveNoiseCov,TRANSTATE_MAT_TYPE& TranslateMatrix)
{
	CalulateKalmanGain(TranslateMatrix,ObserveNoiseCov);
#ifdef  DEBUG_USE_FLAG
	ViewVector(PreStateMean);
	m_mSigma_Viewer;
#endif
#ifdef  DEBUG_USE_FLAG

	ViewVector(ObserveData);
	m_mSigma_Viewer;
#endif
	ObserveObj.Update(PreStateMean);
	//m_mResidual=ObserveData-ObserveObj.m_mPredictObserve;
	m_mResidual(0)=ObserveData(0)-ObserveObj.m_mPredictObserve(0);
	m_mResidual(1)=GetAngulardisplacementIn2Angle_Degree(ObserveObj.m_mPredictObserve(1),ObserveData(1));
	
#ifdef  DEBUG_USE_FLAG

	ViewMatrix(m_mKalmanGain);
	m_mSigma_Viewer;
#endif
#ifdef  DEBUG_USE_FLAG

	ViewVector(m_mResidual);
	m_mSigma_Viewer;
#endif
	m_mStateMeanResult=PreStateMean+prod(m_mKalmanGain,m_mResidual);//计算期望
	LimitAngleIn0_360Degree(m_mStateMeanResult(1));
	//LimitAngleIn0_360Degree(m_mStateMeanResult(3));
#ifdef  DEBUG_USE_FLAG

	ViewVector(m_mStateMeanResult);
	m_mSigma_Viewer;
#endif
	m_mStateCov=PreStateCov-prod(matrix_type(prod(m_mKalmanGain,m_Covariance_S)),trans(m_mKalmanGain));//计算方差
	#ifdef  DEBUG_USE_FLAG

		ViewMatrix(m_mStateCov);
		m_mSigma_Viewer;
	#endif

}
void CUKalmanFilter::initFilter(STATE_STYLE PreStateMean,VARIANCE PreStateCov)
{
	m_mStateMeanResult=PreStateMean;
	m_mStateCov=PreStateCov;
}