惯性导航程序

%%采用WGS-84坐标系
%%长半轴a=6378137m    短半轴b=6356752.3142m   扁率f=1/298.257223563   地球自转角速度w=7.292115×10^-5 rad/s
%%第一偏心率平方=0.00669437999013  地球引力常数GM=3.986004418×10^14 m^3 /s^2 
%%赤道正常重力=9.7803267714m/s^2   两级正常重力=9.832185127m/s^2 
%%初始状态重力加速度为9.788297073
%%初始时间：91620.0 
%%GPS周秒、Gx、Gy、Gz、Ax、Ay、Az （G代表陀螺，A代表加速度计）
%%初始位置（纬经高）：23.1373950708 [deg]  113.3713651222 [deg]  2.175 [m]
%%初始速度：0.0  0.0  0.0 [m/s]
%%初始姿态（degree）：
%%rollpitchheading分别为0.0107951084511778	-2.14251290749072 -75.7498049314083 
%%由初始姿态角可得，q（41）=[0.789216199658865;-0.0114037805155068;-0.0148154631808315;-0.613830795933790]
%%计算方法：a=0.0107951084511778*pi/360;b=-2.14251290749072*pi/360;c= -75.7498049314083*pi/360;
%%        q（41）=[cos（a）*cos（b）*cos（c）+sin（a）*sin（b）*sin（c）;
%%                sin（a）*cos（b）*cos（c）-cos（a）*sin（b）*sin（c）;
%%                cos（a）*sin（b）*cos（c）+sin（a）*cos（b）*sin（c）;
%%                cos（a）*cos（b）*sin（c）-sin（a）*sin（b）*cos（c）];
%%A矩阵为给定观测数据，依次为GPS周秒、Gx、Gy、Gz（陀螺三轴输出）、Ax、Ay、Az（加速度计三轴输出）
%%C矩阵为纯惯导参考结果，依次为GPS周秒、纬度、经度、高度、北向速度、东向速度、垂向速度、横滚角、俯仰角、航向角
%%解算顺序：速度解算——姿态解算——位置解算
  q=zeros（4720201）;    %%q表示从b系到n系的姿态四元数，初始状态已知
  q（1:41）=[0.789216199658865;-0.0114037805155068;-0.0148154631808315;-0.613830795933790];
  m=zeros（3720201）;    %%m表示姿态角，依次为横滚角、俯仰角、航向角
  m（1:31）=[0.000188410185582918;-0.0373939045021898;-1.32208350379651];   %%单位为弧度
  q1=zeros（4720200）;   %%q1表示从b系上一时刻到b系当前时刻的姿态四元数
  q2=zeros（4720200）;   %%q2表示从n系上一时刻到n系当前时刻的姿态四元数
  Cb=zeros（3720200*3）; %%方向余弦矩阵
  Cb（:1:3）=[0.2459845120280550.969228320581583-0.00938522375454779;-0.9685525114201840.2461634155064890.0361884717532193;0.03738519043649890.0001882784724509300.999300911681383];
  v=zeros（3720201）;    %%v表示载体在n系中的地速，依次为北向速度、东向速度、垂向速度
  v（:1）=[0;0;0];
  g=zeros（1720201）;    %%重力
  g（11）=9.788297073;   %%初始时刻重力
  n=zeros（3720201）;    %%n表示每一历元位置，依次为经度、纬度、大地高
  n（:1）=[1.97870359886305;0.403823724320167;2.175];  %%单位分别为弧度、弧度、米
  %%主体程序
  for i=2:720201
      %%姿态解算
      k=[A（i2）;A（i3）;A（i4）]+cross（[A（i-12）;A（i-13）;A（i-14）][A（i2）;A（i3）;A（i4）]）/12;     %%b系更新的矩阵
      q1（:i-1）=[cos（norm（k）/2）;sin（norm（k）/2）*k/norm（k）];
      Rn=6378137/sqrt（1-0.00669437999013*sin（n（2i-1））.^2）;   %%卯酉圈上个历元曲率半径
      Rm=6378137*（1-0.00669437999013）/（（1-0.00669437999013*sin（n（2i-1））.*sin（n（2i-1）））.^1.5）;  %%%%子午圈上个历元曲率半径
      We=[v（2i-1）/（Rn+n（3i-1））;-v（1i-1）/（Rm+n（3i-1））;-v（2i-1）*tan（n（2i-1））/（Rn+n（3i-1））];
      Wi=[7.292115*10^-5*cos（n（2i-1））;0;-7.292115*10^-5*sin（n（2i-1））];
      p=（We+Wi）*（A（i1）-A（i-11））;   %%n系更新的矩阵
      q2（:i-1）=[cos（norm（p）/2）;-sin（norm（p）/2）*p/norm（p）];
      S=[q2（1i-1）-q2（2