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

📄 singer.cpp

📁 VC++实现的kalman代码
💻 CPP
字号:
🔍 
// 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<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.3;
		A_Real[i][1]=-0.3;
	}
	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<349;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滤波
{

	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;
//	cout<<"X(2)"<<endl;
//	X.Display();

/*	CMatrix Fai1(6,6);//状态转移矩阵

	Fai1(1,1)=1;
	Fai1(2,2)=1;
	Fai1(3,3)=1;
	Fai1(1,2)=T;
	Fai1(1,3)=0.5*pow(T,2);
	Fai1(2,3)=T;
	Fai1(4,4)=1;
	Fai1(5,5)=1;
	Fai1(6,6)=1;
	Fai1(4,5)=T;
	Fai1(4,6)=0.5*pow(T,2);
	Fai1(5,6)=T;
	cout<<"Fai1"<<endl;
	Fai1.Display();
*/

	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;	
//	cout<<"Q"<<endl;
//	Q.Display();

	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);
	
//	cout<<"P(2)"<<endl;
//	P.Display();
		
	//头两个滤波值就是观测值
	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;
//		X.Display();
		P=Fai*P*~Fai+Q;
//		P.Display();
		K=P*~H*!(H*P*~H+R);
//		K.Display();
		X=X+K*(Z-H*X);
//		cout<<endl;

//		cout<<XY_Real[k][0]<<"  "<<XY_Real[k][1]<<endl;
//		cout<<XY_Obsv[k][0]<<"  "<<XY_Obsv[k][1]<<endl;
//		cout<<X(1,1)<<"  "<<X(4,1)<<endl;;
		P=(I-K*H)*P;
//		P.Display();
		XY_Filt[k][0]=X(1,1);
		XY_Filt[k][1]=X(4,1);

		
	}

}


void CSinger::Filter_2()//kalman_Singer算法滤波(第二种方法)
{


	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;
//	cout<<"X(2)"<<endl;
//	X.Display();

	CMatrix Fai1(6,6);//状态转移矩阵

	Fai1(1,1)=1;
	Fai1(2,2)=1;
	Fai1(3,3)=1;
	Fai1(1,2)=T;
	Fai1(1,3)=0.5*pow(T,2);
	Fai1(2,3)=T;
	Fai1(4,4)=1;
	Fai1(5,5)=1;
	Fai1(6,6)=1;
	Fai1(4,5)=T;
	Fai1(4,6)=0.5*pow(T,2);
	Fai1(5,6)=T;
//	cout<<"Fai1"<<endl;
//	Fai1.Display();

	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*pow(cova,2)*pow(T,5)/20;
	Q(1,2)=2*alfa*pow(cova,2)*pow(T,4)/8;
	Q(1,3)=2*alfa*pow(cova,2)*pow(T,3)/6;
	Q(2,1)=2*alfa*pow(cova,2)*pow(T,4)/8;
	Q(2,2)=2*alfa*pow(cova,2)*pow(T,3)/3;
	Q(2,3)=2*alfa*pow(cova,2)*pow(T,2)/2;
	Q(3,1)=2*alfa*pow(cova,2)*pow(T,3)/6;
	Q(3,2)=2*alfa*pow(cova,2)*pow(T,2)/2;
	Q(3,3)=2*alfa*pow(cova,2)*T;

	Q(4,4)=2*alfa*pow(cova,2)*pow(T,5)/20;
	Q(4,5)=2*alfa*pow(cova,2)*pow(T,4)/8;
	Q(4,6)=2*alfa*pow(cova,2)*pow(T,3)/6;
	Q(5,4)=2*alfa*pow(cova,2)*pow(T,4)/8;
	Q(5,5)=2*alfa*pow(cova,2)*pow(T,3)/3;
	Q(5,6)=2*alfa*pow(cova,2)*pow(T,2)/2;
	Q(6,4)=2*alfa*pow(cova,2)*pow(T,3)/6;
	Q(6,5)=2*alfa*pow(cova,2)*pow(T,2)/2;
	Q(6,6)=2*alfa*pow(cova,2)*T;	
//	cout<<"Q"<<endl;
//	Q.Display();

	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);
	
//	cout<<"P(2)"<<endl;
//	P.Display();
		
	//头两个滤波值就是观测值
	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=Fai1*X;
//		X.Display();
		P=Fai*P*~Fai+Q;
//		P.Display();
		K=P*~H*!(H*P*~H+R);
//		K.Display();
		X=X+K*(Z-H*X);
//		cout<<endl;

//		cout<<XY_Real[k][0]<<"  "<<XY_Real[k][1]<<endl;
//		cout<<XY_Obsv[k][0]<<"  "<<XY_Obsv[k][1]<<endl;
//		cout<<X(1,1)<<"  "<<X(4,1)<<endl;;
		P=(I-K*H)*P;
//		P.Display();
		XY_Filt[k][0]=X(1,1);
		XY_Filt[k][1]=X(4,1);

	}
}


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



	int i,j;
		
	for(i=0;i<350;i++)
	{
		ex[i]=0;
		ey[i]=0;
		dx[i]=0;
		dy[i]=0;
	}
	GenerateRealTrack();


	for(i=0;i<M;i++)
	{
		
		if(kind==0)
			Filter();
		else
			Filter_2();
		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);
		}
	}
	for(j=0;j<350;j++)
		{
			ex[j]=ex[j]/M;
			ey[j]=ey[j]/M;
			dx[j]=sqrt(dx[j]/M-ex[j]);
			dy[j]=sqrt(dy[j]/M-ey[j]);
		}
	for(j=0;j<350;j++)
		{
		cout<<ex[j]<<"   "<<ey[j]<<endl;
		cout<<dx[j]<<"   "<<dy[j]<<endl;
	}
}


void CSinger::Filter_LMS()//最小二乘递推估计
{
	
	AddNoise();

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

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


	CMatrix H(2,4);//状态->观测
	CMatrix H2(2,4);//状态->观测初始值
	H2(1,1)=1;
	H2(1,2)=T;
	H2(2,3)=1;
	H2(2,4)=T;






//	H(1,1)=1;
//	H(1,2)=T;
//	H(2,3)=1;
//	H(2,4)=T;


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

	CMatrix R(2,2);//观测噪声协方差阵
	R(1,1)=cov*cov;
	R(2,2)=cov*cov;


	CMatrix K(4,2);//增益

	CMatrix P(4,4);//预测误差,滤波协方差阵
	P=!(~H2*!R*H2);

	CMatrix X(2,1);

		
	//头两个滤波值就是观测值
	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];
		}

		H(1,1)=1;
		H(1,2)=k*T;
		H(2,3)=1;
		H(2,4)=k*T;
		
		K=P*~H*!(H*P*~H+R);
		X0=X0+K*(Z-H*X0);
		P=(I-K*H)*P;
		X=H*X0;

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

		
	}
}
void CSinger::CalErrorLms(int M)//计算lms滤波器误差的均值、标准差
{

	
	int i,j;
		
	for(i=0;i<350;i++)
	{
		ex[i]=0;
		ey[i]=0;
		dx[i]=0;
		dy[i]=0;
	}
	GenerateRealTrack();


	for(i=0;i<M;i++)
	{
		
		
		Filter_LMS();
	
		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);
		}
	}
	for(j=0;j<350;j++)
		{
			ex[j]=ex[j]/M;
			ey[j]=ey[j]/M;
			dx[j]=sqrt(dx[j]/M-ex[j]);
			dy[j]=sqrt(dy[j]/M-ey[j]);
		}
	for(j=0;j<350;j++)
		{
		cout<<ex[j]<<"   "<<ey[j]<<endl;
		cout<<dx[j]<<"   "<<dy[j]<<endl;
		}

}

⌨️ 快捷键说明

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