transfer_alignment_modify.m

飞行轨迹的产生

	wiey=wie*cos(L);
	wiez=wie*sin(L);
	%计算地理系t相对地球的角速度，单位：弧度/秒
	wetx=-vy(i)/Rtyh;
	wety=vx(i)/Rtxh;
	wetz=vx(i)*tan(L)/Rtxh;
	%计算MINS比力，单位：米/(秒*秒)
	fmx=dvx(i)-vy(i)*(2*wiez+wetz)+vz(i)*(2*wiey+wety);%利用比力方程反推加速度计输出
	fmy=dvy(i)+vx(i)*(2*wiez+wetz)-vz(i)*wetx;
	fmz=dvz(i)-vx(i)*(2*wiey+wety)+vy(i)*wetx+g;
	%计算地理系t相对惯性系的角速度，单位：弧度/秒
	witx=wiex+wetx;
	wity=wiey+wety;
	witz=wiez+wetz;
	%计算飞机机体坐标系b相对惯性系的角速度，单位：弧度/秒
	wibx=witx+wtbx(i);
	wiby=wity+wtby(i);
	wibz=witz+wtbz(i);
	%计算姿态阵cbt
	cbt=zeros(3,3);
	cbt(1,1)=cos(theta(i))*sin(psi(i));
	cbt(1,2)=sin(gama(i))*cos(psi(i))-sin(theta(i))*cos(gama(i))*sin(psi(i));
	cbt(1,3)=cos(gama(i))*cos(psi(i))+sin(psi(i))*sin(gama(i))*sin(theta(i));
 	cbt(2,1)=cos(theta(i))*cos(psi(i));
	cbt(2,2)=-sin(gama(i))*sin(psi(i))-sin(theta(i))*cos(gama(i))*cos(psi(i));
	cbt(2,3)=-cos(gama(i))*sin(psi(i))+sin(theta(i))*sin(gama(i))*cos(psi(i));
 	cbt(3,1)=sin(theta(i));
	cbt(3,2)=cos(theta(i))*cos(gama(i));
   cbt(3,3)=-cos(theta(i))*sin(gama(i));
	%计算矩阵WE
	WE=zeros(3,3);
	WE(1,1)=2*wibz+2*wiby*xix;
	WE(1,2)=-2*wibz*xiz-2*wibx*xix;
	WE(1,3)=2*wiby*xiz-2*wibx;
	WE(2,1)=2*wibz*xix-2*wiby;
	WE(2,2)=2*wibz*xiy+2*wibx;
	WE(2,3)=-2*wiby*xiy-2*wibx*xix;
	WE(3,1)=-2*wibz*xiz-2*wiby*xiy;
	WE(3,2)=-2*wibz+2*wibx*xiy;
	WE(3,3)=2*wiby+2*wibx*xiz;
	%计算函数RB(X,Y,Z)
	RBXX=(cbt(1,2)*rz-cbt(1,3)*ry)*betax;
	RBXY=(cbt(1,3)*rx-cbt(1,1)*rz)*betay;
	RBXZ=(cbt(1,1)*ry-cbt(1,2)*rx)*betaz;
	RBYX=(cbt(2,2)*rz-cbt(2,3)*ry)*betax;
	RBYY=(cbt(2,3)*rx-cbt(2,1)*rz)*betay;
	RBYZ=(cbt(2,1)*ry-cbt(2,2)*rx)*betaz;
	RBZX=(cbt(3,2)*rz-cbt(3,3)*ry)*betax;
	RBZY=(cbt(3,3)*rx-cbt(3,1)*rz)*betay;
	RBZZ=(cbt(3,1)*ry-cbt(3,2)*rx)*betaz;
	%计算函数WB(X,Y,Z)
	WBXX=WE(1,1)*RBXX+WE(1,2)*RBYX+WE(1,3)*RBZX;
	WBXY=WE(1,1)*RBXY+WE(1,2)*RBYY+WE(1,3)*RBZY;
	WBXZ=WE(1,1)*RBXZ+WE(1,2)*RBYZ+WE(1,3)*RBZZ;
	WBYX=WE(2,1)*RBXX+WE(2,2)*RBYX+WE(2,3)*RBZX;
	WBYY=WE(2,1)*RBXY+WE(2,2)*RBYY+WE(2,3)*RBZY;
	WBYZ=WE(2,1)*RBXZ+WE(2,2)*RBYZ+WE(2,3)*RBZZ;
	WBZX=WE(3,1)*RBXX+WE(3,2)*RBYX+WE(3,3)*RBZX;
	WBZY=WE(3,1)*RBXY+WE(3,2)*RBYY+WE(3,3)*RBZY;
	WBZZ=WE(3,1)*RBXZ+WE(3,2)*RBYZ+WE(3,3)*RBZZ;
	%计算函数RP(X,Y,Z)
	RPXX=cbt(1,3)*ry-cbt(1,2)*rz;
	RPXY=cbt(1,1)*rz-cbt(1,3)*rx;
	RPXZ=cbt(1,2)*rx-cbt(1,1)*ry;
	RPYX=cbt(2,3)*ry-cbt(2,2)*rz;
	RPYY=cbt(2,1)*rz-cbt(2,3)*rx;
	RPYZ=cbt(2,2)*rx-cbt(2,1)*ry;
	RPZX=cbt(3,3)*ry-cbt(3,2)*rz;
	RPZY=cbt(3,1)*rz-cbt(3,3)*rx;
	RPZZ=cbt(3,2)*rx-cbt(3,1)*ry;
	%计算函数EP(X,Y,Z)
	EPXX=WE(1,1)*RPXX+WE(1,2)*RPYX+WE(1,3)*RPZX;
	EPXY=WE(1,1)*RPXY+WE(1,2)*RPYY+WE(1,3)*RPZY;
	EPXZ=WE(1,1)*RPXZ+WE(1,2)*RPYZ+WE(1,3)*RPZZ;
	EPYX=WE(2,1)*RPXX+WE(2,2)*RPYX+WE(2,3)*RPZX;
	EPYY=WE(2,1)*RPXY+WE(2,2)*RPYY+WE(2,3)*RPZY;
	EPYZ=WE(2,1)*RPXZ+WE(2,2)*RPYZ+WE(2,3)*RPZZ;
	EPZX=WE(3,1)*RPXX+WE(3,2)*RPYX+WE(3,3)*RPZX;
	EPZY=WE(3,1)*RPXY+WE(3,2)*RPYY+WE(3,3)*RPZY;
	EPZZ=WE(3,1)*RPXZ+WE(3,2)*RPYZ+WE(3,3)*RPZZ;
	%计算函数CW1,CW2,CW3
	cw1=cbt(1,1)*witx+cbt(2,1)*wity+cbt(3,1)*witz+wtbx(i);
	cw2=cbt(1,2)*witx+cbt(2,2)*wity+cbt(3,2)*witz+wtby(i);
	cw3=cbt(1,3)*witx+cbt(2,3)*wity+cbt(3,3)*witz+wtbz(i);
	%计算函数HF(X,Y,Z)
	HFXX=cbt(1,3)*cw2-cbt(1,2)*cw3;
	HFXY=cbt(2,3)*cw2-cbt(2,2)*cw3;
	HFXZ=cbt(3,3)*cw2-cbt(3,2)*cw3;
	HFYX=cbt(1,1)*cw3-cbt(1,3)*cw1;
	HFYY=cbt(2,1)*cw3-cbt(2,3)*cw1;
	HFYZ=cbt(3,1)*cw3-cbt(3,3)*cw1;
	HFZX=cbt(1,2)*cw1-cbt(1,1)*cw2;
	HFZY=cbt(2,2)*cw1-cbt(2,1)*cw2;
	HFZZ=cbt(3,2)*cw1-cbt(3,1)*cw2;
	%计算状态转移阵a
	a=zeros(15,15);
	a(1,4)=fmz;
	a(1,6)=-fmx;
	a(1,7)=fmz*cbt(1,1)-fmx*cbt(3,1)+WBXX;
	a(1,8)=fmz*cbt(1,2)-fmx*cbt(3,2)+WBXY;
	a(1,9)=fmz*cbt(1,3)-fmx*cbt(3,3)+WBXZ;
	a(1,14)=1;
	a(2,4)=-fmy;
	a(2,5)=fmx;
	a(2,7)=-fmy*cbt(1,1)+fmx*cbt(2,1)+WBYX;
	a(2,8)=-fmy*cbt(1,2)+fmx*cbt(2,2)+WBYY;
	a(2,9)=-fmy*cbt(1,3)+fmx*cbt(2,3)+WBYZ;
	a(2,15)=1;
	a(3,5)=-fmz;
	a(3,6)=fmy;
	a(3,7)=-fmz*cbt(2,1)+fmy*cbt(3,1)+WBZX;
	a(3,8)=-fmz*cbt(2,2)+fmy*cbt(3,2)+WBZY;
	a(3,9)=-fmz*cbt(2,3)+fmy*cbt(3,3)+WBZZ;
	a(3,13)=1;
	a(4,1)=-1/Rtyh;
	a(4,5)=witz;
	a(4,6)=-wity;
	a(4,7)=witz*cbt(2,1)-wity*cbt(3,1);
	a(4,8)=witz*cbt(2,2)-wity*cbt(3,2);
	a(4,9)=witz*cbt(2,3)-wity*cbt(3,3);
	a(4,10)=1;
	a(5,3)=1/Rtxh;
	a(5,4)=-witz;
	a(5,6)=witx;
	a(5,7)=-witz*cbt(1,1)+witx*cbt(3,1);
	a(5,8)=-witz*cbt(1,2)+witx*cbt(3,2);
	a(5,9)=-witz*cbt(1,3)+witx*cbt(3,3);
	a(5,11)=1;
	a(6 ,3)=tan(L)/Rtxh;
	a(6,4)=wity;
	a(6,5)=-witx;
	a(6,7)=wity*cbt(1,1)-witx*cbt(2,1);
	a(6,8)=wity*cbt(1,2)-witx*cbt(2,2);
	a(6,9)=wity*cbt(1,3)-witx*cbt(2,3);
	a(6,12)=1;
	a(7,7)=-betax;
	a(8,8)=-betay;
	a(9,9)=-betaz;
	%量测系数阵H
	H=zeros(6,15);
	H(1,1)=1;
	H(2,2)=1;
	H(3,3)=1;
	H(4,4)=HFXX;
	H(4,5)=HFXY;
	H(4,6)=HFXZ;
	H(4,7)=-betax;
	H(4,8)=-cw3;
	H(4,9)=cw2;
	H(4,10)=cbt(1,1);
	H(4,11)=cbt(2,1);
	H(4,12)=cbt(3,1);
	H(5,4)=HFYX;
	H(5,5)=HFYY;
	H(5,6)=HFYZ;
	H(5,7)=cw3;
	H(5,8)=-betay;
	H(5,9)=cw1;
	H(5,10)=cbt(1,2);
	H(5,11)=cbt(2,2);
	H(5,12)=cbt(3,2);
	H(6,4)=HFZX;
	H(6,5)=HFZY;
	H(6,6)=HFZZ;
	H(6,7)=-cw2;
	H(6,8)=cw1;
	H(6,9)=-betaz;
	H(6,10)=cbt(1,3);
	H(6,11)=cbt(2,3);
	H(6,12)=cbt(3,3);
	%计算系统白噪声方差阵Q
	Q=zeros(9,9);
	Q(1,1)=aay+EPXX^2*acx+EPXY^2*acy+EPXZ^2*acz;
	Q(2,2)=aaz+EPYX^2*acx+EPYY^2*acy+EPYZ^2*acz;
	Q(3,3)=aax+EPZX^2*acx+EPZY^2*acy+EPZZ^2*acz;
	Q(4,4)=abx;
	Q(5,5)=aby;
	Q(6,6)=abz;
	Q(7,7)=acx;
	Q(8,8)=acy;
	Q(9,9)=acz;
	%量测噪声方差阵R
	R=zeros(6,6);
	R(1,1)=1^2;
	R(2,2)=1^2;
	R(3,3)=1^2;
	R(4,4)=cbt(1,1)^2*SWEX^2+cbt(2,1)^2*SWEY^2+cbt(3,1)^2*SWEZ^2+2*betax*Tm*(0.05*pi/180)^2;
	R(5,5)=cbt(1,2)^2*SWEX^2+cbt(2,2)^2*SWEY^2+cbt(3,2)^2*SWEZ^2+2*betay*Tm*(0.05*pi/180)^2;
	R(6,6)=cbt(1,3)^2*SWEX^2+cbt(2,3)^2*SWEY^2+cbt(3,3)^2*SWEZ^2+2*betaz*Tm*(0.05*pi/180)^2;
	%对系统离散化
	A=I+a*Tm+a*a*(Tm^2)/2;
   B=Tm*(I+a*Tm/2+a*a*(Tm^2)/6)*G;
   %动态系统噪声
   sysnoise=zeros(9,1);
   sysnoise(1,1)=randn(1,1)*SWDX+EPXX*randn(1,1)*SWPX+EPXY*randn(1,1)*SWPY+EPXZ*randn(1,1)*SWPZ;
   sysnoise(2,1)=randn(1,1)*SWDY+EPYX*randn(1,1)*SWPX+EPYY*randn(1,1)*SWPY+EPYZ*randn(1,1)*SWPZ;
   sysnoise(3,1)=randn(1,1)*SWDZ+EPZX*randn(1,1)*SWPX+EPZY*randn(1,1)*SWPY+EPZZ*randn(1,1)*SWPZ;
   sysnoise(4,1)=randn(1,1)*SWEX;
   sysnoise(5,1)=randn(1,1)*SWEY;
   sysnoise(6,1)=randn(1,1)*SWEZ;
   sysnoise(7,1)=randn(1,1)*SWPX;
   sysnoise(8,1)=randn(1,1)*SWPY;
   sysnoise(9,1)=randn(1,1)*SWPZ;
   %观测系统噪声
   obsnoise=zeros(6,1);
   obsnoise(1,1)=randn(1,1)*1;
   obsnoise(2,1)=randn(1,1)*1;
   obsnoise(3,1)=randn(1,1)*1;
   obsnoise(4,1)=cbt(1,1)*randn(1,1)*SWEX+cbt(2,1)*randn(1,1)*SWEY+cbt(3,1)*randn(1,1)*SWEZ+randn(1,1)*SWPX;
   obsnoise(5,1)=cbt(1,2)*randn(1,1)*SWEX+cbt(2,2)*randn(1,1)*SWEY+cbt(3,2)*randn(1,1)*SWEZ+randn(1,1)*SWPY;
   obsnoise(6,1)=cbt(1,3)*randn(1,1)*SWEX+cbt(2,3)*randn(1,1)*SWEY+cbt(3,3)*randn(1,1)*SWEZ+randn(1,1)*SWPZ;
   %计算观测值z
   z=H*[A*xfeed+B*sysnoise]+obsnoise;
   %计算一步预测均方误差
   P1=A*P0*A'+B*Q*B';
   %计算滤波增益
   K=P1*H'*inv(H*P1*H'+R);
   %计算估计均方误差
   P=(I-K*H)*P1*(I-K*H)'+K*R*K';
   %计算状态
   x0=A*x0+K*(z-H*x0);
	%下面计算各状态量估计均方误差
	%速度误差  单位:米/秒
	y1(i)=sqrt(P(1,1)); %X轴
	y2(i)=sqrt(P(2,2)); %Y轴
	y3(i)=sqrt(P(3,3)); %Z轴
	%平台误差角  单位:角分
	y4(i)=sqrt(P(4,4))*60*180/pi; %X轴
	y5(i)=sqrt(P(5,5))*60*180/pi; %Y轴
	y6(i)=sqrt(P(6,6))*60*180/pi; %Z轴
	%挠曲变形角  单位:角分
	y7(i)=sqrt(P(7,7))*60*180/pi; %X轴
	y8(i)=sqrt(P(8,8))*60*180/pi; %Y轴
	y9(i)=sqrt(P(9,9))*60*180/pi; %Z轴
	%陀螺漂移  单位:度/小时
	y10(i)=sqrt(P(10,10))*180*3600/pi; %X轴
	y11(i)=sqrt(P(11,11))*180*3600/pi; %Y轴
	y12(i)=sqrt(P(12,12))*180*3600/pi; %Z轴
   %加速度计误差  单位:米/(秒*秒)
   y13(i)=sqrt(P(13,13))*10^3/g0; %X轴
	y14(i)=sqrt(P(14,14))*10^3/g0; %Y轴
   y15(i)=sqrt(P(15,15))*10^3/g0; %Z轴
	%保存各状态量估计
	%速度误差  单位:米/秒
	x1(i)=x0(1,1); %X轴
	x2(i)=x0(2,1); %Y轴
	x3(i)=x0(3,1); %Z轴
	%平台误差角  单位:角分
	x4(i)=x0(4,1)*60*180/pi; %X轴
	x5(i)=x0(5,1)*60*180/pi; %Y轴
	x6(i)=x0(6,1)*60*180/pi; %Z轴
	%挠曲变形角  单位:角分
	x7(i)=x0(7,1)*60*180/pi; %X轴
	x8(i)=x0(8,1)*60*180/pi; %Y轴
	x9(i)=x0(9,1)*60*180/pi; %Z轴
	%陀螺漂移  单位:度/小时
	x10(i)=x0(10,1)*180*3600/pi; %X轴
	x11(i)=x0(11,1)*180*3600/pi; %Y轴
	x12(i)=x0(12,1)*180*3600/pi; %Z轴
	%加速度计误差  单位:米/(秒*秒)
	x13(i)=x0(13,1)*10^3/g0; %X轴
	x14(i)=x0(14,1)*10^3/g0; %Y轴
   x15(i)=x0(15,1)*10^3/g0; %Z轴
   %计算飞机所在点纬度 单位：弧度
	L=L+vy(i)*Tm/Rtyh;
	%计算飞机所在点重力加速度, 单位:米/(秒*秒)
	g=9.7803267714*(1+0.00193185138639*sin(L)^2)/sqrt(1-0.00669437999013*sin(L)^2);
	%计算地理系t偏离初始时刻地理系t0的偏角xi 单位:弧度
	xix=xix+wetx*Tm;
	xiy=xiy+(wiey+wety)*Tm;
	xiz=xiz+(wiez+wetz)*Tm;
	%计算初始对准结束时的SINS姿态阵CMS：CMS=cit*cto*cts*cbt*cbs
	%计算cit阵
	cit=zeros(3,3);
	cit(1,2)=1;
	cit(2,3)=1;
	cit(3,1)=1;
	%计算cto阵
	cto=zeros(3,3);
	cto(1,1)=1;
	cto(1,2)=-xiz;
	cto(1,3)=xiy;
	cto(2,1)=xiz;
	cto(2,2)=1;
	cto(2,3)=-xix;
	cto(3,1)=-xiy;
	cto(3,2)=xix;
	cto(3,3)=1;
	%计算cts阵
	tmpnew=cbt*[x0(7,1);x0(8,1);x0(9,1)];
	cts=zeros(3,3);
	cts(1,1)=1;
	cts(1,2)=-(x0(6,1)+tmpnew(3,1));
	cts(1,3)=x0(5,1)+tmpnew(3,1);
	cts(2,1)=-cts(1,2);
	cts(2,2)=1;
	cts(2,3)=-(x0(4,1)+tmpnew(1,1));
	cts(3,1)=-cts(1,3);
	cts(3,2)=-cts(2,3);
	cts(3,3)=1;
	%计算cbs阵
	cbs=zeros(3,3);
	cbs(1,1)=1;
	cbs(2,2)=0.707;
	cbs(2,3)=0.707;
	cbs(3,2)=-0.707;
	cbs(3,3)=0.707;
	%计算SINS姿态阵cms
	cms=cit*cto*cts*cbt*cbs;%45度安装
	CMS=[CMS,cms];
    cms2=cit*cto*cts*cbt;%0度安装
    CMS2=[CMS2,cms2];
	%当前值作为下一步计算的初始值
   P0=P;
   xfeed=x0;
  

  
J=[0 0 1;1 0 0;0 1 0];
cgbs2=(cto'*J*cms2*J')';%cto为采样时刻t'相对于im的绝对偏差角所构成的偏离矩阵
  fuyang2=asin(cgbs2(2,3))*180/pi;%导弹倾斜0度安装,导弹的姿态角(横滚、俯仰、航向)
  hengun2=atan(-cgbs2(1,3)/cgbs2(3,3))*180/pi;
  hangxiang2=asec(cos(fuyang2*pi/180)/cgbs2(2,2))*180/pi;

 %vb=[vx(i)]
  
  
  if (psi(i)<0)   %不考虑导弹相对机体的坐标延迟，具体见相应文档
       psi(i)=psi(i)+2*pi;
       hangxiang2=360-hangxiang2;
   elseif  (psi(i)>pi)&(psi(i)<=1.5*pi)
       hangxiang2=360-hangxiang2;
   elseif (psi(i)>1.5*pi)&(psi(i)<=2*pi) 
       hangxiang2=360-hangxiang2;
   elseif psi(i)>2*pi
       psi(i)=psi(i)-2*pi;
   end    
  

  %数据格式为采样时间；飞机的姿态角（横滚、俯仰、航向）、飞机的北、东、地速度；经、纬、高度；导弹的姿态角；速度误差均方根（北、东、地）；
  %平台误差角均方根；挠曲变形角均方根(机头、机背、右翼)；陀螺漂移均方根(北、东、地)；加速度计零偏均方根(北、东、地)
 %if( hangxiang2<0)
fprintf(fid,'%f %f %f %f %f %f %f %12.8f %12.8f %12.8f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f \n',t,gama(i)*180/pi,theta(i)*180/pi,psi(i)*180/pi,vy(i),vx(i),-vz(i),long1(i),lat1(i),h(i),hengun2,fuyang2,hangxiang2,y2(i),y1(i),-y3(i),y4(i),y5(i),y6(i),y7(i),y8(i),y9(i),y11(i),y10(i),-y12(i),y14(i),y13(i),-y15(i),x2(i),x1(i),-x3(i),x4(i),x5(i),x6(i),x7(i),x8(i),x9(i),x11(i),x10(i),-x12(i),x14(i),x13(i),-x15(i));
                                                                                                                                                             %1      2             3                  4        5     6      7     8        9      10      11    12       13       14    15     16    17    18    19    20     21   22    23     24      25      26     27     28     29    30     31     32   33     34    35   36   37     38     39       40    41      42    43
  i=i+1;                                                
end		%end of for
fclose(fid);