function?Navi2018
clc
clear
P=fopen（‘result.txt‘‘wt‘）;
Data?=?load（‘imu.txt‘）;%前三列為加速度信息?后三列為角速度信息?共六列?東-北-天
T=0.005;%慣導遞推周期
f_INS?=?Data（:1:3）;%?加載加速度計數(shù)據(jù)?比力
wib_INS?=?Data（:4:6）;%?加載陀螺數(shù)據(jù)?角速度
L0?=?size（Data1）;%數(shù)據(jù)的行數(shù)
Wie?=?7.292115147e-5;?%>?地球自傳角速度
Re?=?6378245;?%>?地球橢球長半徑
h?=?0;%?>?高度，可更改
e?=?1/298.3;%偏心率
%>?重力加速度計算參數(shù)
g0?=?9.7803267714;
gk1?=?0.00193185138639;
gk2?=?0.00669437999013;
Vx?=?zeros（1L0）;Vy?=?zeros（1L0）;Vz?=?zeros（1L0）;
Gama?=?zeros（1L0）;L?=?zeros（1L0）;Roll?=?zeros（1L0）;Pitch?=?zeros（1L0）;Yaw?=?zeros（1L0）;
%>?初始經緯度
%Gama（1）?=?119.2610283*pi/180;%?>?初始經度（弧度）
%L（1）?=22.1197033*pi/180;%?>?初始緯度（弧度）
Gama（1）?=0*pi/180;%?>?初始經度（弧度），可更改
L（1）?=0*pi/180;%?>?初始緯度（弧度）
%>?初始姿態(tài)角
Roll（1）?=0*pi/180;?%>?橫滾角（弧度），可更改
Pitch（1）?=0*pi/180;?%>?俯仰角（弧度）
Yaw（1）?=90*pi/180;%?>?航向角（弧度）
%>?初始速度
Vx（1）?=0;?%>?x軸向速度，可更改
Vy（1）?=0;%?>?y軸向速度
Vz（1）?=0;?%>?z軸向速度
%>?四元素初始值%%%西北工業(yè)大學版《導航理論與應用》P95
e0?=?cos（0.5*Yaw（1））*cos（0.5*Roll（1））*cos（0.5*Pitch（1））+sin（0.5*Yaw（1））*sin（0.5*Roll（1））*sin（0.5*Pitch（1））;
e1?=?cos（0.5*Yaw（1））*sin（0.5*Roll（1））*cos（0.5*Pitch（1））+sin（0.5*Yaw（1））*cos（0.5*Roll（1））*sin（0.5*Pitch（1））;
e2?=?cos（0.5*Yaw（1））*cos（0.5*Roll（1））*sin（0.5*Pitch（1））-sin（0.5*Yaw（1））*sin（0.5*Roll（1））*cos（0.5*Pitch（1））;
e3?=?-sin（0.5*Yaw（1））*cos（0.5*Roll（1））*cos（0.5*Pitch（1））+cos（0.5*Yaw（1））*sin（0.5*Roll（1））*sin（0.5*Pitch（1））;
Ctb?=?[e0^2+e1^2-e2^2-e3^2?2*（e1*e2+e0*e3）?2*（e1*e3-e0*e2）;?%>?用四元素表示得姿態(tài)矩陣
?2*（e1*e2-e0*e3）?e0^2-e1^2+e2^2-e3^2?2*（e2*e3+e0*e1）;
?2*（e1*e3+e0*e2）?2*（e2*e3-e0*e1）?e0^2-e1^2-e2^2+e3^2];
E?=?[e0?e1?e2?e3]‘;%>?四元素的四個元素值
for?i?=?1:L0
????f_INSc?=?f_INS（i:）‘;
????wib_INSc?=?wib_INS（i:）‘;
????Rm（i）?=?Re*（1-2*e+3*e*（sin（L（i）））^2）;%?>?計算子午圈主曲率半徑
????Rn（i）?=?Re*（1+e*（sin（L（i）））^2）;?%>?計算卯酉圈主曲率半徑
????g?=?g0*（1+gk1*（sin（L（i）））^2）*（1-2*h/Re）/sqrt（1-gk2*（sin（L（i）））^2）;%?>?重力加速度計算
????Cbt?=?Ctb‘;
????Wien=[0?Wie*cos（L（i））?Wie*sin（L（i））];
????f_t?=?Cbt*f_INSc;%?>?將體軸系中的比力轉化到地理系
????
????%%%%?速度的更新???西北工業(yè)大學?《導航系統(tǒng)理論與應用》???東－北－天?
????Vx（i+1）=（f_t（1）+2*Wie*sin（L（i）+Vx（i）*tan（L（i））/Rn（i））*Vy（i）-（2*Wie*cos（L（i））+Vx（i）/Rn（i））*Vz（i））*T+Vx（i）;%?>?x軸向速度計算
????Vy（i+1）=（f_t（2）-2*Wie*sin（L（i）+Vx（i）*tan（L（i））/Rn（i））*Vx（i）-Vy（i）/Rm（i）*Vz（i））*T+Vy（i）;%?>?y軸向速度計算
????Vz（i+1）=（f_t（3）+2*Wie*cos（L（i）+Vx（i）/Rn（i））*Vx（i）+Vy（i）/Rm（i）*Vy（i）-0*g）*T+Vz（i）;%?>?z軸向速度計算
????Gama（i+1）?=?T*Vx（i）/cos（L（i））/Rn（i）+Gama（i）;%?>?經度計算
????if?Gama（i+1）>180
????????Gama（i+1）?=?Gama（i+1）-180;%?>經度在-180度（西經）到180（東經）范圍
????end
????L（i+1）?=（T*Vy（i）/Rm（i））+L（i）;?%>?緯度計算
????if?L（i+1）>90
????????L（i+1）?=?180-L（i+1）;%?>緯度小于90度（北緯）
????end
????L?=?L*180/pi;Gama?=?Gama*180/pi;
???fprintf（P‘%10.7f\t%10.7f\n‘Gama（i）L（i））;?
????L?=?L*pi/180;Gama?=?Gama*pi/180;
????Wetx_t（i）?=?-Vy（i）/（Rm（i）+h）;Wety_t（i）?=?Vx（i）/（Rn（i）+h）;Wetz