《捷联惯导的解算程序》由会员分享，可在线阅读，更多相关《捷联惯导的解算程序（8页珍藏版）》请在金锄头文库上搜索。
1、%=本程序为捷联惯导的解算程序(由惯性器件的输出解算出飞行器的位置、速 度、姿态信息)=clear all;close all;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,17 inf);AllData=AllData; NumofE
2、achData=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轴构成右手坐标
3、系 单位：米/秒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);
4、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
5、_err0=roll0+0*deg_rad;%初始捷联矩阵的计算捷联惯导系统P63旋转顺序head-pi tch - 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(pi
6、tch_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)=c
7、os(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(longit
8、ude0);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,l)=sqr t( abs(1.0+Tnb(l,l)-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(l,l)
9、=sqr t(abs(1.0-q(2,1) 2 - q(3,1) 2- q(4,1) ”2);%判断q(l,l)的符号 flag_q11=cos(head_err0/2.0)*cos(pitch_err0/2.0)*cos(roll_err0/2.0)-si n(head_err0/2.0)*sin(pitch_err0/2.0)*sin(roll_err0/2.0);if (flag_qll 0)%此时 q(l,l)取正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
10、 (Tnb(2,1) 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_e Wiee_n Wiee_u; %地球速率在地球系中的投影 东北天 Lat_err(1)=latitude0;Lon_err(1)=longitude0;High_err(1)=High0;Ve_err(1
11、)=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; %地球长轴 惯性导航系统 P28 e=0.0033528106647474807198455286185206; %地球扁率精确值 ee=0.00669437999014131699614;%迭代推算开始for i=1:NumofEachData%惯性仪表数据的获得Wibb(l,l)=omigax(i); %指向右机翼方向为x正方向，指向机头方
12、向为y正向, z轴与x轴和y轴构成右手坐标系Wibb(2,1)=omigay(i); %单位：弧度/妙 Wibb(3,1)=omigaz(i); %右前上fb(l,l)=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);% R
13、N二Re *(l-ee)/(sqr t( 1-ee *sin(La t_err(i)厂3+High_err(i);% RM=Re/sqrt(1-ee*sin(Lat_err(i)+High_err(i);%实验当地重力加速度计算捷联惯导系统P150惯性导航系统P35 g=g0*(1.0+0.0052884*Cne(3,3厂2)-0.0000059*(*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) = -
14、Vn_err(i)/RM;Wenn(2,1) = Ve_err(i)/RN; % P45 考虑了地球 转动的影响.Wenn(3,l) = Ve_err(i) *tan(Lat_err(i)/RN; %计算 Wenn(不太精确),更新 速度和位置矩阵时用Winn=Wien+Wenn;Winb=Tbn*Winn;Wnbb=Wibb-Winb;%姿态速率 在姿态更新时用到fn=Tnb*fb; % x-y-z 东北天% 速度的更新 捷联惯导系统 P30 33 东北天difVe_err=fn(1,1)+(2*Wien(3,1)+Wenn(3,1)*Vn_err(i)-(2*Wien(2,1)+Wenn(
15、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*T
