惯性导航仿真实现源代码

这个程序主要是用matlab编写代码实现惯性导航的仿真，对其工作原理进行仿真，大家可以借鉴

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%         指北方位捷联式惯性导航系统         %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%初始化
clear

load（‘jlfw‘）;

%定义常量
Re=6378245;                                                          
e=1/298.3;
wie_scalar=7.292115147e-5;
g0=9.7803267714;
gk1=0.00193185138639;
gk2=0.00669437999013;
%变量赋初值
longitude=[116.344695283*pi/180 zeros（147999）]; 
latitude=[39.975172*pi/180 zeros（147999）];
h=30;
f=zeros（348000）;
wnb=zeros（348000）;
theta=[0.120992605 zeros（147999）]*pi/180;
gamma=[0.010445947 zeros（147999）]*pi/180;
psi=[360-91.637207 zeros（147999）]*pi/180;
Q=[cos（psi（1）/2）*cos（theta（1）/2）*cos（gamma（1）/2）+sin（psi（1）/2）*sin（theta（1）/2）*sin（gamma（1）/2）;
   cos（psi（1）/2）*sin（theta（1）/2）*cos（gamma（1）/2）+sin（psi（1）/2）*cos（theta（1）/2）*sin（gamma（1）/2）;
   cos（psi（1）/2）*cos（theta（1）/2）*sin（gamma（1）/2）-sin（psi（1）/2）*sin（theta（1）/2）*cos（gamma（1）/2）;
   cos（psi（1）/2）*sin（theta（1）/2）*sin（gamma（1）/2）-sin（psi（1）/2）*cos（theta（1）/2）*cos（gamma（1）/2）];
velocity=[0.000048637 0.000206947 0.007106781];
v=[zeros（148000）];

for i=2:48000
%计算所在地参数
  %Rx为卯酉圈的曲率半径
  Rx=Re/（1-e*sin（latitude（i-1））^2）;
  %Ry为子午圈的曲率半径
  Ry=Re/（1+2*e-3*e*sin（latitude（i-1））^2）;
  %使用WGS-84模型计算该位置的重力加速度
  g=g0*（1+gk1*sin（latitude（i-1））^2）*（1-2*h/Re）/sqrt（1-gk2*sin（latitude（i-1））^2）;
  
  %计算姿态矩阵
  Cnb=[Q（1）^2+Q（2）^2-Q（3）^2-Q（4）^2 2*（Q（2）*Q（3）-Q（1）*Q（4）） 2*（Q（2）*Q（4）+Q（1）*Q（3））;
       2*（Q（2）*Q（3）+Q（1）*Q（4）） Q（1）^2-Q（2）^2+Q（3）^2-Q（4）^2 2*（Q（3）*Q（4）-Q（1）*Q（2））;
       2*（Q（2）*Q（4）-Q（1）*Q（3）） 2*（Q（3）*Q（4）+Q（1）*Q（2）） Q（1）^2-Q（2）^2-Q（3）^2+Q（4）^2];
   
  Cbn=[Q（1）^2+Q（2）^2-Q（3）^2-Q（4）^2 2*（Q（2）*Q（3）+Q（1）*Q（4）） 2*（Q（2）*Q（4）-Q（1）*Q（3））;
       2*（Q（2）*Q（3）-Q（1）*Q（4）） Q（1）^2-Q（2）^2+Q（3）^2-Q（4）^2 2*（Q（3）*Q（4）+Q（1）*Q（2））;
       2*（Q（2）*Q（4）+Q（1）*Q（3）） 2*（Q（3）*Q（4）-Q（1）*Q（2）） Q（1）^2-Q（2）^2-Q（3）^2+Q（4）^2];
  
  %使用毕卡法（角增量法）求解四元数微分方程
  win=[-velocity（2）/Ry;wie_scalar*cos（latitude（i-1））+velocity（1）/Rx;
      wie_scalar*sin（latitude（i-1））+velocity（1）/Rx*tan（latitude（i-1））];
  wnb（:i）=wib_INSc（:i）-Cbn*win;
  delta_big_theta=[0 -wnb（1i） -wnb（2i） -wnb（3i）;
             wnb（1i） 0 wnb（3i） -wnb（2i）;
             wnb（2i） -wnb（3i） 0 wnb（1i）;
             wnb（3i） wnb（2i） -wnb（1i） 0]/80;
  delta_theta=sqrt（（wnb（1i）/80）^2+（wnb（2i）/80）^2+（wnb（3i）/80）^2）;
  Q=（eye（4）*（1-delta_theta^2/8+delta_theta^4/384）+（1/2-delta_theta^2/48）*delta_big_theta）*Q;
  
  %计算姿态角
  theta（i）=asin（Cbn（23））;
  gamma（i）=atan（-Cbn（13）/Cbn（33））;
  psi（i）=atan（Cbn（21）/Cbn（22））;
    if Cbn（22）<0
        psi（i）=psi（i）+pi;
    else if Cbn（21）<0
        psi（i）=psi（i）+2*pi;
        end
    end
    
    if Cbn（33）<0
        if gamma（i）>0 
            gamma（i）=gamma（i）-pi;
        else gamma（i）=gamma（i）+pi;
        end
    end
  
  %计算速度与经纬度
  f（:i）=Cnb*f_INSc（:i）;
  ax=f（1i）+2*wie_scalar*sin（latitude（i-1））*velocity（2）+velocity（1）*velocity（2）*tan（latitude（i-1））/Rx;
  ay=f（2i）-2*wie_scalar*sin（latitude（i-1

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2014-05-06 09:31  惯性导航\
     文件        4672  2009-07-27 14:40  惯性导航\jielian.m
     文件     2304256  2003-12-12 21:33  惯性导航\jlfw.mat
     文件       53248  2007-11-19 13:36  惯性导航\文档.doc