捷联惯导的解算程序

%====本程序为捷联惯导的解算程序(由惯性器件的输出解算出飞行器的位置、速度、姿态信息)======clearall;closeall;clc;deg_rad=pi/180;%由度转化成弧度rad_deg=180/pi;%由弧度转化成度%-------------------------------从源文件中读入数据----------------------------------fid_read=fopen('IMUout.txt','r');%path1_Den.dat是由轨迹发生器产生的数据[AllDataNumofAllData]=fscanf(fid_read,'%g%g%g%g%g%g%g%g%g%g%g%g%g%g%g%g',[17inf]);AllData=AllData';NumofEachData=round(NumofAllData/17);Time=AllData(:,1);longitude=AllData(:,2);%经度单位：弧度latitude=AllData(:,3);%纬度单位：弧度High=AllData(:,4);%高度单位：米Ve=-AllData(:,6);%东向、北向、天向速度单位：米/妙Vn=AllData(:,5);Vu=AllData(:,7);fb_x=AllData(:,9);%比力（fx，fy，fz）fb_y=AllData(:,8);%指向右机翼方向为x正方向，指向机头方向为y正向，z轴与x轴和y轴构成右手坐标系单位：米/秒2fb_z=-AllData(:,10);%右前上pitch=AllData(:,11);%俯仰角（向上为正）单位:弧度head=-AllData(:,13);%偏航角（偏西为正）roll=AllData(:,12);%滚转角（向右为正）omigax=AllData(:,15);%陀螺输出（单位：弧度/秒，坐标轴的定义与比力的相同）omigay=AllData(:,14);omigaz=-AllData(:,16);%-------------------------------程序初始化--------------------------------------latitude0=latitude(1);longitude0=longitude(1);%初始位置High0=High(1);Ve0=Ve(1);Vn0=Vn(1);%初始速度Vu0=Vu(1);pitch0=pitch(1);head0=head(1);%初始姿态roll0=roll(1);TimeEach=0.005;%周期和仿真总时间TimeAll=(NumofEachData-1)*TimeEach;Omega_ie=0.7292115147E-4;%0.00007272205216643040;%地球自转角速度单位：弧度每妙g0=9.78;%------------------------------导航解算开始--------------------------------------%假设没有初始对准误差pitch_err0=pitch0+0*deg_rad;head_err0=head0+0*deg_rad;roll_err0=roll0+0*deg_rad;%初始捷联矩阵的计算《捷联惯导系统》P63旋转顺序head-pitch-roll%导航坐标系n为东北天方向载体坐标系b为右前上偏航角北偏西为正Tbn(1,1)=cos(roll_err0)*cos(head_err0)-sin(roll_err0)*sin(pitch_err0)*sin(head_err0);Tbn(1,2)=cos(roll_err0)*sin(head_err0)+sin(roll_err0)*sin(pitch_err0)*cos(head_err0);Tbn(1,3)=-sin(roll_err0)*cos(pitch_err0);Tbn(2,1)=-cos(pitch_err0)*sin(head_err0);Tbn(2,2)=cos(pitch_err0)*cos(head_err0);Tbn(2,3)=sin(pitch_err0);Tbn(3,1)=sin(roll_err0)*cos(head_err0)+cos(roll_err0)*sin(pitch_err0)*sin(head_err0);Tbn(3,2)=sin(roll_err0)*sin(head_err0)-cos(roll_err0)*sin(pitch_err0)*cos(head_err0);Tbn(3,3)=cos(roll_err0)*cos(pitch_err0);Tnb=Tbn';%位置矩阵的初始化《捷联惯导系统》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';%初始四元数的确定《捷联惯导系统》P151-152方法本身保证了q1^2+q2^2+q3^2+q4^2=1q(2,1)=sqrt(abs(1.0+Tnb(1,1)-Tnb(2,2)-Tnb(3,3)))/2.0;q(3,1)=sqrt(abs(1.0-Tnb(1,1)+Tnb(2,2)-Tnb(3,3)))/2.0;q(4,1)=sqrt(abs(1.0-Tnb(1,1)-Tnb(2,2)+Tnb(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_q110)%此时q(1,1)取正if(Tnb(3,2)Tnb(2,3))q(2,1)=-q(2,1);endif(Tnb(1,3)Tnb(3,1))q(3,1)=-q(3,1);endif(Tnb(2,1)Tnb(1,2))q(4,1)=-q(4,1);endelse%此时q(1,1)取负或0q(1,1)=-q(1,1);if(Tnb(3,2)Tnb(2,3))q(2,1)=-q(2,1);endif(Tnb(1,3)Tnb(3,1))q(3,1)=-q(3,1);endif(Tnb(2,1)Tnb(1,2))q(4,1)=-q(4,1);endend%-------------------------迭代推算用到的参数的初始化------------------------Wiee_e=0;Wiee_n=0;Wiee_u=Omega_ie;Wiee=[Wiee_eWiee_nWiee_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;Re=6378137.0;%6378245.0;%地球长轴《惯性导航系统》P28e=0.0033528106647474807198455286185206;%地球扁率精确值ee=0.00669437999014131699614;%----------------------------迭代推算开始-----------------------------------fori=1:NumofEachData%----------------------------惯性仪表数据的获得------------------------Wibb(1,1)=omigax(i);%指向右机翼方向为x正方向，指向机头方向为y正向，z轴与x轴和y轴构成右手坐标系Wibb(2,1)=omigay(i);%单位：弧度/妙Wibb(3,1)=omigaz(i);%右前上fb(1,1)=fb_x(i);%指向右机翼方向为x正方向，指向机头方向为y正向，z轴与x轴和y轴构成右手坐标系fb(2,1)=fb_y(i);%单位：米/秒2fb(3,1)=fb_z(i);%右前上%--------计算在姿态矩阵和位置矩阵更新时用到的参数------------------RM=Re*(1.0-2.0*e+3.0*e*Cne(3,3)^2)+High_err(i);%《捷联惯导系统》P233P235RN=Re*(1.0+e*Cne(3,3)^2)+High_err(i);%RN=Re*(1-ee)/(sqrt(1-ee*sin(Lat_err(i))))^3+High_err(i);%RM=Re/sqrt(1-ee*sin(Lat_err(i)))+High_err(i);%实验当地重力加速度计算《捷联惯导系统》P150《惯性导航系统》P35g=g0*((1.0+0.0052884*Cne(3,3)^2)-0.0000059*(1-(1-2*Cne(3,3)^2)^2))*(1.0-2.0*High_err(i)/Re);tmp_slat=sin(Lat_err(i))*sin(Lat_err(i));Wien=Cne*Wiee;%地球速率在导航系中的投影Wenn(1,1)=-Vn_err(i)/RM;Wenn(2,1)=Ve_err(i)/RN;%惯性导航系统P45考虑了地球转动的影响.Wenn(3,1)=Ve_err(i)*tan(Lat_err(i))/RN;%计算Wenn(不太精确)，更新速度和位置矩阵时用Winn=Wien+Wenn;Winb=Tbn*Winn;Wnbb=Wibb-Winb;%姿态速率在姿态更新时用到fn=Tnb*fb;%x-y-z东－北－天%速度的更新《捷联惯导系统》P3033东－北－天difVe_err=fn(1,1)+(2*Wien(3,1)+Wenn(3,1))*Vn_err(i)-(2*Wien(2,1)+Wenn(2,1))*Vu_err(i);difVn_err=fn(2,1)-(2*Wien(3,1)+Wenn(3,1))*Ve_err(i)+(2*Wien(1,1)+Wenn(1,1))*Vu_err(i);difVu_err=fn(3,1)+(2*Wien(2,1)+Wenn(2,1))*Ve_err(i)-(2*Wien(1,1)+Wenn(1,1))*Vn_err(i)-g;Ve_err(i+1)=Ve_err(i)+difVe_err*TimeEach;Vn_err(i+1)=Vn_err(i)+difVn_err*TimeEach;Vu_err(i+1)=Vu_err(i)+difVu_err*TimeEach;High_err(i+1)=High_err(i)+Vu_err(i)*TimeEach;%位置矩阵的实时更新《惯性导航系统》P190Cne(1,1)=Cne(1,1)+TimeEach*(Wenn(3,1)*Cne(2,1)-Wenn(2,1)*Cne(3,1));Cn