假设导航坐标系为“东E/北N/天U”、载体坐标系为“右x/前y/上z”，常用的飞机姿态角转动顺序表示为方位-俯仰-横滚。众所周知，在俯仰接近+-90°时，会出现奇异（方位和横滚无法分开），有文献采用双欧拉角表示（可参考“

    通常，双位置对准容易估计出等效水平陀螺零漂和加速度计偏值，提高初始对准精度。以速度为Kalman滤波量测是有效的（12维，[phiE,phiN,phiU, dvE,dvN,dvU, ebx,eby,ebz, dbx,dby,dbz]'）；以比力为量测，5维状态是无效的（[phiE, phiN, phiU, eby, ebz]'），而9维状态是有效的（[phiE,phiN,phiU, ebx,eby,ebz, dbx,dby,dbz]'），对比结果和程序附后。【虽然惯导在静止（单位置）情况下使用5维状态+比力量测也可实现初始对准。】

（1）速度量测对准误差（180°转位之后失准角变小）

（2）比力量测对准误差（5维状态，180°转位之后失准角反而变差）

Matlab主函数如下（子函数调用请参考PSINS工具箱）：

   Aerospace blockset简介：(本段摘自百度文库) 中文意思：网络太空模块；航太模块组；航空航天模块，MATLAB中针对这些方面内容的模型库。Aerospace Blockset提供了大量的在Simulink环境中使用的航空航天模块，涵盖了航空航天飞行器设计中的主要应用模型。Aerospace Blockset将航空航天领域的通用的标准模块在Simulink环境进行了集成，使您能够方便地组合航空航天飞行器的系统结构。在此基础上能够进行完整的系统开发、测试工作，完成航空航天系统的概念设计。Aerospace Blockset通过RTW能够自动生成实时代码，开展半物理仿真研究。特点：提供航空航天飞行器推进系统、控制系统、动力学及作动器模块提供六自由度和三自由度模型库，可以进行固定质量或者变质量系统模型建模与仿真提供重力场、磁场、大气和风的标准环境模型利用各种转换模块方便实现坐标转换能够完成各物理量的单位制转换提供动画模块，直观显示三自由度和六自由度动画。

Matlab/simulink/Aerospace Blockset有一点关于惯性导航的，大家可参考一下，特别是陀螺加计仿真和误差建模，但没发现其导航更新解算功能。

陀螺误差模型设置：

   在卡尔曼滤波组合导航中，如果量测噪声方差阵是严格对角矩阵，则可以改成序贯滤波的处理方式（理论上完全成立，但数值上由计算顺序不同可能会引起一些误差），省去矩阵求逆运算（即简化为标量除法运算，在C语言编程时特别方便），以下是Matlab流程示意：

function kf = kfupdatesq(kf, yk, measflag)
% KF sequential filtering for measurement update.
%
% See also  kfupdate.

% Copyright(c) 2009-2015, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 22/02/2015
    if measflag~='M', error('kfupdatesq only used for KF measurement update!\n'); end
    Hk = kf.Hk; Rk = kf.Rk;
    for k=1:length(yk)
        kf.Hk = Hk(k,:); kf.Rk = Rk(k,k);
        kf = kfupdate(kf, yk(k), 'M');
    end
    kf.Hk = Hk; kf.Rk