捷联惯导的解算程序

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

else %此时q(1,1)取负 或0 q(1,1) = - 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(1,2)) q(4,1) = - q(4,1); end end

%-------------------------迭代推算用到的参数的初始化------------------------ 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)=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;c78245.0; %地球长轴 《惯性导航系统》 P28 e=0.0033528106647474807198455286185206; %地球扁率精确值 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,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*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 东－北－天

% 速度的更新 《捷联惯导系统》 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))*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)*Cne(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)+TimeEach*(-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_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(i+1)=asin(Cne(3,3));

Lon_err(i+1)=atan(Cne(3,2)/Cne(3,1)); %这是经度的主值 if (Cne(3,1) < 0) if (Lon_err(i+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), 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(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(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,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_roll<0)

flag_roll=flag_roll+pi; end

if(flag_roll>0)

flag_roll=flag_roll-pi; end end

% 偏航角范围 -180度——180度 北偏西为正 if(Tnb(2,2)<0)

if(flag_head<0)

flag_head=flag_head+pi; end

if(flag_head>0)

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(i)-High(i); %高度误差 单位：米

ddVe(i)=Ve_err(i)-Ve(i);

ddVn(i)=Vn_err(i)-Vn(i); % 速度误差 单位：米/妙2 ddVu(i)=Vu_err(i)-Vu(i);

ddpitch(i)=(pitch_err(i)-pitch(i))*rad_deg*3600; %姿态误差 单位：度 ddhead(i)=(head_err(i)-head(i))*rad_deg*3600; ddroll(i)=(roll_err(i)-roll(i))*rad_deg*3600; end

fclose(fid_read);

%---------------------------绘图开始--------------------------------- figure(1)

plot(Time,ddLog)

ylabel('经度误差(度)'),xlabel('时间(秒)'); figure(2)

plot(Time,ddLat)

ylabel('纬度误差(度)'),xlabel('时间(秒)'); figure(3)

plot(Time,ddHigh);

ylabel('高度误差(米)'),xlabel('时间(秒)'); figure(4)

plot(Time,ddhead)

ylabel('偏航角误差(角妙)'),xlabel('时间(秒)'); figure(5)

plot(Time,ddpitch)

ylabel('俯仰角误差(角妙)'),xlabel('时间(秒'); figure(6)

plot(Time,ddroll);

ylabel('滚转角误差(角妙)'),xlabel('时间(秒)'); figure(7)

plot(Time,ddVe);

ylabel('东向速度误差(米/秒)'),xlabel('时间(秒)'); figure(8)

plot(Time,ddVn)

ylabel('北向速度误差(米/秒)'),xlabel('时间(秒)'); figure(9)

plot(Time,ddVu)

ylabel('天向速度误差(米/秒)'),xlabel('时间(秒)');

%------------------------------绘图结束-------------------------------

本文来源：https://www.bwwdw.com/article/okg2.html