第1章 引言 1
1.1 状态估计简史 1
1.2 传感器、测量和问题定义 3
1.3 本书组织结构 4
1.4 与其他教程的关系 5
第一部分 状态估计机理 7
第2章 概率论基础 9
2.1 概率密度函数 9
2.1.1 定义 9
2.1.2 贝叶斯公式及推断 10
2.1.3 矩 11
2.1.4 样本均值和样本方差 12
2.1.5 统计独立性与不相关性 12
2.1.6 归一化积 13
2.1.7 香农信息和互信息 13
2.1.8 克拉美罗下界和费歇尔信息量 14
2.2 高斯概率密度函数 15
2.2.1 定义 15
2.2.2 Isserlis定理 16
2.2.3 联合高斯概率密度函数，分解与推断 18
2.2.4 统计独立性、不相关性 19
2.2.5 高斯分布随机变量的线性变换 20
2.2.6 高斯概率密度函数的归一化积 21
2.2.7 Sherman-Morrison-Woodbury等式 22
2.2.8 高斯分布随机变量的非线性变换 23
2.2.9 高斯分布的香农信息 27
2.2.10 联合高斯概率密度函数的互信息 28
2.2.11 高斯概率密度函数的克拉美罗下界 29
2.3 高斯过程 30
2.4 总结 31
2.5 习题 32
第3章 线性高斯系统的状态估计 33
3.1 离散时间的批量估计问题 33
3.1.1 问题定义 33
3.1.2 最大后验估计 34
3.1.3 贝叶斯推断 38
3.1.4 存在性、唯一性与能观性 41
3.1.5 MAP的协方差 44
3.2 离散时间的递归平滑算法 45
3.2.1 利用批量优化结论中的稀疏结构 45
3.2.2 Cholesky平滑算法 46
3.2.3 Rauch-Tung-Striebel平滑算法 49
3.3 离散时间的递归滤波算法 52
3.3.1 批量优化结论的分解 52
3.3.2 通过MAP推导卡尔曼滤波 56
3.3.3 通过贝叶斯推断推导卡尔曼滤波 60
3.3.4 从增益最优化的角度来看卡尔曼滤波 62
3.3.5 关于卡尔曼滤波的讨论 62
3.3.6 误差动态过程 63
3.3.7 存在性、唯一性以及能观性 64
3.4 连续时间的批量估计问题 65
3.4.1 高斯过程回归 65
3.4.2 一种稀疏的高斯过程先验方法 68
3.4.3 线性时不变情况 73
3.4.4 与批量离散时间情况的关系 77
3.5 总结 77
3.6 习题 78
第4章 非线性非高斯系统的状态估计 81
4.1 引言 81
4.1.1 全贝叶斯估计 82
4.1.2 最大后验估计 83
4.2 离散时间的递归估计问题 85
4.2.1 问题定义 85
4.2.2 贝叶斯滤波 86
4.2.3 扩展卡尔曼滤波 88
4.2.4 广义高斯滤波 91
4.2.5 迭代扩展卡尔曼滤波 92
4.2.6 从MAP角度看IEKF 93
4.2.7 其他将PDF传入非线性函数的方法 94
4.2.8 粒子滤波 100
4.2.9 sigmapoint卡尔曼滤波 103
4.2.10 迭代sigmapoint卡尔曼滤波 107
4.2.11 ISPKF与后验均值 108
4.2.12 滤波器分类 110
4.3 离散时间的批量估计问题 110
4.3.1 最大后验估计 110
4.3.2 贝叶斯推断 116
4.3.3 最大似然估计 118
4.3.4 讨论 122
4.4 连续时间的批量估计问题 123
4.4.1 运动模型 124
4.4.2 观测模型 126
4.4.3 贝叶斯推断 126
4.4.4 算法总结 127
4.5 总结 128
4.6 习题 128
第5章 偏差、匹配和外点 131
5.1 处理输入和测量的偏差 131
5.1.1 偏差对于卡尔曼滤波器的影响 131
5.1.2 未知的输入偏差 134
5.1.3 未知的测量偏差 136
5.2 数据关联 137
5.2.1 外部数据关联 138
5.2.2 内部数据关联 138
5.3 处理外点 139
5.3.1 随机采样一致性 140
5.3.2 M估计 141
5.3.3 协方差估计 143
5.4 总结 145
5.5 习题 145
第二部分 三维空间运动机理 147
第6章 三维几何学基础 149
6.1 向量和参考系 149
6.1.1 参考系 150
6.1.2 点积 151
6.1.3 叉积 151
6.2 旋转 152
6.2.1 旋转矩阵 152
6.2.2 基本旋转矩阵 153
6.2.3 其他的旋转表示形式 154
6.2.4 旋转运动学 160
6.2.5 加上扰动的旋转 164
6.3 姿态 167
6.3.1 变换矩阵 168
6.3.2 机器人学的符号惯例 169
6.3.3 弗莱纳参考系 170
6.4 传感器模型 173
6.4.1 透视相机 174
6.4.2 立体相机 180
6.4.3 距离-方位角-俯仰角模型 182
6.4.4 惯性测量单元 183
6.5 总结 185
6.6 习题 186
第7章 矩阵李群 187
7.1 几何学 187
7.1.1 特殊正交群和特殊欧几里得群 187
7.1.2 李代数 189
7.1.3 指数映射 191
7.1.4 伴随 197
7.1.5 Baker-Campbell-Hausdorff 200
7.1.6 距离、体积与积分 207
7.1.7 插值 209
7.1.8 齐次坐标点 214
7.1.9 微积分和优化 215
7.1.10 公式摘要 222
7.2 运动学 222
7.2.1 旋转 222
7.2.2 姿态 224
7.2.3 旋转线性化 227
7.2.4 线性化姿态 230
7.3 概率与统计 232
7.3.1 高斯随机变量和概率分布函数 232
7.3.2 旋转向量的不确定性 236
7.3.3 姿态组合 238
7.3.4 姿态融合 243
7.3.5 非线性相机模型中的不确定性传播 246
7.4 总结 253
7.5 习题 254
第三部分 应用 257
第8章 位姿估计问题 259
8.1 点云对准 259
8.1.1 问题描述 259
8.1.2 单位四元数解法 260
8.1.3 旋转矩阵解法 263
8.1.4 变换矩阵解法 275
8.2 点云跟踪 277
8.2.1 问题描述 277
8.2.2 运动先验 278
8.2.3 测量模型 279
8.2.4 EKF解法 280
8.2.5 批量式最大后验解法 282
8.3 位姿图松弛化 287
8.3.1 问题定义 287
8.3.2 批量式最大似然解法 288
8.3.3 初始化 290
8.3.4 利用稀疏性 290
8.3.5 边的例子 291
第9章 位姿和点的估计问题 295
9.1 光束平差法 295
9.1.1 问题描述 295
9.1.2 测量模型 296
9.1.3 最大似然解 299
9.1.4 利用稀疏性 302
9.1.5 插值的例子 304
9.2 同时定位与地图构建 308
9.2.1 问题描述 308
9.2.2 批量式最大后验的解 308
9.2.3 利用稀疏性 310
9.2.4 例子 310
第10章 连续时间的估计 313
10.1 运动先验 313
10.1.1 原问题 313
10.1.2 对问题的简化 317
10.2 同时轨迹估计与地图构建 318
10.2.1 问题建模 318
10.2.2 观测模型 318
10.2.3 批量式最大后验解 319
10.2.4 稀疏性分析 320
10.2.5 插值 321
10.2.6 后记 322
附录A 补充材料 325
A.1 李群的工具 325
A.1.1 SE（3）上的导数 325
A.2 运动学 326
A.2.1 SO（3）上的雅可比恒等式 326
A.2.2 SE（3）的雅可比恒等式 326
参考文献 329
索引 335