?? ekf.cpp
字號(hào):
// EKF.cpp: implementation of the EKF class.
//
//////////////////////////////////////////////////////////////////////
//#include "stdafx.h"
//#include "MultiSensorFusion.h"
#include "EKF.h"
#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif
//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
EKF::EKF()
{
PredictState.Init(6,1);
UpdateState.Init(6,1);
CurrentState.Init(6,1);
PredictCovariance.Init(6,6);
UpdateCovariance.Init(6,6);
CurrentCovariance.Init(6,6);
LastUpdateState.Init(6,1);
LastUpdateCov.Init(6,6);
F.Init(6,6);
G.Init(6,2);
Q.Init(2,2);
Z.Init(2,1);
H.Init(2,6);
R.Init(2,2);
S.Init(6,6);
Gain.Init(6,6);
}
EKF::EKF(EKF &ekf)
{
PredictState=ekf.PredictState;
UpdateState=ekf.UpdateCovariance;
CurrentState=ekf.CurrentState;
PredictCovariance=ekf.PredictCovariance;
UpdateCovariance=ekf.UpdateCovariance;
CurrentCovariance=ekf.CurrentCovariance;
F=ekf.F;
G=ekf.G;
Q=ekf.Q;
Z=ekf.Z;
H=ekf.H;
R=ekf.R;
S=ekf.S;
Gain=ekf.Gain;
}
EKF EKF::operator =(EKF ekf)
{
PredictState=ekf.PredictState;
UpdateState=ekf.UpdateCovariance;
CurrentState=ekf.CurrentState;
PredictCovariance=ekf.PredictCovariance;
UpdateCovariance=ekf.UpdateCovariance;
CurrentCovariance=ekf.CurrentCovariance;
F=ekf.F;
G=ekf.G;
Q=ekf.Q;
Z=ekf.Z;
H=ekf.H;
R=ekf.R;
S=ekf.S;
Gain=ekf.Gain;
return *this;
}
EKF::~EKF()
{
PredictState.UnInit();
UpdateState.UnInit();
CurrentState.UnInit();
PredictCovariance.UnInit();
UpdateCovariance.UnInit();
CurrentCovariance.UnInit();
LastUpdateState.UnInit();
LastUpdateCov.UnInit();
F.UnInit();
G.UnInit();
Q.UnInit();
Z.UnInit();
H.UnInit();
R.UnInit();
S.UnInit();
Gain.UnInit();
}
void EKF::SetProcessNoiseCov(double X_Acc_Cov,double Y_Acc_Cov)
{
Q.Clear();
Q.SetElement(0, 0, X_Acc_Cov);
Q.SetElement(1, 1, Y_Acc_Cov);
}
void EKF::SetObservationNoiseCov(double RadiusCov, double AzimuthCov)
{
R.Clear();
R.SetElement(0,0,RadiusCov);
R.SetElement(1,1,AzimuthCov*(3.1415926536*3.1415926536)/(180.0*180.0));
}
void EKF::Initialize(double Radius1,double Azimuth1,
double Radius2,double Azimuth2,
double Radius3,double Azimuth3,
double T1,double T2)
{
double x1,x2,x3,y1,y2,y3;
x1=Radius1*cos(Azimuth1*3.14159265359/180.0);
x2=Radius2*cos(Azimuth2*3.14159265359/180.0);
x3=Radius3*cos(Azimuth3*3.14159265359/180.0);
y1=Radius1*sin(Azimuth1*3.14159265359/180.0);
y2=Radius2*sin(Azimuth2*3.14159265359/180.0);
y3=Radius3*sin(Azimuth3*3.14159265359/180.0);
int ColNum, RowNum;
ColNum = CurrentState.GetNumColumns();
RowNum = CurrentState.GetNumRows();
double *StateData = new double[ColNum*RowNum];
StateData[0]=x3;
StateData[1]=x3*(T1+2*T2)/(T2*(T1+T2))-x2*(T1+T2)/(T1*T2)+x1*T2/(T1*(T1+T2));
StateData[2]=2.0*x3/(T2*(T1+T2))-2.0*x2/(T1*T2)+2.0*x1/(T1*(T1+T2));
StateData[3]=y3;
StateData[4]=y3*(T1+2*T2)/(T2*(T1+T2))-y2*(T1+T2)/(T1*T2)+y1*T2/(T1*(T1+T2));
StateData[5]=2.0*y3/(T2*(T1+T2))-2.0*y2/(T1*T2)+2.0*y1/(T1*(T1+T2));
CurrentState.SetData(StateData); // 要注意是否有問題
PredictState=CurrentState;
UpdateState=CurrentState;
ColNum = CurrentCovariance.GetNumColumns();
RowNum = CurrentCovariance.GetNumRows();
double *B_Element=new double [ColNum*RowNum];
B_Element[0]=0.0;
B_Element[1]=0.0;
B_Element[2]=0.0;
B_Element[3]=0.0;
B_Element[4]=1.0;
B_Element[5]=0.0;
B_Element[6]=1.0*T2/(T1*(T1+T2));
B_Element[7]=0.0;
B_Element[8]=0-1.0*(T1+T2)/(T1*T2);
B_Element[9]=0.0;
B_Element[10]=1.0*(T1+2*T2)/(T2*(T1+T2));
B_Element[11]=0.0;
B_Element[12]=2.0/(T1*(T1+T2));
B_Element[13]=0.0;
B_Element[14]=0-2.0/(T1*T2);
B_Element[15]=0.0;
B_Element[16]=2.0/(T2*(T1+T2));
B_Element[17]=0.0;
B_Element[18]=0.0;
B_Element[19]=0.0;
B_Element[20]=0.0;
B_Element[21]=0.0;
B_Element[22]=0.0;
B_Element[23]=1.0;
B_Element[24]=0.0;
B_Element[25]=1.0*T2/(T1*(T1+T2));
B_Element[26]=0.0;
B_Element[27]=0-1.0*(T1+T2)/(T1*T2);
B_Element[28]=0.0;
B_Element[29]=1.0*(T1+2*T2)/(T2*(T1+T2));
B_Element[30]=0.0;
B_Element[31]=2.0/(T1*(T1+T2));
B_Element[32]=0.0;
B_Element[33]=0-2.0/(T1*T2);
B_Element[34]=0.0;
B_Element[35]=2.0/(T2*(T1+T2));
CMatrix B(RowNum, ColNum, B_Element);
// R_Expansion(6*6) ----- 三點(diǎn)啟動(dòng)
CMatrix R_Expansion(RowNum, ColNum);
/* ColNum = R->GetNumColumns();*/
int RowNum_R = R.GetNumRows();
int i,j;
for(i=0;i<RowNum;i++)
{
j=i%RowNum_R;
R_Expansion.SetElement(i,i,R.GetElement(j,j));
}
CurrentCovariance= B*R_Expansion*(B.Transpose());
PredictCovariance=CurrentCovariance;
UpdateCovariance=CurrentCovariance;
}
void EKF::Predict(double T)//T means the Sample time interval
{
F.Clear();
F.SetElement(0,0,1.0);
F.SetElement(0,1,T);
F.SetElement(0,2,0.5*T*T);
F.SetElement(1,1,1.0);
F.SetElement(1,2,T);
F.SetElement(2,2,1.0);
F.SetElement(3,3,1.0);
F.SetElement(3,4,T);
F.SetElement(3,5,0.5*T*T);
F.SetElement(4,4,1.0);
F.SetElement(4,5,T);
F.SetElement(5,5,1.0);
G.Clear();
G.SetElement(0,0,0.5*T*T);
G.SetElement(1,0,T);
G.SetElement(2,0,1.0);
G.SetElement(3,1,0.5*T*T);
G.SetElement(4,1,T);
G.SetElement(5,1,1);
PredictState=F*UpdateState;
PredictCovariance=F*UpdateCovariance*(F.Transpose())+G*Q*(G.Transpose());
CurrentState=PredictState;
CurrentCovariance=PredictCovariance;
}
void EKF::Update(double Radius,double Azimuth)
{
H.Clear();
H.SetElement(0,0,1.0);
H.SetElement(1,3,1.0);
S=H*PredictCovariance*(H.Transpose())+R;
S.InvertGaussJordan();
Gain=PredictCovariance*(H.Transpose())*S;
Z.Clear();
double Zx=Radius*cos(Azimuth*3.1415926536/180.0);
double Zy=Radius*sin(Azimuth*3.1415926536/180.0);
Z.SetElement(0,0,Zx);
Z.SetElement(1,0,Zy);
CMatrix Z0(2,1);
Z0.SetElement(0,0,PredictState.GetElement(0,0));
Z0.SetElement(1,0,PredictState.GetElement(3,0));
// TRACE0(CurrentState.ToString("\t\t"));
UpdateState=PredictState+Gain*(Z-Z0);
UpdateCovariance=PredictCovariance-Gain*S*(Gain.Transpose());
CurrentState=UpdateState;
CurrentCovariance=UpdateCovariance;
// TRACE0(CurrentState.ToString("\t\t"));
LastUpdateState=UpdateState;
LastUpdateCov=UpdateCovariance;
}
CMatrix EKF::GetState()
{
return CurrentState;
}
CMatrix EKF::GetUpdateState()
{
return UpdateState;
}
CMatrix EKF::GetCovariance()
{
return CurrentCovariance;
}
?? 快捷鍵說明
復(fù)制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號(hào)
Ctrl + =
減小字號(hào)
Ctrl + -