?? uncentedscheml.cpp
字號:
//#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
//構(gòu)造同m_mSigmaPoint列數(shù)的均值矩陣
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)//注意輸入的狀態(tài)變量是擴充維數(shù)后的
{
double Scale_c = ut_weights(StateMean.size(),alpha,beta,kappa);//計算所有sigma點權(quán)值以及尺度c！應(yīng)該調(diào)用一次就夠了！！！
//__asm int 3;
#ifdef DEBUG_USE_FLAG
ViewMatrix(StateCov);
m_mSigma_Viewer;
#endif
SIGMA_POINTS& X= ut_sigmas(StateMean,StateCov,Scale_c);//計算生成所有sigma點
///////////////////////將sigma點導(dǎo)入函數(shù)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點導(dǎo)入
{
STATE_STYLE SigmaPoint=column(X,i);
#ifndef USE_DEGREE_ANGLE//若未定義標志，則用的角度單位為弧度
column(m_mSigmaPointsAfterProces,i)=PredictObj.fx(SigmaPoint,dl,dr);//擴充后的導(dǎo)入,此時StateMean含6個元素
#else
#ifdef CONSIDER_TIME_IN_VEL
column(m_mSigmaPointsAfterProces,i)=PredictObj.fx_Degree2ConsiderTime(SigmaPoint,dl,dr,TimeStep);//擴充后的導(dǎo)入,此時StateMean含6個元素
#else
column(m_mSigmaPointsAfterProces,i)=PredictObj.fx_Degree(SigmaPoint,dl,dr);//擴充后的導(dǎo)入,此時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點)對應(yīng)的權(quán)值求和
/*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);//相當(dāng)于是Xsigma(i)-x
m_mStateCov+=m_mSigmaWeight(1,i)*outer_prod(StateTmp,StateTmp);//相當(dāng)于是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;//將權(quán)值wm存第一行
m_mSigmaWeight(1,j) = wc;//將權(quán)值wc存第二行
//j++;//注意不可去掉，也不能放到循環(huán)條件中去
}
return n + lambda;
}
?? 快捷鍵說明
復(fù)制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -