uncentedscheml.h

InnovLabSimu在vc++下实现

#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接口函数，目前仅在预测阶段用到了本方法，
				所以仅ut_transform接口函数仅有输入预测模型的版本，若需用于更新函数，则必须加一个重载函数。
				另
				  可以用于扩展后的UKF，需在构造函数中指定扩充维数，但存放结果的m_mStateMeanResult，和m_mStateCov
				都是实际状态变量的期望与方差阵，并不考虑扩充后的部分。
*********************************************************************/
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);
	/**
	*构造函数中输入的AugmentedSize指定扩充维数，若它不为0则表示工作在扩充后状态，与sigma点有关的部分均需加上扩充
	*StateSize为实际状态维数，不考虑扩充维
	*/
	CUncentedScheml(size_t StateSize,size_t AugmentedSize=0);
public:
	virtual~CUncentedScheml(void);
protected:

	/**
	*Cholesky分解，返回整形结果，0表示输入矩阵半正定，<0表示负定
	*分解结果矩阵放在成员变量 m_mSCholResult
	*
	*/
	int SCholesky_f(matrix<double>& A );
	matrix<double>  m_mSCholResult;//，指定维数于构造函数
	const size_t m_nStateSize;///<状态变量维数
	/**
	*求取sigma点
	*结果sigma点矩阵放在成员变量 m_mSigmaPoint,并返回其引用
	*/
	SIGMA_POINTS& ut_sigmas(STATE_STYLE &StateMean,VARIANCE&StateCov,const double DisParm);
	SIGMA_POINTS m_mSigmaPoint;///<sigma点集，指定维数于构造函数
	
	/**
	*求取sigma点的权值
	*结果sigma点的权值矩阵放在成员变量m_mSigmaWeight,并返回幅度系数c
	*/
	double ut_weights(int n, PARM alpha,PARM beta,PARM kappa);
	SIGMA_POINT_WEIGHT m_mSigmaWeight;///<sigma点权值矩阵,第一行为Wm,第二行为Wc,指定维数于构造函数
	
public:
	/**
	*UT变换
	*结果包括sigma点矩阵，计算后均值、方差放在成员变量中,可以由外界输入sigma点及权值 
	*/
	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变换
	*结果包括sigma点矩阵，计算后均值、方差放在成员变量中
	*无输入sigma点矩阵，权值镇，因此属于预测调用
	*/
	void ut_transform(STATE_STYLE &StateMean,VARIANCE&StateCov,
		CPredictModel&  PredictObj,double dl,double dr,
		PARM alpha,PARM beta,PARM kappa,
		double TimeStep=0);
	///**
	//*UT变换
	//*结果包括sigma点矩阵，计算后均值、方差放在成员变量中
	//*无输入sigma点矩阵，权值镇，因此属于预测调用,不输入参数,让他们去默认值
	//*/
	//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;///<经导入计算后的Sigma点集,每一列对应一个sigma点,他应是4行13列，即未扩状态的维数n行，2*n+1列
public:
	STATE_STYLE m_mStateMeanResult;///<经计算后的状态向量期望(4维)注意其中的距离单位为毫米，而角度单位可能是弧度也可能是角度，由USE_DEGREE_ANGLE宏决定
	VARIANCE m_mStateCov;///<经计算后的方差矩阵期望(4X4)


};