捷联惯导解算程序四元数法

close all;
clear all;
clc;
format long
%初始化参数
%vx（1）=0.000048637;
%vy（1）=0.000206947;
%vz（1）=0.007106781;          %初始速度
vx（1）=0;
vy（1）=0;
vz（1）=0;
jd（1）=116.344695283*2*pi/360;
L（1）=39.975172*2*pi/360;                     %初始经纬度
%B（1）=-91.637207*2*pi/360;         %初始航向角   tian
%C（1）=0.120992605*2*pi/360;         %初始俯仰角   dong
%D（1）=0.010445947*2*pi/360;         %初始横滚角   bei %初始姿态角
B（1）=0;
C（1）=0;
D（1）=0;
re=6378245;
Wie=7.27221e-5;
e=1/298.3;
Ti=1/20;
j=100;
N=200;
g=9.78049;

    Wibxb_INSc=randn（j1）;
    Wibyb_INSc=randn（j1）;
    Wibzb_INSc=randn（j1）;%陀螺在各个方向上测量的数据
    Fbx_INSc=randn（j1）;
    Fby_INSc=randn（j1）;
    Fbz_INSc=randn（j1）;%加速度计在各个方向上测量的数据
for i=1:j
    Rx=re/（1-e*sin（L（1））^2）;
    Ry=re/（1+2*e-3*e*sin（L（1））^2）;
  
    T31=Fbx_INSc（i1）/g*Ti;
    T32=Fby_INSc（i1）/g*Ti;
    T33=Fbz_INSc（i1）/g*Ti;
    T21=（Wibxb_INSc（i1）-Ti*T31*Wie*sin（L（1）））/Ti*Wie*cos（L（1））;
    T22=（Wibyb_INSc（i1）-Ti*T32*Wie*sin（L（1）））/Ti*Wie*cos（L（1））;
    T23=（Wibzb_INSc（i1）-Ti*T33*Wie*sin（L（1）））/Ti*Wie*cos（L（1））;
    T11=T22*T33-T23*T32;
    T12=T23*T31-T21*T33;
    T13=T21*T32-T22*T31;   
    Cbn=[T11 T12 T13;T21 T22 T23;T31 T32 T33];%粗对准后确定的姿态矩阵
    Cnb=Cbn‘;
    
    gnx=0;
    gny=0;
    gnz=g;
    gn=[gnx;gny;gnz];%3*1
    gb=gn‘*Cbn;   %重力加速度在机体系的表示 1*3*3*3=1*3
    Wien_x=0;
    Wien_y=Wie*cos（L（1））;
    Wien_z=Wie*sin（L（1））;
    Wien=[Wien_x;Wien_y;Wien_z];%3*1
    Wieb=Wien‘*Cbn; %地球自转角速度在机体系的表示
    Wibb=[Wibxb_INSc（i1） Wibyb_INSc（i1） Wibzb_INSc（i1）]‘;%陀螺输出的各个轴表示
    Fb=[Fbx_INSc（i1） Fby_INSc（i1） Fbz_INSc（i1）]‘;  %加速度计输出的各个轴表示
    %Wibb‘=Wien‘*Cbn;
    %[Fbx_INSc（i1） Fby_INSc（i1） Fbz_INSc（i1）]=-1*gn‘*Cbn;