捷联惯导姿态解算

function Navi2018
clc
clear
P=fopen（‘result.txt‘‘wt‘）;
Data = load（‘imu.txt‘）;%前三列为加速度信息 后三列为角速度信息 共六列 东-北-天
T=0.005;%惯导递推周期
f_INS = Data（:1:3）;% 加载加速度计数据 比力
wib_INS = Data（:4:6）;% 加载陀螺数据 角速度
L0 = size（Data1）;%数据的行数
Wie = 7.292115147e-5; %> 地球自传角速度
Re = 6378245; %> 地球椭球长半径
h = 0;% > 高度，可更改
e = 1/298.3;%偏心率
%> 重力加速度计算参数
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;% > 初始纬度（弧度）
%> 初始姿态角
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轴向速度
%> 四元素初始值%%%西北工业大学版《导航理论与应用》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）; %> 用四元素表示得姿态矩阵
 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;% > 将体轴系中的比力转化到地理系
    
    %%%% 速度的更新   西北工业大学 《导航系统理论与应用》   东－北－天 
    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