matlab-捷联惯性导航与GPS组合导航，采用自适应算法对其进行处理

clear R=6378137; e=0.0033528106647474807198455286185206; %地球扁率精确值 ee=0.00669437999014131699614; omega=7292115.1467e-11; g=9.78; %TimeEach=1; %周期 和仿真总时间 deg_rad=pi/180; %由度转化成弧度 rad_deg=180/pi; %由弧度转化成度 T=0.01; TimeEach=T; time=3600; m=100; % aa=zeros(1000,1)'; % bb=zeros(1000,1)'; % cc=zeros(1000,1)'; % dd=zeros(1000,1)'; jjj=1; yinzi1=0.5; yinzi2=0.5; fid_read=fopen('e:\TESTIMU.TXT','r'); [AllData NumofAllData]=fscanf(fid_read,'%g %g %g %g %g %g %g %g %g %g %g',[11 inf]); % 东向、北向、天向速度 单位：米/妙 Ve(time,1)=0; Vn(time,1)=0; Vu(time,1)=0; %经度 纬度 高度 longitude(time,1)=0; latitude(time,1)=0; High(time,1)=0; fb_x=AllData(8,:); %比力（fx，fy，fz） fb_y=AllData(9,:); %指向右机翼方向为x正方向，指向机头方向为y正向，z轴与x轴和y轴构成右手坐标系 单位：米/秒2 fb_z=AllData(10,:)*g; %右前上 sg1(1,time)=0; sg2(1,time)=0; sg3(1,time)=0; sg4(1,time)=0; sg5(1,time)=0; sg6(1,time)=0; sg7(1,time)=0; sg8(1,time)=0; sg9(1,time)=0; pitch(time,1)=0;%俯仰角（向上为正） 单位:弧度 head(time,1)=0;%偏航角（偏西为正） roll(time,1)=0; %滚转角（向右为正） omigax=AllData(5,:); %陀螺输出（单位：弧度/秒，坐标轴的定义与比力的相同） omigay=AllData(6,:); omigaz=-AllData(7,:); %-------------------------------程序初始化-------------------------------------- latitude0= 39.975172*2*pi/360; % 初始纬度 longitude0=116.344695283*2*pi/360; %% 初始经度 %High0=High(1); High(time,1)=0; Ve0=0; %Ve(1); Vn0=0; %Vn(1); %初始速度 Vu0=0; %Vu(1); pitch0=0; %pitch(1); head0=0; %head(1); %初始姿态 roll0=0; %roll(1); vye0=0.01; %假设没有初始对准误差 pitch_err0=pitch0+0*deg_rad; head_err0=head0+0*deg_rad; roll_err0=roll0+0*deg_rad; faie0=2.0/60*pi/180; lamdae0=2.0/60*pi/180; afae0=3.0/60*pi/180; beitae0=3.0/60*pi/180; gamae0=5.0/60*pi/180; hxjz=pi/4; vx=20*1852/3600*sin(hxjz); vy=20*1852/3600*cos(hxjz); vz=0; % weichagps=25;%GPS位置误差 suchagps=0.05;%GPS速度误差 gyroe0=(0.01/3600)*pi/180; gyrotime=1/7200;%陀螺漂移反向相关时间 atime=1/1800; gyronoise=(0.001/3600)/180*pi;%陀螺漂移白噪声 beta_d=1/6000.0; %速度偏移误差反向相关时间 beta_drta=1/6000.0; %偏流角误差反向相关时间 %matrix of system equation % fai=latitude0; % lamada=longitude0; zong=0*pi/180; heng=0*pi/180; hang=0*pi/180; F(15,15)=0; %initial value x1(15,1)=0; %the error of sins xx=x1; xx(1)=faie0; %ljn xx(2)=lamdae0; xx(5)=afae0; xx(6)=beitae0; xx(7)=gamae0; xx(8)=(0.01/3600)*pi/180; xx(9)=(0.01/3600)*pi/180; xx(10)=(0.01/3600)*pi/180; xx(11)=0.0005; xx(12)=0.0005; xx(13)=0.0005; %w=[gyronoise,gyronoise,gyronoise,gyronoise,gyronoise,gyronoise,g*1e-5,g*1e-5]'; g1=randn(1,time) ; g2=randn(1,time) ; g3=randn(1,time) ; g4=randn(1,time) ; g5=randn(1,time) ; g6=randn(1,time) ; g7=randn(1,time) ; g8=randn(1,time) ; g9=randn(1,time) ; % attitude change matrix cbn(1,1)=cos(zong)*cos(hang)+sin(zong)*sin(heng)*sin(hang); cbn(1,2)=-cos(zong)*sin(hang)+sin(zong)*sin(heng)*cos(hang); cbn(1,3)=-sin(zong)*cos(heng); cbn(2,1)= cos(heng)*sin(hang); cbn(2,2)=cos(heng)*cos(hang); cbn(2,3)=sin(heng); cbn(3,1)= sin(zong)*cos(hang)-cos(zong)*sin(heng)*sin(hang); cbn(3,2)=-sin(zong)*sin(hang)-cos(zong)*sin(heng)*cos(hang); cbn(3,3)=cos(zong)*cos(heng); %位置矩阵的初始化 《捷联惯导系统》P46 其中游动方位角 a＝0 假使初始经纬度确知 Cne(1,1) = - sin(longitude0); Cne(1,2) = cos(longitude0); Cne(1,3) = 0; Cne(2,1) = - sin(latitude0) * cos(longitude0); Cne(2,2) = - sin(latitude0) * sin(longitude0); Cne(2,3) = cos(latitude0); Cne(3,1) = cos(latitude0) * cos(longitude0); Cne(3,2) = cos(latitude0) * sin(longitude0); Cne(3,3) = sin(latitude0); Cen=Cne'; %--------计算在姿态矩阵和位置矩阵更新时用到的参数------------------ Rm=R*(1.0-2.0*e+3.0*e*Cne(3,3)^2); %《捷联惯导系统》 P233 P235 Rn=R*(1.0+e*Cne(3,3)^2); %初始四元数的确定 《捷联惯导系统》 P151-152 方法本身保证了q1^2+q2^2+q3^2+q4^2=1 q(2,1) = sqrt(abs(1.0 + cbn(1,1) - cbn(2,2) - cbn(3,3))) / 2.0; q(3,1) = sqrt(abs(1.0 - cbn(1,1) + cbn(2,2) - cbn(3,3))) / 2.0; q(4,1) = sqrt(abs(1.0 - cbn(1,1) - cbn(2,2) + cbn(3,3))) / 2.0; q(1,1) = sqrt(abs(1.0 - q(2,1) ^2 - q(3,1) ^2 - q(4,1) ^2)); % 判断q(1,1)的符号 flag_q11=cos(head_err0/2.0)*cos(pitch_err0/2.0)*cos(roll_err0/2.0)-sin(head_err0/2.0)*sin(pitch_err0/2.0)*sin(roll_err0/2.0); if (flag_q11 >0) %此时q(1,1)取正 if (cbn(3,2) < cbn(2,3)) q(2,1) = - q(2,1); end if (cbn(1,3) < cbn(3,1)) q(3,1) = - q(3,1); end if (cbn(2,1) < cbn(1,2)) q(4,1) = - q(4,1); end else %此时q(1,1)取负 或0 q(1,1) = - q(1,1); if (cbn(3,2) > cbn(2,3)) q(2,1) = - q(2,1); end if (cbn(1,3) > cbn(3,1)) q(3,1) = - q(3,1); end if (cbn(2,1) > cbn(1,2)) q(4,1) = - q(4,1); end end %-------------------------迭代推算用到的参数的初始化------------------------ Wiee_e = 0; Wiee_n = 0; Wiee_u = omega; Wiee = [Wiee_e Wiee_n Wiee_u]'; %地球速率在地球系中的投影 东－北－天 Lat_err(1)=latitude0; Lon_err(1)=longitude0; %High_err(1)=High0; Ve_err(1)=Ve0; Vn_err(1)=Vn0; Vu_err(1)=Vu0; pitch_err(1)=pitch_err0; head_err(1)=head_err0; roll_err(1)=roll_err0; h=0; F(1,2)= omega*sin(latitude0)+vx*tan(latitude0)/(Rn+h); F(1,3)= -(omega*cos(latitude0)+vx/(Rn+h)); F(1,5)= -1/(Rm+h); F(2,1)=-(omega*sin(latitude0)+vx*tan(latitude0)/(Rn+h)); F(2,3)=-vy/(Rm+h); F(2,4)=1/(Rn+h); F(2,7)=-omega*sin(latitude0); F(3,1)=omega*cos(latitude0)+vx/(Rn+h); F(3,2)=vy/(Rm+h); F(3,4)=tan(latitude0)/(Rn+h); F(3,7)=omega*cos(latitude0)+sec(latitude0)^2*vx/(Rn+h); F(4,2)=-fb_z(1); F(4,3)=fb_y(1); F(4,4)=(vy*tan(latitude0)/(Rm+h)-vz/(Rm+h)); F(4,5)=2*omega*sin(latitude0)+vx*tan(latitude0)/(Rn+h); F(4,6)=-(2*omega*cos(latitude0)+vx*tan(latitude0)/(Rn+h)); F(4,7)=2*vy*omega*cos(latitude0)+vx*vy*sec(latitude0)^2/(Rn+h)+2*omega*sin(latitude0)*vz; F(5,1)=fb_z(1); F(5,3)=-fb_x(1); F(5,4)=-2*(omega*sin(latitude0)+vx*tan(latitude0)/(Rn+h)); F(5,5)=-vz/(Rm+h); F(5,6)=-vy/(Rm+h); F(5,7)=-(2*omega*cos(latitude0)+vx*sec(latitude0)^2/(Rn+h))*vx; F(6,1)=-fb_y(1); F(6,2)=fb_x(1); F(6,4)=2*(omega*cos(latitude0)+vx/(Rn+h)); F(6,5)=2*vy/(Rm+h); F(6,7)=-2*vx*omega*sin(latitude0); F(7,5)=1/(Rm+h); F(8,4)=sec(latitude0)/(Rn+h); F(8,7)=vx*sec(latitude0)*tan(latitude0)/(Rn+h); F(9,6)=1; for i=1:3 for j=1:3 F(i,9+j)=cbn(i,j); F(3+i,12+j)=cbn(i,j); end end G=[cbn(1,1),cbn(1,2),cbn(1,3),0,0,0; cbn(2,1),cbn(2,2),cbn(2,3),0,0,0; cbn(3,1),cbn(3,2),cbn(3,3),0,0,0; 0,0,0,cbn(1,1),cbn(1,2),cbn(1,3); 0,0,0,cbn(2,1),cbn(2,2),cbn(2,3); 0,0,0,cbn(3,1),cbn(3,2),cbn(3,3); 0,0,0,0,0,0; 0,0,0,0,0,0; 0,0,0,0,0,0; 0,0,0,0,0,0; 0,0,0,0,0,0; 0,0,0,0,0,0; 0,0,0,0,0,0; 0,0,0,0,0,0; 0,0,0,0,0,0]; % p=diag([1,1,1,0.01,0.01,0.01,1,1,2,100,100,100,0.00000025,0.00000025,0.00000025]); [A,B]=c2d(F,G,T); rk1=[0,0,0]'; qq=[0,0,0]'; %自适应参数定义 rr1=zeros(3,m); rr11=zeros(3,1); R1=zeros(m,3,3); R11=zeros(3,3); Bq1=zeros(m,15); Bq11=zeros(15,1); BQ1B1=zeros(m,15,15); BQ1B11=zeros(15,15); % R1j=zeros(3,3); % BQ1B1j=zeros(15,15); for i=1:time w(1,1)=gyronoise*g1(1,i); w(2,1)=gyronoise*g2(1,i); w(3,1)=gyronoise*g3(1,i); w(4,1)=(0.5*g*1e-5)*g4(1,i); w(5,1)=(0.5*g*1e-5)*g