uncentedscheml.cpp

InnovLabSimu在vc++下实现

//#include "StdAfx.h"
#include "UncentedScheml.h"
using namespace std;
CUncentedScheml::CUncentedScheml(void): m_nStateSize(4)
{
}
CUncentedScheml::CUncentedScheml(size_t StateSize,size_t AugmentedSize): m_nStateSize(StateSize),m_mSigmaWeight(2,2*(StateSize+AugmentedSize)+1),
m_mSCholResult((StateSize+AugmentedSize),(StateSize+AugmentedSize)),m_mSigmaPoint((StateSize+AugmentedSize),2*(StateSize+AugmentedSize)+1),
m_mSigmaPointsAfterProces(StateSize,2*(StateSize+AugmentedSize)+1),
m_mStateCov(StateSize,StateSize),m_mStateMeanResult(StateSize)
//分配空间
{
	
}
CUncentedScheml::~CUncentedScheml(void)
{
}


int CUncentedScheml::SCholesky_f(matrix<double>& A )
{
	//L  = zeros(size(A));
	//matrix<double> & L=(m_mSCholResult);
//#ifdef  DEBUG_USE_FLAG
//	ViewMatrix(A);
//
//	//m_mSigma_Viewer;
//#endif
	
	ClearMatrix(m_mSCholResult);

	int def = 1;
	for(int i=0;i<A.size1();i++)
	{
		for (int j=0;j<=i;j++)
		{

			double s = A(i,j);
	
			for (int k=0;k<j;k++)
			{
				s = s - m_mSCholResult(i,k)*m_mSCholResult(j,k);
			}
			if (j < i)
			{
				if (m_mSCholResult(j,j) > 0)
				{	m_mSCholResult(i,j) = s / m_mSCholResult(j,j);}
				else
				{m_mSCholResult(i,j) = 0;}
			}
			else
			{
				if (s < 0)
				{
					s = 0;
					def = -1;
				}
				else if (s < 0)
				{
					s = 0;
					def = min(0,def);
				}
				m_mSCholResult(j,j) = sqrt(s);
			}
/*#ifdef  DEBUG_USE_FLAG
			ViewMatrix(m_mSCholResult);

			//m_mSigma_Viewer;
#endif*/
		}
	}
//#ifdef  DEBUG_USE_FLAG
//	ViewMatrix(m_mSCholResult);
//	//m_mSigma_Viewer;
//#endif
	return def;
}

 SIGMA_POINTS& CUncentedScheml::ut_sigmas(STATE_STYLE& StateMean,VARIANCE&StateCov,const double DisParm)
{


	int IsPSD=SCholesky_f(StateCov);
	size_t StateSize= StateMean.size();
#ifdef  DEBUG_USE_FLAG
	ViewMatrix(StateCov);
	//m_mSigma_Viewer;
#endif
#ifdef  DEBUG_USE_FLAG
	ViewVector(StateMean);
	//m_mSigma_Viewer;
#endif
	//m_mSigmaPoint(StateSize,2*StateSize+1);

	if (IsPSD>=0)//至少半正定的话
	{
		ClearMatrix(m_mSigmaPoint);
		//m_mSigmaPoint = [zeros(size(M)) A -A];
		range LineRange(0,StateSize);//从第0行开始到StateSize-1行
		range RowRange1(1,StateSize+1);//从第1行开始到StateSize行
		range RowRange2(StateSize+1,2*StateSize+1);//从第1行开始到2*StateSize+1行
		//column(m_mSigmaPoint, 0) =StateMean;不能要	 
		project(m_mSigmaPoint, LineRange,RowRange1)=m_mSCholResult;
		project(m_mSigmaPoint, LineRange,RowRange2)=-m_mSCholResult;
#ifdef  DEBUG_USE_FLAG
		ViewMatrix(m_mSigmaPoint);
		//m_mSigma_Viewer;
#endif
		//构造同m_mSigmaPoint列数的均值矩阵
		matrix<double>MeanTmpMat(m_mSigmaPoint.size1(),m_mSigmaPoint.size2());
		for (int i=0;i<MeanTmpMat.size2();i++)
		{
			column(MeanTmpMat, i) =StateMean;
		}
		//计算Sigma点


		//matrix<double> ttmp(m_mSigmaPoint);
		//ttmp=sqrt(DisParm)*m_mSigmaPoint+ MeanTmpMat;

		m_mSigmaPoint = sqrt(DisParm)*m_mSigmaPoint + MeanTmpMat;
	}
	else
	{
		//MessageBox(NULL,_T("非正定矩阵"),NULL,MB_OK);
	}

	return m_mSigmaPoint;
}
 void CUncentedScheml::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)
 {
	
 }
 void CUncentedScheml::ut_transform(STATE_STYLE &StateMean, VARIANCE&StateCov,
	 CPredictModel& PredictObj,double dl,double dr,
	 PARM alpha,PARM beta,PARM kappa,
	 double TimeStep)//注意输入的状态变量是扩充维数后的
 {
	double Scale_c = ut_weights(StateMean.size(),alpha,beta,kappa);//计算所有sigma点权值以及尺度c！应该调用一次就够了！！！
//__asm int 3;
#ifdef  DEBUG_USE_FLAG
	ViewMatrix(StateCov);
	m_mSigma_Viewer;
#endif
	SIGMA_POINTS& X= ut_sigmas(StateMean,StateCov,Scale_c);//计算生成所有sigma点
	///////////////////////将sigma点导入函数pPROCESS_F进行计算///////////////////////////////////////////////////
	//w = {WM,WC,c};
	#ifdef  DEBUG_USE_FLAG
		ViewMatrix(m_mSigmaWeight);
		m_mSigma_Viewer;
	#endif
#ifdef  DEBUG_USE_FLAG
		ViewMatrix(X);
		m_mSigma_Viewer;
#endif
	//size_t RowOfX=X.size2();
	SIGMA_POINTS SigmaPointTmp(m_nStateSize,(X.size2()));//中间暂存矩阵,用于后面的计算
	STATE_STYLE StateTmp(m_nStateSize);//中间暂存向量,用于后面的计算
	//STATE_STYLE StateTmp2(m_nStateSize);//中间暂存向量,用于后面的计算
	ClearMatrix(m_mStateCov);
	//m_mStateCov.size2()
//	__asm int 3;
	for(int i=0;i<X.size2();i++)//依次取每一个sigma点导入
	{
		STATE_STYLE SigmaPoint=column(X,i);
#ifndef USE_DEGREE_ANGLE//若未定义标志，则用的角度单位为弧度
		column(m_mSigmaPointsAfterProces,i)=PredictObj.fx(SigmaPoint,dl,dr);//扩充后的导入,此时StateMean含6个元素
#else
	#ifdef CONSIDER_TIME_IN_VEL
		column(m_mSigmaPointsAfterProces,i)=PredictObj.fx_Degree2ConsiderTime(SigmaPoint,dl,dr,TimeStep);//扩充后的导入,此时StateMean含6个元素
	#else
		column(m_mSigmaPointsAfterProces,i)=PredictObj.fx_Degree(SigmaPoint,dl,dr);//扩充后的导入,此时StateMean含6个元素
	#endif
#ifdef  DEBUG_USE_FLAG
		ViewMatrix(m_mSigmaPointsAfterProces);
		m_mSigma_Viewer;
#endif
#endif
		column(SigmaPointTmp,i)=column(m_mSigmaPointsAfterProces,i)*m_mSigmaWeight(0,i);//m_mSigmaPointsAfterProces的第i列,乘Wm(i)
	}
#ifdef  DEBUG_USE_FLAG
	ViewMatrix(SigmaPointTmp);
	//m_mSigma_Viewer;
#endif
	//vector<double> MeanVecTmp(m_nStateSize);
	for (int i=0;i<m_nStateSize;i++)//计算均值
	{
		m_mStateMeanResult(i)=SumVector(BoostSpa::vector<double>(row(SigmaPointTmp,i)));//m_mSigmaPointsAfterProces的每行*第i列(第i个sigma点)对应的权值求和
		/*double tmp=norm_1(vector<double>(row(SigmaPointTmp,i)));
		double tmp0=m_mStateMeanResult(i);*/
		
	}
#ifdef  DEBUG_USE_FLAG
	ViewVector(m_mStateMeanResult);
	//m_mSigma_Viewer;
#endif
	///////////////////计算方差///////////////////////////////////////////////////////
	for (int i=0;i<X.size2();i++)
	{
		StateTmp=(column(m_mSigmaPointsAfterProces,i)-m_mStateMeanResult);//相当于是Xsigma(i)-x
		m_mStateCov+=m_mSigmaWeight(1,i)*outer_prod(StateTmp,StateTmp);//相当于是Wc(i)*(Xsigma(i)-x)*(Xsigma(i)-x)'
	}
#ifdef  DEBUG_USE_FLAG
	ViewMatrix(m_mStateCov);
	//m_mSigma_Viewer;
#endif
 }
 double CUncentedScheml::ut_weights(int n, PARM alpha,PARM beta,PARM kappa)
 {
	 double lambda = alpha*alpha * (n + kappa) - n;
	 //WM = zeros(2*n+1,1);
	 //WC = zeros(2*n+1,1);
	 double wm=0;
	 double wc=0;
	 for(int j=0;j<2*n+1;j=j+1) 
	 {
		 if (j==0)
		 {
			 wm = lambda / (n + lambda);
			 wc = lambda / (n + lambda) + (1 - alpha*alpha + beta);
		 }			
		 else
		 {
			 wm = 1 / (2 * (n + lambda));
			 wc = wm;
		 }
		 m_mSigmaWeight(0,j) = wm;//将权值wm存第一行
		 m_mSigmaWeight(1,j) = wc;//将权值wc存第二行
		 //j++;//注意不可去掉，也不能放到循环条件中去
	 }
	 return n + lambda;
 }