kalmandoc.cpp

基于VC环境下的组合导航卡尔曼滤波仿真器设计

	GQGt=G*Qt*G.t();
	matrix Qk(10,10);//系统噪声方差阵，即гQгt
	Qk=0.0;
	matrix Rk(unit(2));//观测噪声方差阵,先设为单位阵
	Rk*=0.02*0.02/(60*57.3*60*57.3);//观测噪声为0.02，单位：′
	matrix H(2,10);//观测矩阵
	H=0.0;
	H.set(0,0,1.0);H.set(1,1,1.0);
	//P0初始值设定
	matrix p(unit(10));//初始估计误差方差阵P0
	p.set(0,0,1.0*1.0/(60*57.3*60*57.3));
	p.set(1,1,1.0*1.0/(60*57.3*60*57.3));//初始经纬度误差，单位：弧度
	p.set(2,2,1.0*1.0);
	p.set(3,3,1.0*1.0);//初始速度误差，单位：m/s
	p.set(4,4,1.0*1.0/(60*57.3*60*57.3));
	p.set(5,5,1.0*1.0/(60*57.3*60*57.3));
	p.set(6,6,3.0*3.0/(60*57.3*60*57.3));//平台初始误差角，单位：弧度
	p.set(7,7,0.01*0.01/(57.3*3600*57.3*3600));
	p.set(8,8,0.01*0.01/(57.3*3600*57.3*3600));
	p.set(9,9,0.01*0.01/(57.3*3600*57.3*3600));//陀螺随机漂移，单位：°/h
	matrix y(2);//y放观测值(经纬度差值加观测噪声)
	gassvector gpsv(Rk);//产生观测噪声，单位：角分(′)
	double F[10][10];//此数组保存状态方程的系数矩阵
	matrix K(10,2);//滤波增益矩阵
	matrix ymar(2);//经纬度差值
	double ux,uy,uz;//持续性校正量
	ux=uy=uz=0.0;//陀螺随机漂移校正量
	ua=ub=ur=0.0;//平台误差角校正量
	//以下进行导航参数计算并进行卡尔曼滤波，观测值由理想惯导系统输出参数加入白噪声产生
	for(j=0;j<=(T*3600/h);j++)//模拟时间10h，计算步长为0.1秒
	{
		//匀速直线运动下的地理坐标系下东向和北向理想加速度
		Ax=-(2*w*sin(zswd)+Vx*tan(zswd)/RN)*Vy;
		Ay=(2*w*sin(zswd)+Vx*tan(zswd)/RN)*Vx;

		//(1)平台坐标系下加速度计输出方程
		Apx=Ax-B*g+ax;
		Apy=Ay+A*g+ay;     //模拟加速度计的输出

		//此时到了一个卡尔曼滤波计算周期，得到了t时刻的观测值，可以进行卡尔曼滤波计算
		if(0==(j%Filstep))
		{
			if(j>0)//进行卡尔曼滤波计算(0时刻不进行)
			{
				ymar.set(0,wd-zswd);//单位：角分(′)
				ymar.set(1,jd-zsjd);
				y=ymar+gpsv();//产生观测噪声，单位是弧度
				//进行卡尔曼滤波计算
				p=f*p*f.t()+Qk;	// 预测误差协方差
				K=p*H.t()*((H*p*H.t()+Rk).inv()); // 新息的增益
				x=K*y;	//闭环系统，得出估计值
		//		p=(I-K*H)*p;// 估计误差协方差阵
				p=(I-K*H)*p*(I-K*H).t()+K*Rk*K.t();

				//kalman滤波算出估计值后，进行闭环反馈校正
				wd-=x(0);jd-=x(1);
				Vx-=x(2);Vy-=x(3);
				/////////////////////////////////以上两项为一次性校正量
				ua=-x(4)/Filcycle;
				ub=-x(5)/Filcycle;
				ur=-x(6)/Filcycle;//平台误差角校正量
				ux-=x(7);//原单位：°/h
				uy-=x(8);
				uz-=x(9);//陀螺markov随机漂移校正量
				/////////////////////////////////以上两项为持续性校正量
			}
			//完成卡尔曼滤波之后f和Qk进行重新设置
			f=I;//f重设为单位阵
			Qk=0.0;//将系统噪声方差阵重新设为0
		}

		//列写卡尔曼滤波状态方程的系数矩阵
		//当j%Filstep==Filstep-1时，得出了系统状态转移矩阵及驱动噪声方差阵，
		//待下一个导航参数计算周期到来，得到观测值后，即可进行卡尔曼滤波计算。
		for(int num1=0;num1<10;num1++)
				for(int num2=0;num2<10;num2++)
					F[num1][num2]=0.0;//全设为0
		F[0][3]=1/RM;
		F[1][0]=Vcx*tan(wd)/(RM*cos(wd));F[1][2]=1.0/(RM*cos(wd));
		F[2][0]=2.0*w*cos(wd)*Vcy+Vcx*Vcy/(RM*cos(wd)*cos(wd));F[2][2]=Vcy*tan(wd)/RM;
		F[2][3]=2.0*w*sin(wd)+Vcx*tan(wd)/RM;F[2][5]=-g;F[2][6]=Apy;
		F[3][0]=-2.0*w*cos(wd)*Vcx-Vcx*Vcx/(RM*cos(wd)*cos(wd));
		F[3][2]=-2.0*w*sin(wd)-2.0*Vcx*tan(wd)/RM;F[3][4]=g;F[3][6]=-Apx;
		F[4][3]=-1.0/RM;F[4][5]=w*sin(wd)+Vcx*tan(wd)/RM;
		F[4][6]=-(w*cos(wd)+Vcx/RM);F[4][7]=1.0;
		F[5][0]=-w*sin(wd);F[5][2]=1.0/RM;F[5][4]=-(w*sin(wd)+Vcx*tan(wd)/RM);
		F[5][6]=-Vcy/RM;F[5][8]=1.0;
		F[6][0]=w*cos(wd)+Vcx/(RM*cos(wd)*cos(wd));F[6][2]=tan(wd)/RM;
		F[6][4]=w*cos(wd)+Vcx/RM;F[6][5]=Vcy/RM;F[6][9]=1.0;
		F[7][7]=-1.0/(sgt*3600);
		F[8][8]=-1.0/(sgt*3600);
		F[9][9]=-1.0/(sgt*3600);
		matrix temp(F,10,10);
		f=(I+h*temp)*f;//算出状态转移矩阵Ф
		Qk+=(temp*Qk+Qk*temp.t()+GQGt)*h;//用欧拉积分法算出Qk
				
///////////////////////////////////////////////////////////////////以下开始解算导航参数		
		//(2)求出实际惯导系统的计算速度和经纬度
		a[0]=Vcx;a[1]=Vcy;a[2]=wd;a[3]=jd;
		k[0]=0.0;k[1]=0.0;k[2]=0.0;k[3]=0.0;
	
		Speed();Load();//求出K1
		k[0]+=dVcx;k[1]+=dVcy;k[2]+=dwd;k[3]+=djd;
		Vcx=a[0]+0.5*h*dVcx;Vcy=a[1]+0.5*h*dVcy;
		wd=a[2]+0.5*h*dwd;jd=a[3]+0.5*h*djd;
			
		Speed();Load();//求出K2
		k[0]+=2*dVcx;k[1]+=2*dVcy;k[2]+=2*dwd;k[3]+=2*djd;
		Vcx=a[0]+0.5*h*dVcx;Vcy=a[1]+0.5*h*dVcy;
		wd=a[2]+0.5*h*dwd;jd=a[3]+0.5*h*djd;
	
		Speed();Load();//求出K3
		k[0]+=2*dVcx;k[1]+=2*dVcy;k[2]+=2*dwd;k[3]+=2*djd;
		Vcx=a[0]+h*dVcx;Vcy=a[1]+h*dVcy;
		wd=a[2]+h*dwd;jd=a[3]+h*djd;
	
		Speed();Load();//求出K4
		k[0]+=dVcx;k[1]+=dVcy;k[2]+=dwd;k[3]+=djd;
		//算出速度和经纬度
		Vcx=a[0]+(h*k[0])/6.0;Vcy=a[1]+(h*k[1])/6.0;
		wd=a[2]+(h*k[2])/6.0;jd=a[3]+(h*k[3])/6.0;
	
		//到达两极时退出
		if(wd>1.5)
		{
			AfxMessageBox("接近北极!");
			return;
		}
		if(wd<-1.5)
		{
			AfxMessageBox("接近南极!");
			return;
		}
		
		//求出真实的速度和经纬度
		a[0]=Vx;a[1]=Vy;a[2]=zswd;a[3]=zsjd;
		k[0]=0.0;k[1]=0.0;k[2]=0.0;k[3]=0.0;

		RealSpeed();RealLoad();//求出K1
		k[0]+=dVx;k[1]+=dVy;k[2]+=dzswd;k[3]+=dzsjd;
		Vx=a[0]+0.5*h*dVx;Vy=a[1]+0.5*h*dVy;
		zswd=a[2]+0.5*h*dzswd;zsjd=a[3]+0.5*h*dzsjd;
		
		RealSpeed();RealLoad();//求出K2
		k[0]+=2*dVx;k[1]+=2*dVy;k[2]+=2*dzswd;k[3]+=2*dzsjd;
		Vx=a[0]+0.5*h*dVx;Vy=a[1]+0.5*h*dVy;
		zswd=a[2]+0.5*h*dzswd;zsjd=a[3]+0.5*h*dzsjd;

		RealSpeed();RealLoad();//求出K3
		k[0]+=2*dVx;k[1]+=2*dVy;k[2]+=2*dzswd;k[3]+=2*dzsjd;		
		Vx=a[0]+h*dVx;Vy=a[1]+h*dVy;
		zswd=a[2]+h*dzswd;zsjd=a[3]+h*dzsjd;

		RealSpeed();RealLoad();//求出K4
		k[0]+=dVx;k[1]+=dVy;k[2]+=dzswd;k[3]+=dzsjd;
		Vx=a[0]+(h*k[0])/6.0;Vy=a[1]+(h*k[1])/6.0;
		zswd=a[2]+(h*k[2])/6.0;zsjd=a[3]+(h*k[3])/6.0;
/////////////////////////////////////////////////////////////////至此算出t+1时刻的导航参数
		if(zswd>1.5)
		{
			AfxMessageBox("接近北极!");
			return;
		}
		if(zswd<-1.5)
		{
			AfxMessageBox("接近南极!");
			return;
		}

		//(3)旋转角速度控制方程
		Wcx=-Vcy/RM;
		Wcy=w*cos(wd)+Vcx/RN;
		Wcz=w*sin(wd)+Vcx*tan(wd)/RN;

		//求出载体所在地理坐标系相对惯性空间的旋转角速度
		Wx=-Vy/RM;
		Wy=w*cos(zswd)+Vx/RN;
		Wz=w*sin(zswd)+Vx*tan(zswd)/RN;

		//产生马尔可夫过程的随机漂移
		if(0==(j%sjstepnum))//隔2秒随机漂移值变化一次
			if(j)//如果j不为0则变化一次
				rr=axs*rr+gyronoise();//随机漂移
		grx=rr(0);gry=rr(1);grz=rr(2);
		//陀螺漂移补偿量随时间呈指数规律下降
		gcorx=(1-fsgt*(j%Filstep)*h)*ux;
		gcory=(1-fsgt*(j%Filstep)*h)*uy;
		gcorz=(1-fsgt*(j%Filstep)*h)*uz;

		//(4)平台误差角微分方程
		a[0]=A;a[1]=B;a[2]=R;
		k[0]=0.0;k[1]=0.0;k[2]=0.0;
				
		ErrorAngle();//求出K1
		k[0]+=dA;k[1]+=dB;k[2]+=dR;
		A=a[0]+0.5*h*dA;
		B=a[1]+0.5*h*dB;
		R=a[2]+0.5*h*R;
		
		ErrorAngle();//求出K2
		k[0]+=2*dA;k[1]+=2*dB;k[2]+=2*dR;
		A=a[0]+0.5*h*dA;
		B=a[1]+0.5*h*dB;
		R=a[2]+0.5*h*R;

		ErrorAngle();//求出K3
		k[0]+=2*dA;k[1]+=2*dB;k[2]+=2*dR;
		A=a[0]+h*dA;
		B=a[1]+h*dB;
		R=a[2]+h*R;

		ErrorAngle();//求出K4
		k[0]+=dA;k[1]+=dB;k[2]+=dR;
		
		A=a[0]+(h*k[0])/6.0;
		B=a[1]+(h*k[1])/6.0;
		R=a[2]+(h*k[2])/6.0;
		
		if(j%savestep==0)//180秒保存一个点
		{
			if(j<((T*3600/h)/savestep)*savestep)
			{
				//保存α,β,γ值
				of1<<A<<',';of2<<B<<',';of3<<R<<',';
				//保存经纬度误差值
				of4<<wd-zswd<<',';of5<<jd-zsjd<<',';
				//保存东向和北向速度误差值
				of6<<Vcx-Vx<<',';of7<<Vcy-Vy<<',';
				of8<<rr(0)<<',';
			}
			else
			{
				//保存α,β,γ值
				of1<<A<<';';of2<<B<<';';of3<<R<<';';
				//保存经纬度误差值
				of4<<wd-zswd<<';';of5<<jd-zsjd<<';';
				//保存东向和北向速度误差值
				of6<<Vcx-Vx<<';';of7<<Vcy-Vy<<';';
				of8<<rr(0)<<';';
			}
		}
	}
	of1.close();of2.close();of3.close();
	of4.close();of5.close();of6.close();of7.close();
	of8.close();
//	of9.close();
	AfxMessageBox("运算完毕!");
}
	
void CKalmanDoc::Speed()
{
	dVcx=Apx+(2*w*sin(wd)+Vcx*tan(wd)/RN)*Vcy;
	dVcy=Apy-(2*w*sin(wd)+Vcx*tan(wd)/RN)*Vcx;
}

void CKalmanDoc::RealSpeed()
{
	dVx=0;
	dVy=0;
}

void CKalmanDoc::Load()
{
	dwd=Vcy/RM;
	djd=Vcx/(RN*cos(wd));
}

void CKalmanDoc::RealLoad()
{
	dzswd=Vy/RM;
	dzsjd=Vx/(RN*cos(zswd));
}

void CKalmanDoc::ErrorAngle()
{
	dA=Wcx-Wx-R*Wy+B*Wz+gcx+grx+gcorx+ua;
	dB=Wcy-Wy-A*Wz+R*Wx+gcy+gry+gcory+ub;
	dR=Wcz-Wz-B*Wx+A*Wy+gcz+grz+gcorz+ur;
}