目 录

第 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