资源描述：
《捷联惯导的解算程序》由会员上传分享，免费在线阅读，更多相关内容在行业资料-天天文库。
1、%====本程序为捷联惯导的解算程序(由惯性器件的输出解算出飞行器的位置、速度、姿态信息)======clearall;closeall;clc;deg_rad=pi/180;%由度转化成弧度rad_deg=180/pi;%由弧度转化成度%-------------------------------从源文件中读入数据----------------------------------fid_read=fopen('IMUout.txt','r');%path1_Den.dat是由轨迹发生器产生
2、的数据[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=-All
3、Data(:,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=A
4、llData(:,12);%滚转角（向右为正）omigax=AllData(:,15);%陀螺输出（单位：弧度/秒，坐标轴的定义与比力的相同）omigay=AllData(:,14);omigaz=-AllData(:,16);%-------------------------------程序初始化--------------------------------------latitude0=latitude(1);longitude0=longitude(1);%初始位置High0=High
5、(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;%------------------------------
6、导航解算开始--------------------------------------%假设没有初始对准误差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(r
7、oll_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);Tb
8、n(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';%位置矩阵的初始
