singer.cpp

执行对一个 L 航迹进行Kalman滤波

// Singer.cpp: implementation of the CSinger class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "Singer.h"
#include "stdlib.h"
#include "math.h"
#include "iostream.h"
#include "Matrix.h"


//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

CSinger::CSinger()
{

	srand((unsigned)time( NULL ) );
	int i;
	cov=100;
	T=2;
	alfa=1.0/60;
	cova=0.03;
	
	
	for(i=0;i<350;i++)
	{
		A_Real[i][0]=0;
		A_Real[i][1]=0;
		V_Real[i][0]=0;
		V_Real[i][1]=0;
		XY_Real[i][0]=0;
		XY_Real[i][1]=0;
	
		XY_Filt[i][0]=0;
		XY_Filt[i][1]=0;
	
	}
	//GenerateRealTrack();

}

CSinger::~CSinger()
{

}

void CSinger::AddNoise()//产生正态白噪声
{

	double r1,r2;
	double noise;

	int i;
	for(i=0;i<350;i++)
	{
		r1=(double)rand()/RAND_MAX;
		r2=(double)rand()/RAND_MAX;
		noise=cov*sqrt(-2*log(r1))*cos(2*PIE*r2);
		XY_Obsv[i][0]=XY_Real[i][0]+noise;
	}
//	srand((unsigned)time( NULL ));
	for(i=0;i<350;i++)
	{
		r1=(double)rand()/RAND_MAX;
		r2=(double)rand()/RAND_MAX;
		noise=cov*sqrt(-2*log(r1))*cos(2*PIE*r2);
		XY_Obsv[i][1]=XY_Real[i][1]+noise;
	}


}


void CSinger::GenerateRealTrack()//产生真实的轨迹
{

	int i;
	for(i=0;i<350;i++)
	{
		A_Real[i][0]=0;
		A_Real[i][1]=0;
		V_Real[i][0]=0;
		V_Real[i][1]=0;
		XY_Real[i][0]=0;
		XY_Real[i][1]=0;
	
		XY_Filt[i][0]=0;
		XY_Filt[i][1]=0;
	
	}
	//加速度的初始值
	for(i=0;i<200;i++)
	{
		A_Real[i][0]=0;
		A_Real[i][1]=0;
	}
	for(i=200;i<300;i++)
	{
		A_Real[i][0]=0.075;
		A_Real[i][1]=0.075;
	}
	for(i=300;i<305;i++)
	{
		A_Real[i][0]=0;
		A_Real[i][1]=0;
	}
	for(i=305;i<330;i++)
	{
		A_Real[i][0]=0;
		A_Real[i][1]=0;
	}
	for(i=330;i<350;i++)
	{
		A_Real[i][0]=0;
		A_Real[i][1]=0;
	}
	
	//速度的初始值
	for(i=0;i<200;i++)
	{
		V_Real[i][0]=0;
		V_Real[i][1]=-15;
	}
	//位置的初始值
	XY_Real[0][0]=2000;
	XY_Real[0][1]=10000;
	
	//计算速度
	for(i=0;i<349;i++)
		
	{
		V_Real[i+1][0]=V_Real[i][0]+T*A_Real[i][0];
		V_Real[i+1][1]=V_Real[i][1]+T*A_Real[i][1];
	}

	//计算位置坐标
	for(i=0;i<350;i++)
		
	{
		XY_Real[i+1][0]=XY_Real[i][0]+T*V_Real[i][0]+0.5*pow(T,2)*A_Real[i][0];
		XY_Real[i+1][1]=XY_Real[i][1]+T*V_Real[i][1]+0.5*pow(T,2)*A_Real[i][1];
	}


}

void CSinger::Filter()//kalman滤波
{
    GenerateRealTrack();
	AddNoise();

	int k=3;
	CMatrix I(6,6);//单位矩阵
	I(1,1)=1;
	I(2,2)=1;
	I(3,3)=1;
	I(4,4)=1;
	I(5,5)=1;
	I(6,6)=1;

	CMatrix X(6,1);//状态矩阵 
	X(1,1)=XY_Obsv[1][0];
	X(2,1)=(XY_Obsv[1][0]-XY_Obsv[0][0])/T;
	X(4,1)=XY_Obsv[1][1];
	X(5,1)=(XY_Obsv[1][1]-XY_Obsv[0][1])/T;

	CMatrix Fai(6,6);//状态转移矩阵
	Fai(1,1)=1;
	Fai(2,2)=1;
	Fai(3,3)=exp(-alfa*T);
	Fai(1,2)=T;
	Fai(1,3)=1/pow(alfa,2)*(-1+alfa*T+exp(-alfa*T));
	Fai(2,3)=1/alfa*(1-exp(-alfa*T));
	Fai(4,4)=1;
	Fai(5,5)=1;
	Fai(6,6)=exp(-alfa*T);
	Fai(4,5)=T;
	Fai(4,6)=1/pow(alfa,2)*(-1+alfa*T+exp(-alfa*T));
	Fai(5,6)=1/alfa*(1-exp(-alfa*T));
//	cout<<"Fai"<<endl;
//	Fai.Display();

	CMatrix H(2,6);//状态->观测

	H(1,1)=1;
	H(2,4)=1;
//	cout<<"H"<<endl;
//	H.Display();


	CMatrix Z(2,1);//观测值

	CMatrix R(2,2);//观测噪声协方差阵
	R(1,1)=pow(cov,2);
	R(2,2)=pow(cov,2);
	
	CMatrix Q(6,6);//扰动噪声协方差阵
	
	Q(1,1)=2*alfa*cova*cova*pow(T,5)/20;
	Q(1,2)=2*alfa*cova*cova*pow(T,4)/8;
	Q(1,3)=2*alfa*cova*cova*pow(T,3)/6;
	Q(2,1)=2*alfa*cova*cova*pow(T,4)/8;
	Q(2,2)=2*alfa*cova*cova*pow(T,3)/3;
	Q(2,3)=2*alfa*cova*cova*pow(T,2)/2;
	Q(3,1)=2*alfa*cova*cova*pow(T,3)/6;
	Q(3,2)=2*alfa*cova*cova*pow(T,2)/2;
	Q(3,3)=2*alfa*cova*cova*T;

	Q(4,4)=2*alfa*cova*cova*pow(T,5)/20;
	Q(4,5)=2*alfa*cova*cova*pow(T,4)/8;
	Q(4,6)=2*alfa*cova*cova*pow(T,3)/6;
	Q(5,4)=2*alfa*cova*cova*pow(T,4)/8;
	Q(5,5)=2*alfa*cova*cova*pow(T,3)/3;
	Q(5,6)=2*alfa*cova*cova*pow(T,2)/2;
	Q(6,4)=2*alfa*cova*cova*pow(T,3)/6;
	Q(6,5)=2*alfa*cova*cova*pow(T,2)/2;
	Q(6,6)=2*alfa*cova*cova*T;	

	CMatrix K(6,2);//kalman增益

	CMatrix P(6,6);//预测误差,滤波协方差阵
	P(1,1)=pow(cov,2);
	P(1,2)=pow(cov,2)/T;
	P(2,1)=P(1,2);
	P(2,2)=2*pow(cov,2)/pow(T,2);
	P(4,4)=pow(cov,2);
	P(4,5)=pow(cov,2)/T;
	P(5,4)=P(4,5);
	P(5,5)=2*pow(cov,2)/pow(T,2);
	

	//头两个滤波值就是观测值
	XY_Filt[0][0]=XY_Obsv[0][0];
	XY_Filt[0][1]=XY_Obsv[0][1];
	XY_Filt[1][0]=XY_Obsv[1][0];
	XY_Filt[1][1]=XY_Obsv[1][1];

	for(k=2;k<350;k++)
	{
		if((XY_Obsv[k][0]-XY_Obsv[k-1][0])/T>MAX_SPEED||(XY_Obsv[k][1]-XY_Obsv[k-1][1])/T>MAX_SPEED)
		{
			Z(1,1)=XY_Obsv[k-1][0];
			Z(2,1)=XY_Obsv[k-1][1];
	//		XY_Obsv[k][0]=XY_Obsv[k-1][0];
	//		XY_Obsv[k][1]=XY_Obsv[k-1][1];
			
		}
		else
		{
			Z(1,1)=XY_Obsv[k][0];
			Z(2,1)=XY_Obsv[k][1];
		}
		X=Fai*X;

		P=Fai*P*~Fai+Q;

		K=P*~H*!(H*P*~H+R);

		X=X+K*(Z-H*X);

		P=(I-K*H)*P;

		XY_Filt[k][0]=X(1,1);
		XY_Filt[k][1]=X(4,1);

		
	}

}



void CSinger::CalError()//计算滤波器误差的均值、标准差
{



	int i,j;
		
	for(i=0;i<350;i++)
	{
		ex[i]=0;
		ey[i]=0;
		dx[i]=0;
		dy[i]=0;
	}
		
	for(j=0;j<350;j++)
	{
		ex[j]+=XY_Real[j][0]-XY_Filt[j][0];
		ey[j]+=XY_Real[j][1]-XY_Filt[j][1];
		dx[j]+=pow(XY_Real[j][0]-XY_Filt[j][0],2);
		dy[j]+=pow(XY_Real[j][1]-XY_Filt[j][1],2);
	}
	exx=eyy=dxx=dyy=0;
	for(j=0;j<350;j++)
	{
		exx+=ex[j];
		eyy+=ey[j];
		dxx+=dx[j];
		dyy+=dy[j];
	}
	exx=fabs(exx/350);
	eyy=fabs(eyy/350);
	dxx=dxx/350;
	dyy=dyy/350;
	
}