捷联惯导的解算程序

1、%=本程序为捷联惯导的解算程序(由惯性器件的输出解算出飞行器的位置、速度、姿态信息)= 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=alldat

2、a; 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正方向，指向机头方向为

3、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=longitu

4、de(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

5、*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)*

6、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

7、_err0)*sin(pitch_err0)*cos(head_err0); tbn(3,3)=cos(roll_err0)*cos(pitch_err0); tnb=tbn; %位置矩阵的初始化 捷联惯导系统p46 其中游动方位角 a0 假使初始经纬度确知 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(

8、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 方法本身保证了q12+q22+q32+q42=1 q(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

9、(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_q11 0) %

10、此时q(1,1)取正 if (tnb(3,2) tnb(2,3) q(2,1) = - q(2,1); end if (tnb(1,3) tnb(3,1) q(3,1) = - q(3,1); end if (tnb(2,1) tnb(2,3) q(2,1) = - q(2,1); end if (tnb(1,3) tnb(3,1) q(3,1) = - q(3,1); end if (tnb(2,1) tnb(1,2) q(4,1) = - q(4,1); end end %-迭代推算用到的参数的初始化- wiee_e = 0; wiee_n = 0; wiee_u = omega_ie;

11、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; re=6378137.0;%6378245.0; %地球长轴 惯性导航系统 p28 e=0.003352810664747480719845528

12、6185206; %地球扁率精确值 ee=0.00669437999014131699614; %-迭代推算开始- for i=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); %单位：米/秒2 fb(3,

13、1)=fb_z(i); %右前上 %-计算在姿态矩阵和位置矩阵更新时用到的参数- rm=re*(1.0-2.0*e+3.0*e*cne(3,3)2)+high_err(i); %捷联惯导系统 p233 p235 rn=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 惯性导航系统 p35 g=g0*(1.0+0.0052884*c

14、ne(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=t

15、bn*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(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)*

16、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; % 位置矩阵的实时更新 惯性导航系统 p190 cne(1,1)=cne(1,1)+timeeach*(wenn(3,1)*cne(2,1)-wenn(2,1)*c

17、ne(3,1); cne(1,2)=cne(1,2)+timeeach*(wenn(3,1)*cne(2,2)-wenn(2,1)*cne(3,2); cne(1,3)=cne(1,3)+timeeach*(wenn(3,1)*cne(2,3)-wenn(2,1)*cne(3,3); cne(2,1)=cne(2,1)+timeeach*(-wenn(3,1)*cne(1,1)+wenn(1,1)*cne(3,1); cne(2,2)=cne(2,2)+timeeach*(-wenn(3,1)*cne(1,2)+wenn(1,1)*cne(3,2); cne(2,3)=cne(2,3)+tim

18、eeach*(-wenn(3,1)*cne(1,3)+wenn(1,1)*cne(3,3); cne(3,1)=cne(3,1)+timeeach*(wenn(2,1)*cne(1,1)-wenn(1,1)*cne(2,1); cne(3,2)=cne(3,2)+timeeach*(wenn(2,1)*cne(1,2)-wenn(1,1)*cne(2,2); cne(3,3)=cne(3,3)+timeeach*(wenn(2,1)*cne(1,3)-wenn(1,1)*cne(2,3); % mat_wenn(1,1)=0; % mat_wenn(1,2)=wenn(3,1); % mat_

19、wenn(1,3)=-wenn(2,1); %wenn的反对阵矩阵取负 % mat_wenn(2,1)=-wenn(3,1); %这里位置矩阵的及时修正为: dcne/dt=mat_wenn*cne % mat_wenn(2,2)=0; % mat_wenn(2,3)=wenn(1,1); % mat_wenn(3,1)=wenn(2,1); % mat_wenn(3,2)=-wenn(1,1); % mat_wenn(3,3)=0; % % mat_wenn=mat_wenn*cne*timeeach; % cne=cne+mat_wenn; cen=cne; % 计算经纬度 lat_err

20、(i+1)=asin(cne(3,3); lon_err(i+1)=atan(cne(3,2)/cne(3,1); %这是经度的主值 if (cne(3,1) 0) lon_err(i+1) = lon_err(i+1) - pi; else lon_err(i+1) = lon_err(i+1) + pi; end end % 四元数的及时修正 惯性导航系统 p194 % mat_wnbb= 0, -wnbb(1,1), -wnbb(2,1), -wnbb(3,1); % wnbb(1,1), 0, wnbb(3,1), -wnbb(2,1); % wnbb(2,1), -wnbb(3,1)

21、, 0, wnbb(1,1); % wnbb(3,1), wnbb(2,1), -wnbb(1,1), 0; % q=q+mat_wnbb*q*timeeach/2.0; q(1,1)=q(1,1)+timeeach*(-wnbb(1,1)*q(2,1)-wnbb(2,1)*q(3,1)-wnbb(3,1)*q(4,1)/2.0; q(2,1)=q(2,1)+timeeach*(wnbb(1,1)*q(1,1)+wnbb(3,1)*q(3,1)-wnbb(2,1)*q(4,1)/2.0; q(3,1)=q(3,1)+timeeach*(wnbb(2,1)*q(1,1)-wnbb(3,1)*q(

22、2,1)+wnbb(1,1)*q(4,1)/2.0; q(4,1)=q(4,1)+timeeach*(wnbb(3,1)*q(1,1)+wnbb(2,1)*q(2,1)-wnbb(1,1)*q(3,1)/2.0; % 四元数归一化处理 q_norm=sqrt(sum(q.*q); q=q/q_norm; % 计算姿态矩阵 tnb tnb(1,1) = q(1,1) 2 + q(2,1) 2 - q(3,1)2 - q(4,1)2; tnb(1,2) = 2.0 * (q(2,1) * q(3,1) - q(1,1) * q(4,1); tnb(1,3) = 2.0 * (q(2,1) * q(

23、4,1) + q(1,1) * q(3,1); tnb(2,1) = 2.0 * (q(2,1) * q(3,1) + q(1,1) * q(4,1); tnb(2,2) = q(1,1)2 - q(2,1)2 + q(3,1)2 - q(4,1)2; tnb(2,3) = 2.0 * (q(3,1) * q(4,1) - q(1,1) * q(2,1); tnb(3,1) = 2.0 * (q(2,1) * q(4,1) - q(1,1) * q(3,1); tnb(3,2) = 2.0 * (q(3,1) * q(4,1) + q(1,1) * q(2,1); tnb(3,3) = q(1

24、,1)2 - q(2,1)2 - q(3,1)2 + q(4,1)2; tbn=tnb; flag_pitch=asin(tnb(3,2); flag_roll=atan(-tnb(3,1)/tnb(3,3); flag_head=atan(-tnb(1,2)/tnb(2,2); if(tnb(3,3)0) if(flag_roll0) flag_roll=flag_roll-pi; end end % 偏航角范围 -180度180度 北偏西为正 if(tnb(2,2)0) if(flag_head0) flag_head=flag_head-pi; end end % 姿态角更新 pitch_err(i+1)=flag_pitch; head_err(i+1)=flag_head; roll_err(i+1)=flag_roll; % 解算完毕 由对准结果、陀螺、加表的输出解算出载体的位置、速度、姿态 %-计算解算误差- ddlat(i)=(lat_err(i)-latitude(i)*rad_deg; %纬度误差 单位：度 ddlog(i)=(lon_err(i)-longitude(i)*rad_deg; %经度误差 单位：度 ddhigh(i)=high_err