SINS捷联惯导解算程序

clc;
clear;
format long;
%%%%%%%%%%%%%%%%%%%%%读数据%%%%%%%%%%%%%%%%%%%%
fid=fopen（‘Data180528022452.tsv‘‘r‘）;
tline = fgetl（fid）; tline = fgetl（fid）;%读两行字符
C=textscan（fid‘%f‘）;
count1=size（C{11}）;
n=count1（1）/7; %n是数据的组数
delta_t=0.01; %采样时间0.01s
%%%%%%%%%%%%%%%%%常值参数%%%%%%%%%%%%%%%%%
g0=9.78049;
w_ie=7.292115e-5; %弧度
Re=6378137;
Rp=6356752;
f=1/298.257;
%%%%%%%%%%%%%%%%%%%初始值%%%%%%%%%%%%%%%%%%%%%%
theta=11.8/180*pi; gamma=168.76/180*pi; psi=141.3/180*pi; %初始姿态角
v_E_n=0; v_N_n=0; v_U_n=0;     %初始速度
latitude=39.96083*pi/180; longitude=116.31922*pi/180;  height=60; %初始经纬高
% 初始四元数 及C_n_bC_b_n计算↘
q=[0;0;0;0];
q（1）=cos（psi/2）*cos（theta/2）*cos（gamma/2）-sin（psi/2）*sin（theta/2）*sin（gamma/2）;
q（2）=cos（psi/2）*sin（theta/2）*cos（gamma/2）+sin（psi/2）*cos（theta/2）*sin（gamma/2）;
q（3）=cos（psi/2）*cos（theta/2）*sin（gamma/2）-sin（psi/2）*sin（theta/2）*cos（gamma/2）;
q（4）=cos（psi/2）*sin（theta/2）*sin（gamma/2）-sin（psi/2）*cos（theta/2）*cos（gamma/2）;
T11=q（1）^2+q（2）^2-q（3）^2-q（4）^2;
T12=2*（q（2）*q（3）-q（1）*q（4））;
T13=2*（q（2）*q（4）+q（1）*q（3））;
T21=2*（q（2）*q（3）+q（1）*q（4））;
T22=q（1）^2-q（2）^2+q（3）^2-q（4）^2;
T23=2*（q（4）*q（3）-q（1）*q（2））;
T31=2*（q（2）*q（4）-q（1）*q（3））;
T32=2*（q（4）*q（3）+q（1）*q（2））;
T33=q（1）^2-q（2）^2-q（3）^2+q（4）^2;
C_n_b=[T11T12T13;T21T22T23;T31T32T33];
C_b_n=C_n_b.‘;
% 由地球自转角速率计算w_n_ie，w_n_en↘
Rm=Re*（1-2*f+3*f*（sin（latitude））^2）;
Rn=Re*（1+f*（sin（latitude））^2）;
w_n_ie=[0; w_ie*cos（latitude）; w_ie*sin（latitude） ];
w_n_en=[ -v_N_n/（Rm+height）; v_E_n/（Rn+height）; v_E_n*tan（latitude）/（Rn+height） ];
% 重力加速度的初始值计算g↘
g=g0*（1-2*height/Re）;
%%%%%%%%%%%%%%%%%计算循环%%%%%%%%%%%%%%%%%%%%%%
for i=1:n-1
    %%%%%%%%%%%%%%%%%%%%%%%%%读一组数据%%%%%%%%%%%%%%
    w_b_ib=zeros（31）; f_b_ib=zeros（31）;
    time=C{11}（i*7+1）;
    w_b_ib（1）=C{11}（i*7+5）;
    w_b_ib（2）=C{11}（i*7+6）;
    w_b_ib（3）=C{11}（i*7+7）;
    f_b_ib（1）=C{11}（i*7+2）*g;
    f_b_ib（2）=C{11}（i*7+3）*g;
    f_b_ib（3）=C{11}（i*7+4）*g;
    %%%%%%%%%%%%%%%%%%姿态角速度w_b_nb 计算%%%%%%%%%%%%%%%
    w_b_nb=w_b_ib-C_n_b*（w_n_ie+w_n_en）;
    %%%%%%%%%%%%%%%%%%%姿态矩阵求解%%%%%%%%%%%%%%%%%%%
    %/******************四元数更新矩阵***********************/%
    DELTA_THETA=w_b_nb*delta_t;
    DELTA_THETA0=norm（DELTA_THETA）;
    I=diag（[1111]）;
    DELTA_THETA_MATRIX=[ 0 -w_b_nb（1）*delta_t -w_b_nb（2）*delta_t -w_b_nb（3）*delta_t;
        w_b_nb（1）*delta_t 0 w_b_nb（3）*delta_t -w_b_nb（2）*delta_t;
        w_b_