捷联惯导的姿态解算matlab.docVIP

%====本程序为捷联惯导的解算程序(由惯性器件的输出解算出飞行器的位置、速度、姿态信息)======?? clear?all;?? close?all;? clc;?? deg_rad=pi/180;???%由度转化成弧度? ?rad_deg=180/pi;???%由弧度转化成度?? %-------------------------------从源文件中读入数据----------------------------------?? fid_read=fopen(IMUout.txt,r);????%path1_Den.dat??是由轨迹发生器产生的数据???? [AllData?NumofAllData]=fscanf(fid_read,%g?%g?%g?%g?%g?%g?%g?%g?%g?%g?%g?%g?%g?%g?%g?%g,[17?inf]);? ?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轴构成右手坐标系?单位：米/秒2?? fb_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