📄 navigation.m
字号:
%%%%%%%%%%%%%导航解算程序北天东%%%%%%%%%%%%%%
clear all
close all
clc
deg_rad=pi/180; %由度转化成弧度
rad_deg=180/pi; %由弧度转化成度
%-----------------------常值定义区----------------------
Omega_ie=0.00007272205216643040; %地球自转角速度 单位：弧度每妙
g0=9.78;
Wiee = [0 0 Omega_ie]'; %地球速率在地球系中的投影
Re=6378245.0; %地球长轴 《惯性导航系统》 P28
e=1/298.3;
%-------------------从文件中读取数据----------------------
fid_IMUout=fopen('IMUout.txt','r'); %读取惯性器件的输出 无误差
[AllData NumofAllData]=fscanf(fid_IMUout,'%g %g %g %g %g %g %g %g %g %g %g %g %g %g %g %g %g',[17 inf]);
AllData=AllData';
NumofEachData=fix(NumofAllData/17);
%惯性器件误差的设定 惯性器件误差由常值误差和随机漂移误差组成
delta_ax=1e-4*g0+0.5e-4*randn(NumofEachData,1)*g0; %加速度计误差为常值1e-4g,随机偏差为0.5e-4g
delta_ay=1e-4*g0+0.5e-4*randn(NumofEachData,1)*g0;
delta_az=1e-4*g0+0.5e-4*randn(NumofEachData,1)*g0;
epslon_x=0.2*pi/180/3600+0.2*pi/180/3600*randn(NumofEachData,1); %陀螺仪的误差常值为0.2度/小时,
epslon_y=0.2*pi/180/3600+0.2*pi/180/3600*randn(NumofEachData,1); %随机漂移为0.2度/小时
epslon_z=0.2*pi/180/3600+0.2*pi/180/3600*randn(NumofEachData,1);
TimeSINS=AllData(:,1);
longitude=AllData(:,2); %经度 单位：弧度
latitude=AllData(:,3); %纬度 单位：弧度
High=AllData(:,4); %高度 单位：米
Vn=AllData(:,5);
Vu=AllData(:,7);
Ve=-AllData(:,6);
fb_x=AllData(:,8); %比力
fb_y=-AllData(:,10); %
fb_z=AllData(:,9); %
pitch=AllData(:,11); %俯仰角（向上为正）
roll=AllData(:,12); %滚转角（向右为正）
head=-AllData(:,13); %偏航角（偏西为正）
omigax=AllData(:,14); %陀螺输出
omigay=-AllData(:,16);
omigaz=AllData(:,15);
fb_x=fb_x+delta_ax;
fb_y=fb_y+delta_ay;
fb_z=fb_z+delta_az; %加表加入误差
omigax=omigax+epslon_x;
omigay=omigay+epslon_y;
omigaz=omigaz+epslon_z; %陀螺加入误差
%%%%%%%%%%%%输出导航解算结果文件%%%%%%%%%%%%
fid_Nav=fopen('Navigation.txt','w');
%%%%%%%%%%%%导航初始化%%%%%%%%%%%%%%%
T_SINS=TimeSINS(2)-TimeSINS(1); %惯导程序的解算周期
TimeAll=(NumofEachData-1)*T_SINS;
latitude0=latitude(1);
longitude0=longitude(1); %初始位置
High0=High(1);
Vn0=Vn(1); %初始速度
Vu0=Vu(1);
Ve0=Ve(1);
pitch0=pitch(1);
head0=head(1); %初始姿态
roll0=roll(1);
%初始对准误差设置
pitch_err0=pitch0+1*deg_rad;
head_err0=head0+1*deg_rad;
roll_err0=roll0+1*deg_rad;
%初始捷联矩阵的计算
Tbn(1,1)=cos(pitch_err0)*cos(head_err0); %地理坐标系 北天东
Tbn(1,2)=sin(pitch_err0); %载体坐标系 前上右
Tbn(1,3)=-cos(pitch_err0)*sin(head_err0); %旋转顺序 head-pitch-roll
Tbn(2,1)=sin(roll_err0)*sin(head_err0)-sin(pitch_err0)*cos(roll_err0)*cos(head_err0);
Tbn(2,2)=cos(roll_err0)*cos(pitch_err0);
Tbn(2,3)=cos(roll_err0)*sin(head_err0)*sin(pitch_err0)+sin(roll_err0)*cos(head_err0);
Tbn(3,1)=cos(roll_err0)*sin(head_err0)+sin(roll_err0)*sin(pitch_err0)*cos(head_err0);
Tbn(3,2)=-sin(roll_err0)*cos(pitch_err0);
Tbn(3,3)=-sin(roll_err0)*sin(head_err0)*sin(pitch_err0)+cos(roll_err0)*cos(head_err0);
Tnb=Tbn';
%位置矩阵的初始化
Cne(1,1) = - sin(latitude(1)) * cos(longitude(1));
Cne(1,2) = - sin(latitude(1)) * sin(longitude(1)); %地理坐标系：北天东方向
Cne(1,3) = cos(latitude(1));
Cne(2,1) = cos(latitude(1)) * cos(longitude(1));
Cne(2,2) = cos(latitude(1)) * sin(longitude(1));
Cne(2,3) = sin(latitude(1));
Cne(3,1) = - sin(longitude(1));
Cne(3,2) = cos(longitude(1));
Cne(3,3) = 0;
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
%递推初始化
Lat_err(1)=latitude0;
Lon_err(1)=longitude0; %无初始位置误差
High_err(1)=High0;
Vn_err(1)=Vn0;
Vu_err(1)=Vu0; %无初始速度误差
Ve_err(1)=Ve0;
pitch_err(1)=pitch_err0;
head_err(1)=head_err0; %初始姿态误差
roll_err(1)=roll_err0;
%%%%%%%%%%%%导航解算开始%%%%%%%%%%%%%%%%%
for i=1:NumofEachData
Wibb(1,1)=omigax(i);
Wibb(2,1)=omigay(i);
Wibb(3,1)=omigaz(i);
fb(1,1)=fb_x(i);
fb(2,1)=fb_y(i);
fb(3,1)=fb_z(i);
%计算在姿态矩阵和位置矩阵更新时用到的参数
RM=Re*(1.0-2.0*e+3.0*e*Cne(2,3)^2)+High_err(i);
RN=Re*(1.0+e*Cne(2,3)^2)+High_err(i);
%实验当地重力加速度计算 《捷联惯导系统》P150 《惯性导航系统》 P35
%g=g0*((1.0+0.0052884*Cne(2,3)^2)-0.0000059*(1-(1-2*Cne(2,3)^2)^2))*(1.0-2.0*High_err(i+(ii-1)*NUM)/Re);
g=g0*((1.0+0.0052884*Cne(2,3)^2)-0.0000059*4*Cne(2,3)^2*(1-Cne(2,3)^2))*(1.0-2.0*High_err(i)/Re);
Wien = Cne * Wiee; %地球速率在导航系中的投影
Wenn(1,1) = Ve_err(i)/RN;
Wenn(2,1) = Ve_err(i)*tan(Lat_err(i))/RN;
Wenn(3,1) = -Vn_err(i)/RM;
Winn=Wien+Wenn;
Winb=Tbn*Winn;
Wnbb=Wibb-Winb; %姿态速率 在姿态更新时用到
fn=Tnb*fb;
% 速度的更新
difVn_err=fn(1,1)+(2*Wien(3,1)+Wenn(3,1))*Vu_err(i)-(2*Wien(2,1)+Wenn(2,1))*Ve_err(i);
difVu_err=fn(2,1)-(2*Wien(3,1)+Wenn(3,1))*Vn_err(i)+(2*Wien(1,1)+Wenn(1,1))*Ve_err(i)-g;
difVe_err=fn(3,1)+(2*Wien(2,1)+Wenn(2,1))*Vn_err(i)-(2*Wien(1,1)+Wenn(1,1))*Vu_err(i);
Vn_err(i+1)=Vn_err(i)+difVn_err*T_SINS;
Vu_err(i+1)=Vu_err(i)+difVu_err*T_SINS;
Ve_err(i+1)=Ve_err(i)+difVe_err*T_SINS;
High_err(i+1)=High_err(i)+Vu_err(i)*T_SINS;
% 位置矩阵的实时更新 《惯性导航系统》 P190
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*T_SINS;
Cne=Cne+Mat_Wenn;
Cen=Cne';
%计算经纬度
Lat_err(i+1)=asin(Cne(2,3));
Lon_err(i+1)=atan(Cne(2,2)/Cne(2,1)); %这是经度的主值
if (Cne(2,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*T_SINS/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(2,1));
flag_roll=atan(-Tnb(2,3)/Tnb(2,2));
flag_head=atan(-Tnb(3,1)/Tnb(1,1));
if(Tnb(2,2)<0)
if(flag_roll<0)
flag_roll=flag_roll+pi;
else
flag_roll=flag_roll-pi;
end
end
% 偏航角范围 -180度——180度 北偏西为正
if(Tnb(1,1)<0)
if(flag_head<0)
flag_head=flag_head+pi;
else
flag_head=flag_head-pi;
end
end
% 姿态角更新
head_err(i+1)=flag_head;
pitch_err(i+1)=flag_pitch;
roll_err(i+1)=flag_roll;
%----------------------计算解算误差------------------
ddpitch(i)=(pitch_err(i)-pitch(i))*rad_deg*60;
ddhead(i)=(head_err(i)-head(i))*rad_deg*60;
ddroll(i)=(roll_err(i)-roll(i))*rad_deg*60; %角分
ddLat(i)=(Lat_err(i)-latitude(i)); %单位：弧度
ddLon(i)=(Lon_err(i)-longitude(i));
ddHigh(i)=High_err(i)-High(i); %高度误差 单位：米
ddVn(i)=Vn_err(i)-Vn(i); % 速度误差 单位：米/妙2
ddVu(i)=Vu_err(i)-Vu(i);
ddVe(i)=Ve_err(i)-Ve(i);
fprintf(fid_Nav,'%g %g %g %g %g %g %g %g %g %g %g %g %g %g %g %g \n',TimeSINS(i),...
Lon_err(i), Lat_err(i),High_err(i),Vn_err(i),Vu_err(i),Ve_err(i),pitch_err(i),roll_err(i),head_err(i),...
omigax(i), omigay(i),omigaz(i),fb_x(i),fb_y(i),fb_z(i)); %导航解算结果，加表和陀螺的输出是加入误差之后的
end;
fclose(fid_IMUout);
fclose(fid_Nav);
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -