#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;
};