MATLAB惯性系统定位误差事后校正技术研究仿真(8)
基于上面的分析，选择GPS/SINS组合导航算法作为进行惯性系统定位误差事后校正的方法，并将设计GPS/SINS组合导航算法的仿真程序作为编写惯性系统定位误差事后校正方法仿真软件的核心部分。
结合课题的实际背景，设计的仿真软件应主要具备如下功能：
1）具有轨迹发生器功能，利用其可以生成静基座条件下惯性系统的理想轨迹，并生成与理想轨迹相对应的惯性系统的三个轴向陀螺仪和加速度计的输出数据；
2）利用轨迹发生器生成的数据，进行SINS的单独解算；
3）结合2.3节给出的SINS误差模型和2.4节给出的传感器误差模型，设计用于实现GPS/SINS组合导航算法的卡尔曼滤波器；
4）生成GPS数据，并利用通过SINS解算得到的数据，进行GPS/SINS组合导航解算。
3.2 总体方案设计
结合功能分析中仿真软件应该具备的功能，同时为了便于比较单独进行SINS解算与GPS/SINS组合导航解算的效果，设计了三种不同结构的仿真程序，分别为：
1）单SINS解算程序；
2）采用卡尔曼滤波开环校正方法的GPS/SINS 组合导航解算程序；
3）采用卡尔曼滤波闭环校正方法的GPS/SINS 组合导航解算程序。
三种不同结构解算程序的结构简图（只给出关键环节）如图3.1所示，详细的程序结构将在第四章中给出。
图3.1a 单SINS解算程序结构简图
图3.1b GPS/SINS 组合导航解算程序（卡尔曼滤波开环校正方法）结构简图图3.1c GPS/SINS 组合导航解算程序（卡尔曼滤波闭环校正方法）结构简图
3.3 SINS解算算法的设计
根据图2.1给出的SINS的解算模型示意图，设计了SINS的力学模型编排如下。
1）参数初始化
惯导系统工作开始前，需要知道初始条件，如初始速度、位置和初始姿态矩阵 , 可由初始对准得到，从而可进一步计算出初始四元数、初始位置和速度。
2）四元数Q的即时修正
设载体坐标系相对导航坐标系的转动四元数为： (3.1)
Q的即时修正可通过求解下面的四元数微分方程来实现。
(3.2)
四元数微分方程的求解采用2.2节给出的毕卡逼近算法求解。
3）姿态矩阵更新
通过求解四元数微分方程得到Q，即可以计算捷联惯导系统的姿态矩阵 =
4）四元数的最佳归一化处理
为确保“数学平台”中导航坐标系的3个基的模均为1，需要对四元数周期性的进行归一化处理。以欧几里得范数最小为准则的四元数最佳归一化可以由下式获得：
式中 ，Q：归一化前后的四元数值。一般进行若干次即时修正后进行一次归一化处理。
5）比力的坐标变换
将加速度计测量的载体系上的比力 转换为导航坐标系上的比力
(3.4)
6）速度的即时修正
速度的修正如下式，可通过解该微分方程来完成速度计算。
(3.5)
7）位置的即时修正
设当地地理纬度为L，则经度、纬度、高度的变化率如下面公式：
位置的修正即可通过解该微分方程组来完成位置计算。
8）位置速度和地球速度的计算