#pragma once


#include "UKF_COMMAN_HEAD.h"
#include "ObservModel.h"
#include "PredictModel.h"

/********************************************************************
	created:	2008/01/04
	created:	4:1:2008   15:09
	filename: 	c:\boosttest\MyTargetTrack\MyTargetTrack\UncentedKalmanFilt\UncentedScheml.h
	file path:	c:\boosttest\MyTargetTrack\MyTargetTrack\UncentedKalmanFilt
	file base:	UncentedScheml
	file ext:	h
	author:		GaoYang
	
	purpose:	UT變換算法，ut_transform接口函數(shù)，目前僅在預(yù)測階段用到了本方法，
				所以僅ut_transform接口函數(shù)僅有輸入預(yù)測模型的版本，若需用于更新函數(shù)，則必須加一個重載函數(shù)。
				另
				  可以用于擴(kuò)展后的UKF，需在構(gòu)造函數(shù)中指定擴(kuò)充維數(shù)，但存放結(jié)果的m_mStateMeanResult，和m_mStateCov
				都是實(shí)際狀態(tài)變量的期望與方差陣，并不考慮擴(kuò)充后的部分。
*********************************************************************/
typedef STATE_STYLE& (CPredictModel::*pPREDICT_PROCESS_F) (STATE_STYLE& State);
typedef STATE_STYLE& (CObservModel::*pOBSERVE_PROCESS_F) (STATE_STYLE& State);
using namespace boost::numeric::ublas;
namespace BoostSpa=boost::numeric::ublas;
class CUncentedScheml:virtual public BOOST_DEBUG_TOOL
{
public:
	CUncentedScheml(void);
	/**
	*構(gòu)造函數(shù)中輸入的AugmentedSize指定擴(kuò)充維數(shù)，若它不為0則表示工作在擴(kuò)充后狀態(tài)，與sigma點(diǎn)有關(guān)的部分均需加上擴(kuò)充
	*StateSize為實(shí)際狀態(tài)維數(shù)，不考慮擴(kuò)充維
	*/
	CUncentedScheml(size_t StateSize,size_t AugmentedSize=0);
public:
	virtual~CUncentedScheml(void);
protected:

	/**
	*Cholesky分解，返回整形結(jié)果，0表示輸入矩陣半正定，<0表示負(fù)定
	*分解結(jié)果矩陣放在成員變量 m_mSCholResult
	*
	*/
	int SCholesky_f(matrix<double>& A );
	matrix<double>  m_mSCholResult;//，指定維數(shù)于構(gòu)造函數(shù)
	const size_t m_nStateSize;///<狀態(tài)變量維數(shù)
	/**
	*求取sigma點(diǎn)
	*結(jié)果sigma點(diǎn)矩陣放在成員變量 m_mSigmaPoint,并返回其引用
	*/
	SIGMA_POINTS& ut_sigmas(STATE_STYLE &StateMean,VARIANCE&StateCov,const double DisParm);
	SIGMA_POINTS m_mSigmaPoint;///<sigma點(diǎn)集，指定維數(shù)于構(gòu)造函數(shù)
	
	/**
	*求取sigma點(diǎn)的權(quán)值
	*結(jié)果sigma點(diǎn)的權(quán)值矩陣放在成員變量m_mSigmaWeight,并返回幅度系數(shù)c
	*/
	double ut_weights(int n, PARM alpha,PARM beta,PARM kappa);
	SIGMA_POINT_WEIGHT m_mSigmaWeight;///<sigma點(diǎn)權(quán)值矩陣,第一行為Wm,第二行為Wc,指定維數(shù)于構(gòu)造函數(shù)
	
public:
	/**
	*UT變換
	*結(jié)果包括sigma點(diǎn)矩陣，計算后均值、方差放在成員變量中,可以由外界輸入sigma點(diǎn)及權(quán)值 
	*/
	void ut_transform(STATE_STYLE &StateMean,VARIANCE&StateCov,
		CPredictModel&  PredictObj,double dl,double dr,
		PARM alpha,PARM beta,PARM kappa,SIGMA_POINTS& X,SIGMA_POINT_WEIGHT& w,
		double TimeStep=0);
	
	/**
	*UT變換
	*結(jié)果包括sigma點(diǎn)矩陣，計算后均值、方差放在成員變量中
	*無輸入sigma點(diǎn)矩陣，權(quán)值鎮(zhèn)，因此屬于預(yù)測調(diào)用
	*/
	void ut_transform(STATE_STYLE &StateMean,VARIANCE&StateCov,
		CPredictModel&  PredictObj,double dl,double dr,
		PARM alpha,PARM beta,PARM kappa,
		double TimeStep=0);
	///**
	//*UT變換
	//*結(jié)果包括sigma點(diǎn)矩陣，計算后均值、方差放在成員變量中
	//*無輸入sigma點(diǎn)矩陣，權(quán)值鎮(zhèn)，因此屬于預(yù)測調(diào)用,不輸入?yún)?shù),讓他們?nèi)ツJ(rèn)值
	//*/
	//void ut_transform(STATE_STYLE &StateMean,const matrix<double>&StateCov,
	//	pPROCESS_F Func,float dl,float dr,
	//	PARM alpha,PARM beta,PARM kappa);
	SIGMA_POINTS m_mSigmaPointsAfterProces;///<經(jīng)導(dǎo)入計算后的Sigma點(diǎn)集,每一列對應(yīng)一個sigma點(diǎn),他應(yīng)是4行13列，即未擴(kuò)狀態(tài)的維數(shù)n行，2*n+1列
public:
	STATE_STYLE m_mStateMeanResult;///<經(jīng)計算后的狀態(tài)向量期望(4維)注意其中的距離單位為毫米，而角度單位可能是弧度也可能是角度，由USE_DEGREE_ANGLE宏決定
	VARIANCE m_mStateCov;///<經(jīng)計算后的方差矩陣期望(4X4)


};