一个关于惯性导航解算的程序源代码，本人的研究结果，希望大家互相学习

INS_OUT一个关于惯性导航解算的程序源代码，本人的研究结果，希望大家互相学习，共同进步,包括姿态更新、速度更新、位置更新。

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;
  %姿态角的计算
    if abs（Cnb（22））>1e-10
        if Cnb（22）>0
           B（i+1）=atan（Cnb（21）/Cnb（22））;              
        elseif Cnb（21）>0
           B（i+1）=atan（Cnb（21）/Cnb（22））+pi;
        else 
            B（i+1）=atan（Cnb（21）/Cnb（22））-pi;
        end
    elseif Cnb（21）>0
       B（i+1）=pi/2;
    else 
       B（i+1）=-pi/2;
    end                     %求航向角
   
    C（i+1）=asin（Cnb（23））;   %求俯仰角
    if abs（Cnb（33））>1e-10
        if Cnb（33）>0
           D（i+1）=atan（-Cnb（13）/Cnb（33））;
        elseif Cnb（13）>0
            D（i+1）=atan（-Cnb（13）/Cnb（33））-pi;
        else
           D（i+1）=atan（-Cnb（13）/Cnb（33））+pi;
        end
   elseif Cnb（13）>0
       D（i+1）=-pi/2;
   else 
       D（i+1）=pi/2;
   end                   %求横滚角
   
    vx（i+1）=（Fbx_INSc（i1）+2*Wie*sin（L（1））*vy（i）+vx（i）*vy（i）*tan（L（1））/Rx-2*Wie*cos（L（1））*vz（i）-vx（i）*vz（i）/Rx）*Ti+vx（i）;%东向速度
    vy（i+1）=（Fby_INSc（i1）-2*Wie*sin（L（1））*vx（i）-vx（i）*vx（i）*tan（L（1））/Rx-vy（i）*vz（i）/Rx）*Ti+vy（i）;%北向速度
    vz（i+1）=（Fbz_INSc（i1）+（2*Wie*cos（L（1））+vx（i）/Rx）*vx（i）+vy（i）*vy（i）/Ry-g）*Ti+vz（i）;%天向速度
    %L（i+1）=vy（i）*Ti/Ry+L（i）;                    %纬度
    %jd（i+1）=vx（i）*Ti/（Rx*cos（L（i）））+jd（i）;        %经度
     
    

end


 t=0:0.1:j;
  %figure（1）
  %plot（jd‘r‘）;xlab

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       3616  2007-11-08 19:19  INS_OUT.m

----------- ---------  ---------- -----  ----

                 3834                    2