⭐ 欢迎来到虫虫下载站！ | 📦 资源下载 📁 资源专辑 ℹ️ 关于我们
⭐ 虫虫下载站
📤 上传资源

📄 ekf.cpp

📁 Kalman滤波器的C++实现
💻 CPP
字号:
🔍 
// 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) ----- 三点启动
	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;
}

⌨️ 快捷键说明

复制代码 Ctrl + C
搜索代码 Ctrl + F
全屏模式 F11
切换主题 Ctrl + Shift + D
显示快捷键 ?
增大字号 Ctrl + =
减小字号 Ctrl + -