?? adaptiveoptimaser.cpp
字號:
//#include "StdAfx.h"
#include "AdaptiveOptimaser.h"
#ifndef USE_TRADITIONAL_AKF
CAdaptiveOptimaser::CAdaptiveOptimaser(void):m_iCovEstNum(4),m_mResidualCovStack(4),m_calResidualCoviarance(2,2)
,m_mTherioticalResCov(2,2),m_SumOfAllResCov(2,2),m_mDif_ResCov2TherResCov(2,2),m_mCovDifStack(3),m_mPreDeltaStep(2,2),
m_mAlterStepVal(2,2),m_mPreErDif(2,2),m_mNewModelNoise(4,4),m_mCurModeNoiseModStep(4,4)
//,m_dStableDifThres(2)
{
m_iNumOfInputResidual=0;
//m_bStableTag=false;
m_bStableTag[0]=m_bStableTag[1]=false;
for (int i=0;i<2;i++)
{
for (int j=0;j<2;j++)
{
m_SumOfAllResCov(i,j)=0;
m_mPreDeltaStep(i,j)=0.9;
m_mPreErDif(i,j)=1;
}
}
for (int i=0;i<4;i++)
{
for (int j=0;j<4;j++)
{
m_mCurModeNoiseModStep(i,j)=0;
}
}
//memset(m_mCurModeNoiseModStep.data(),0,m_mCurModeNoiseModStep.size1()*m_mCurModeNoiseModStep.size2());
m_iNumOfCovDif=0;
}
CAdaptiveOptimaser::CAdaptiveOptimaser(int CovEstNum,size_t ObserveSize,size_t StateSize):m_iCovEstNum(CovEstNum),m_mResidualCovStack(CovEstNum),
m_calResidualCoviarance(ObserveSize,ObserveSize),m_mTherioticalResCov(ObserveSize,ObserveSize),m_SumOfAllResCov(ObserveSize,ObserveSize),
m_mDif_ResCov2TherResCov(ObserveSize,ObserveSize),m_mCovDifStack(3),//,m_dStableDifThres(ObserveSize)
m_mPreDeltaStep(CovEstNum,CovEstNum),m_mAlterStepVal(CovEstNum,CovEstNum),m_mPreErDif(CovEstNum,CovEstNum),m_mNewModelNoise(StateSize,StateSize),
m_mCurModeNoiseModStep(StateSize,StateSize)
{
m_iNumOfInputResidual=0;
for (int i=0;i<ObserveSize;i++)
{
for (int j=0;j<ObserveSize;j++)
{
m_SumOfAllResCov(i,j)=0;
/* m_mCurModeNoiseModStep(i,j)=0;
m_mCurModeNoiseModStep(i+2,j+2)=0;*/
m_mPreDeltaStep(i,j)=0.9;
m_mPreErDif(i,j)=1;
}
}
for (int i=0;i<ObserveSize+2;i++)
{
for (int j=0;j<ObserveSize+2;j++)
{
m_mCurModeNoiseModStep(i,j)=0;
}
}
m_bStableTag[0]=m_bStableTag[1]=false;
m_iNumOfCovDif=0;
}
#else
CAdaptiveOptimaser::CAdaptiveOptimaser(void):m_iCovEstNum(4),m_mResidualCovStack(4),m_calResidualCoviarance(2,2)
,m_mTherioticalResCov(2,2),m_SumOfAllResCov(2,2),m_mCovDifStack(3),m_mNewModelNoise(4,4)
//,m_mAlterStepVal(2,2),m_mPreErDif(2,2),m_mCurModeNoiseModStep(4,4),m_mDif_ResCov2TherResCov(2,2),m_mPreDeltaStep(2,2)
//,m_dStableDifThres(2)
{
m_iNumOfInputResidual=0;
for (int i=0;i<2;i++)
{
for (int j=0;j<2;j++)
{
m_SumOfAllResCov(i,j)=0;
}
}
}
#endif
CAdaptiveOptimaser::~CAdaptiveOptimaser(void)
{
}
#ifdef USE_TRADITIONAL_AKF
VARIANCE& CAdaptiveOptimaser::ModelNoiseAdapter(RESIDUAL_STYLE Residual,VARIANCE& PredictVar,VARIANCE& StateVar,matrix_type& KalmanGain)
{
if(CalculateTheroticalCovOfResidual(Residual))//若計算得到理論殘差方差
{
//__asm int 3;
ConventionalAdapter( PredictVar,StateVar,m_mTherioticalResCov,KalmanGain);
}
else
{
//目前沒有什么做的
}
return m_mNewModelNoise;
}
#endif
#ifndef USE_TRADITIONAL_AKF
VARIANCE& CAdaptiveOptimaser::ModelNoiseAdapter(RESIDUAL_STYLE Residual, VARIANCE ResidualCoviarance)
{
//VARIANCE QStep();
range Line(0,2);
range Row(0,2);
range Line2(2,4);
range Row2(2,4);
#ifdef DEBUG_USE_FLAG
ViewMatrix(m_mNewModelNoise);
m_mSigma_Viewer;
#endif
if(CalculateTheroticalCovOfResidual(Residual,ResidualCoviarance))//若計算得到理論殘差方差
{
if(CalculateDifOfError())//若計算得到誤差的導數
{
// __asm int 3;
if ((!m_bStableTag[0])||(!m_bStableTag[1]))//更改為逐位判斷是否穩態，有一位不穩定則尋幽
{
for (unsigned i=0;i<m_mCurrentErDif.size1();i++)
{
if (!m_bStableTag[i])//不穩定
{
//for (int j=0;j<m_mCurrentErDif.size2();j++)//因m_mNewModelNoise為對角陣，故去掉此部分
//{
unsigned j=i;
m_mAlterStepVal(i,j)=1+RPPOP_DeltaStepSeek(m_mCurrentErDif(i,j),m_mPreErDif(i,j),m_mPreDeltaStep(i,j));//為提高效率將matlab程序中的1+步長值放這運算了。
}
#ifdef DEBUG_USE_FLAG
ViewMatrix(m_mAlterStepVal);
m_mSigma_Viewer;
#endif
//}
}
project(m_mCurModeNoiseModStep, Line, Row)=m_mAlterStepVal;//將結果重復組合成Q陣的變化陣
project(m_mCurModeNoiseModStep, Line2, Row2)=m_mAlterStepVal;
//#ifdef DEBUG_USE_FLAG
//
// ViewMatrix(m_mCurModeNoiseModStep);
// m_mSigma_Viewer;
//#endif
//
//#ifdef DEBUG_USE_FLAG
//
// ViewMatrix(m_mNewModelNoise);
// m_mSigma_Viewer;
//#endif
//
m_mNewModelNoise=element_prod(m_mCurModeNoiseModStep,m_mNewModelNoise);
LimitModifiedModelNoise(m_mNewModelNoise);
//#ifdef DEBUG_USE_FLAG
//
// ViewMatrix(m_mNewModelNoise);
// m_mSigma_Viewer;
//#endif
}
OptmiseInStableCheck(m_mAlterStepVal);//判斷尋優是否穩定，本次調整步長的變化值放在
}
}
else
{
//目前沒有什么做的
}
return m_mNewModelNoise;
}
bool CAdaptiveOptimaser::CalculateTheroticalCovOfResidual(RESIDUAL_STYLE& Residual,VARIANCE ResidualCov)
{
#ifdef DEBUG_USE_FLAG
ViewVector(Residual);
m_mSigma_Viewer;
#endif
if(m_iNumOfInputResidual>=m_iCovEstNum)//已經傳了足夠多的Residual供計算的話才開始尋優
{
m_calResidualCoviarance=outer_prod(Residual,Residual);//殘差計算方差Residual*Residua'boost中的向量是行向量所以用outer_prod
m_mResidualCovStack.insert(m_mResidualCovStack.begin(),m_calResidualCoviarance);//入一個放頭部，最新的放頭部
std::vector<VARIANCE>::iterator EndOfStack=m_mResidualCovStack.end();
//#ifdef DEBUG_USE_FLAG
//
// ViewMatrix(*(EndOfStack-1));
// m_mSigma_Viewer;
//#endif
#ifdef DEBUG_USE_FLAG
ViewMatrix(m_SumOfAllResCov);
m_mSigma_Viewer;
#endif
m_SumOfAllResCov=m_SumOfAllResCov-(*(--EndOfStack));//用上一次算得的和減去要擠出m_mResidualCovStack的末項,再加上新插入的項即得總和
m_SumOfAllResCov+=m_calResidualCoviarance;
m_mTherioticalResCov=(m_SumOfAllResCov)/m_iCovEstNum;
m_mResidualCovStack.pop_back();//刪除最后一個
/////////////////////////計算差值，并按位取絕對值/////////////////////////////////////////////////
m_mDif_ResCov2TherResCov=ResidualCov-m_mTherioticalResCov;//ls-TheroResidulCov
MatElemABS(m_mDif_ResCov2TherResCov);
#ifdef DEBUG_USE_FLAG
ViewMatrix(m_mDif_ResCov2TherResCov);
m_mSigma_Viewer;
#endif
////////////////////////////將計算出來的差值保存起來，并且總共只保存3個，因此要存一個，出一個,第一個最新的//////////////////////////////////////////////
m_mCovDifStack.insert(m_mCovDifStack.begin(),m_mDif_ResCov2TherResCov);
m_mCovDifStack.pop_back();//刪除最后一個h
if (m_iNumOfCovDif>2)
{
return true;
}
else
{
m_iNumOfCovDif++;
return false;
}
}
else
{
m_calResidualCoviarance=outer_prod(Residual,Residual);//殘差計算方差Residual*Residua'boost中的向量是行向量所以用outer_prod
m_iNumOfInputResidual++;
m_SumOfAllResCov+=m_calResidualCoviarance;
#ifdef DEBUG_USE_FLAG
ViewMatrix(m_SumOfAllResCov);
m_mSigma_Viewer;
#endif
m_mResidualCovStack.insert(m_mResidualCovStack.begin(),m_calResidualCoviarance);//入一個放頭部，最新的放頭部
m_mResidualCovStack.pop_back();//刪除最后一個
return false;
}
}
#endif
#ifdef USE_TRADITIONAL_AKF
bool CAdaptiveOptimaser::CalculateTheroticalCovOfResidual(RESIDUAL_STYLE& Residual)
{
#ifdef DEBUG_USE_FLAG
ViewVector(Residual);
m_mSigma_Viewer;
#endif
if(m_iNumOfInputResidual>=m_iCovEstNum)//已經傳了足夠多的Residual供計算的話才開始尋優
{
m_calResidualCoviarance=outer_prod(Residual,Residual);//殘差計算方差Residual*Residua'boost中的向量是行向量所以用outer_prod
m_mResidualCovStack.insert(m_mResidualCovStack.begin(),m_calResidualCoviarance);//入一個放頭部，最新的放頭部
std::vector<VARIANCE>::iterator EndOfStack=m_mResidualCovStack.end();
//#ifdef DEBUG_USE_FLAG
//
// ViewMatrix(*(EndOfStack-1));
// m_mSigma_Viewer;
//#endif
#ifdef DEBUG_USE_FLAG
ViewMatrix(m_SumOfAllResCov);
m_mSigma_Viewer;
#endif
m_SumOfAllResCov=m_SumOfAllResCov-(*(--EndOfStack));//用上一次算得的和減去要擠出m_mResidualCovStack的末項,再加上新插入的項即得總和
m_SumOfAllResCov+=m_calResidualCoviarance;
m_mTherioticalResCov=(m_SumOfAllResCov)/m_iCovEstNum;
m_mResidualCovStack.pop_back();//刪除最后一個
return true;
}
else
{
m_calResidualCoviarance=outer_prod(Residual,Residual);//殘差計算方差Residual*Residua'boost中的向量是行向量所以用outer_prod
m_iNumOfInputResidual++;
m_SumOfAllResCov+=m_calResidualCoviarance;
#ifdef DEBUG_USE_FLAG
ViewMatrix(m_SumOfAllResCov);
m_mSigma_Viewer;
#endif
m_mResidualCovStack.insert(m_mResidualCovStack.begin(),m_calResidualCoviarance);//入一個放頭部，最新的放頭部
m_mResidualCovStack.pop_back();//刪除最后一個
return false;
}
}
#endif
#ifndef USE_TRADITIONAL_AKF
bool CAdaptiveOptimaser::CalculateDifOfError()
{
std::vector<VARIANCE>::iterator HeadOfStack=m_mCovDifStack.begin();
// std::vector<VARIANCE>::iterator p2Stack;
//#ifdef DEBUG_USE_FLAG
//
// ViewMatrix(VARIANCE(*HeadOfStack));
// m_mSigma_Viewer;
//#endif
//#ifdef DEBUG_USE_FLAG
//
// ViewMatrix(VARIANCE(*(++HeadOfStack)));
// m_mSigma_Viewer;
//#endif
//p2Stack=HeadOfStack+1;
size_t Size=(*HeadOfStack).size1();
VARIANCE aTmp=(*HeadOfStack);
VARIANCE bTmp=(*(++HeadOfStack));
m_mCurrentErDif=aTmp-bTmp;//當前時刻的誤差函數的導數，用差值代替，而且由于RPPOD算法僅用到了導數的符號，所以沒有考慮時間長度
aTmp=*(++HeadOfStack);
for (int i=0;i<Size;i++)
{
for (int j=0;j<Size;j++)
{
if (m_mPreErDif(i,j)!=0)//防止因外部傳遞過來的0被忽略
{
m_mPreErDif(i,j)=bTmp(i,j)-aTmp(i,j);//前一時刻的誤差函數的導數，用差值代替，而且由于RPPOD算法僅用到了導數的符號，所以沒有考慮時間長度
}
}
}
return true;
}
bool CAdaptiveOptimaser::OptmiseInStableCheck(VARIANCE& CovAlterStep)
{
//double a=fabs(CovAlterStep(0,0)-1);//尋優步長的變化值即為最終步長值減去基準1
//double b=fabs(CovAlterStep(1,1)-1);//尋優步長的變化值即為最終步長值減去基準1
//double c=a+b;
for (unsigned iii=0;iii<2;iii++)
{
double a=fabs(CovAlterStep(iii,iii)-1);//尋優步長的變化值即為最終步長值減去基準1
if ((!m_bStableTag[iii])&&a<STABLE_THRES)// %進入穩定
{
m_dStableDifThres[iii]=m_fStableDifAmplitudeParm*m_mDif_ResCov2TherResCov(iii,iii);//%記錄方差偏差的當前值*m_fStableDifAmplitudeParm作為判斷是否跳出穩定的閾值
//m_dStableDifThres[0]=m_fStableDifAmplitudeParm*m_mDif_ResCov2TherResCov(0,0);//%記錄方差偏差的當前值*m_fStableDifAmplitudeParm作為判斷是否跳出穩定的閾值
//m_dStableDifThres[1]=m_fStableDifAmplitudeParm*m_mDif_ResCov2TherResCov(1,1);//%記錄方差偏差的當前值*m_fStableDifAmplitudeParm作為判斷是否跳出穩定的閾值
m_bStableTag[iii]=true;
}
else if (
m_bStableTag[iii] &&
(m_mDif_ResCov2TherResCov(iii,iii)>m_dStableDifThres[iii])
)// %退出穩定,則重設本優化器
{
m_bStableTag[iii]=false;
m_mNewModelNoise(iii,iii)=m_mNodelNoiseOriginalVal(iii,iii);//%重置初始值,并重開始一輪尋優
m_mNewModelNoise(iii+2,iii+2)=m_mNodelNoiseOriginalVal(iii+2,iii+2);//%重置初始值,并重開始一輪尋
m_mPreDeltaStep(iii,iii)*=0;
m_mPreErDif(iii,iii)*=0;
m_mPreErDif(iii,iii)=0.9;
m_mPreDeltaStep(iii,iii)=0.9;
//for (int i=0;i<m_mPreErDif.size1();i++)//%重置初始值,并重開始一輪尋優
//{
// m_mPreErDif(i,i)=0.9;
// m_mPreDeltaStep(i,i)=0.9;
//}
}
}
if (m_bStableTag[0]&&m_bStableTag[1])
{
return true ;//所有位都穩定則返回鎮
}
else
{
return false;
}
}
#endif
bool CAdaptiveOptimaser::initAdapter(VARIANCE& NodelNoiseOriginalVal,float StableDifAmplitudeParm)
{
m_mNodelNoiseOriginalVal=NodelNoiseOriginalVal;
m_fStableDifAmplitudeParm=StableDifAmplitudeParm;
m_mNewModelNoise=m_mNodelNoiseOriginalVal;
return true;
}
// 限制模型噪聲不太大也別太小,
//bool CAdaptiveOptimaser::LimitModNoiseInSafeVal(double&ModelNoise)
//{
//
//}
// 對尋優調整的結果（模型噪聲）進行限幅，不能太大也不能太小，輸入為尋優的結果矩陣的引用
//void CAdaptiveOptimaser::LimitModifiedModelNoise(VARIANCE& ModifyResult)
//{
//
//}
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -