捷联惯导算法与组合导航原理讲义（20170220）.pdf

【核心代码】捷联惯导算法与组合导航原理讲义

目 录
第 1 章 概 述................................................................................................................................ 6
1.1 捷联惯导算法简介....................................................................................................................................6
1.2 Kalman 滤波与组合导航原理简介.........................................................................................................7
第 2 章 捷联惯导姿态解算基础..................................................................................................10
2.1 反对称阵及其矩阵指数函数.................................................................................................................10
2.1.1 反对称阵.......................................................................................................................................10
2.1.2 反对称阵的矩阵指数函数.......................................................................................................... 12
2.2 方向余弦阵与等效旋转矢量.................................................................................................................13
2.2.1 方向余弦阵.................................................................................................................................. 13
2.2.2 等效旋转矢量.............................................................................................................................. 14
2.3 方向余弦阵微分方程及其求解.............................................................................................................17
2.3.1 方向余弦阵微分方程.................................................................................................................. 17
2.3.2 方向余弦阵微分方程的求解...................................................................................................... 17
2.4 姿态更新的四元数表示.........................................................................................................................20
2.4.1 四元数的基本概念...................................................................................................................... 20
2.4.2 四元数微分方程.......................................................................................................................... 23
2.4.3 四元数微分方程的求解.............................................................................................................. 25
2.5 等效旋转矢量微分方程及其泰勒级数解.............................................................................................26
2.5.1 等效旋转矢量微分方程.............................................................................................................. 26
2.5.2 等效旋转矢量微分方程的泰勒级数解...................................................................................... 29
2.6 圆锥运动条件下的等效旋转矢量算法.................................................................................................31
2.6.1 圆锥运动的描述.......................................................................................................................... 31
2.6.2 圆锥误差补偿算法...................................................................................................................... 33
第 3 章 地球形状与重力场基础..................................................................................................40
3.1 地球的形状描述......................................................................................................................................40
3.2 地球的正常重力场................................................................................................................................. 46
3.3 地球重力场的球谐函数模型................................................................................................................. 50
3.3.1 球谐函数的基本概念.................................................................................................................. 50
3.3.2 地球引力位函数.......................................................................................................................... 58
3.3.3 重力位及重力计算...................................................................................................................... 63
第 4 章 捷联惯导更新算法及误差分析....................................................................................69
4.1 捷联惯导数值更新算法.........................................................................................................................69
4.1.1 姿态更新算法.............................................................................................................................. 69
4.1.2 速度更新算法.............................................................................................................................. 70
4.1.3 位置更新算法.............................................................................................................................. 76
4.2 捷联惯导误差方程.................................................................................................................................76
4.2.1 惯性传感器测量误差................................................................................................................... 76
4.2.2 姿态误差方程............................................................................................................................... 78
4.2.3 速度误差方程............................................................................................................................... 79
4.2.4 位置误差方程............................................................................................................................... 79
4.2.5 误差方程的整理........................................................................................................................... 80
4.3 静基座误差特性分析.............................................................................................................................82
4.3.1 静基座误差方程.......................................................................................................................... 82
4.3.2 高度通道.......................................................................................................................................83
4.3.3 水平通道.......................................................................................................................................83
4.3.4 水平通道的简化.......................................................................................................................... 88
4.3.5 水平通道误差方程的仿真.......................................................................................................... 90
4
第 5 章 卡尔曼滤波基本理论......................................................................................................92
5.1 递推最小二乘法..................................................................................................................................... 92
5.2 Kalman 滤波方程的推导.......................................................................................................................94
5.3 连续时间随机系统的离散化与连续时间 Kalman 滤波.................................................................... 101
5.4 噪声相关条件下的 Kalman 滤波.........................................................................................................107
5.5 序贯滤波................................................................................................................................................111
5.6 信息滤波与信息融合...........................................................................................................................114
5.7 平方根滤波........................................................................................................................................... 116
5.8 遗忘滤波...............................................................................................................................................124
5.9 Sage-Husa 自适应滤波.....................................................................................................................125
5.10 最优平滑算法.....................................................................................................................................127
5.11 非线性系统的 EKF 滤波、二阶滤波与迭代滤波............................................................................ 130
5.12 间接滤波与滤波校正.........................................................................................................................136
5.13 联邦滤波（待完善）.........................................................................................................................136
5.14 滤波的稳定性与可观测度分析.........................................................................................................141
第 6 章 初始对准及组合导航技术..........................................................................................147
6.1 捷联惯导粗对准...................................................................................................................................147
6.1.1 矢量定姿原理............................................................................................................................. 147
6.1.2 解析粗对准方法......................................................................................................................... 149
6.1.3 间接粗对准方法......................................................................................................................... 152
6.2 捷联惯导精对准...................................................................................................................................153
6.3 惯性/卫星组合导航.............................................................................................................................157
6.3.1 空间杆臂误差............................................................................................................................. 157
6.3.2 时间不同步误差......................................................................................................................... 158
6.3.3 状态空间模型............................................................................................................................. 159
6.4 车载惯性/里程仪组合导航.................................................................................................................159
6.4.1 航位推算算法............................................................................................................................. 159
6.4.2 航位推算误差分析..................................................................................................................... 161
6.4.3 惯性/里程仪组合........................................................................................................................ 164
6.5 低成本姿态航向参考系统（AHRS）...................................................................................................167
6.5.1 简化的惯导算法及误差方程..................................................................................................... 168
6.5.2 地磁场测量及误差方程............................................................................................................. 169
6.5.3 低成本组合导航系统模型......................................................................................................... 170
6.5.4 低成本惯导的姿态初始化......................................................................................................... 171
6.5.5 捷联式地平仪的工作原理......................................................................................................... 173
第 7 章 捷联惯导与组合导航仿真............................................................................................176
7.1 飞行轨迹和惯性器件信息仿真...........................................................................................................176
7.1.1 飞行轨迹设计............................................................................................................................. 176
7.1.2 捷联惯导反演算法.................................................................................................................... 177
7.1.3 仿真.............................................................................................................................................178
7.2 捷联惯导仿真....................................................................................................................................... 180
7.2.1 Matlab 子函数............................................................................................................................. 180
7.2.2 捷联惯导仿真主程序................................................................................................................. 185
7.3 惯导/卫星组合导航仿真......................................................................................................................186
7.3.1Matlab 子函数.............................................................................................................................. 186
7.3.2 组合导航仿真主程序................................................................................................................. 187
附 录............................................................................................................................................ 190
A 一些重要的三维矢量运算关系...............................................................................................................190
B 运载体姿态的欧拉角描述......................................................................................................................192
C 姿态更新的毕卡算法、龙格—库塔算法及精确数值解法................................................................. 199
5
D 从非直角坐标系到直角坐标系的矩阵变换......................................................................................... 207
E 线性系统基本理论.................................................................................................................................. 211
F 加权最小二乘估计..................................................................................................................................216
G 矩阵求逆引理..........................................................................................................................................217
H 几种矩阵分解方法（QR、Cholesky 与 UD）....................................................................................... 219
I 二阶滤波中的引理证明..........................................................................................................................223
J 方差阵上界的证明..................................................................................................................................225
K 三阶非奇异方阵的奇异值分解..............................................................................................................226
L Matlab 仿真程序.....................................................................................................................................231
M 练习题......................................................................................................................................................237
参考文献......................................................................................................................................241