第1页 / 共333页
第2页 / 共333页
第3页 / 共333页
第4页 / 共333页
第5页 / 共333页
第6页 / 共333页
第7页 / 共333页
第8页 / 共333页
State Estimation for Robotics_高翔翻译_2018.01.15.pdf
引 言
状态估计简史
传感器，测量和问题定义
本书组织结构
与其它教程的关系
第一部分 状态估计机理
概率论基础
概率密度函数
定 义
贝叶斯公式及推断
矩
样本均值和样本方差
统计独立性与不相关性
归一化积
香农和互信息
克拉美罗下界和费歇尔信息量
高斯概率密度函数
定 义
Isserlis定理
联合高斯概率密度函数，分解与推断
统计独立性，不相关性
高斯分布随机变量的线性变换
高斯概率密度函数的归一化积
Sherman-Morrison-Woodbury 等式
高斯分布随机变量的非线性变换
高斯分布的香农信息
联合高斯概率密度函数的互信息
高斯概率密度函数的克拉美罗下界
高斯过程
总 结
习 题
线性高斯系统的状态估计
离散时间的批量估计问题
问题定义
最大后验估计
贝叶斯推断
存在性，唯一性与能观性
MAP的协方差
离散时间的递归平滑算法
利用批量优化结论中的稀疏结构
Cholesky平滑算法
Rauch-Tung-Striebel平滑算法
离散时间的递归滤波算法
批量优化结论的分解
通过MAP推导卡尔曼滤波
通过贝叶斯推断推导卡尔曼滤波
从增益最优化的角度来看卡尔曼滤波
关于卡尔曼滤波的讨论
误差动态过程
存在性，唯一性以及能观性
连续时间的批量估计问题
高斯过程回归
一种稀疏的高斯过程先验方法
线性时不变情况
与批量离散时间情况的关系
总 结
习 题
非线性非高斯系统的状态估计
引 言
全贝叶斯估计
最大后验估计
离散时间的递归估计问题
问题定义
贝叶斯滤波
扩展卡尔曼滤波
广义高斯滤波
迭代扩展卡尔曼滤波
从MAP角度看IEKF
其它将PDF传入非线性函数的方法
粒子滤波
sigmapoint卡尔曼滤波
迭代sigmapoint卡尔曼滤波
ISPKF与后验均值
滤波器分类
离散时间的批量估计问题
最大后验估计
贝叶斯推断
最大似然估计
讨 论
连续时间的批量估计问题
运动模型
观测模型
贝叶斯推断
算法总结
总 结
习 题
偏差，匹配和外点
处理输入和测量的偏差
偏差对于卡尔曼滤波器的影响
未知的输入偏差
未知的测量偏差
数据关联
外部数据关联
内部数据关联
处理外点
随机采样一致性
M-估计
协方差估计
总 结
习 题
第二部分 三维空间运动机理
三维几何学基础
向量和参考系
参考系
点 积
叉 积
旋 转
旋转矩阵
基本旋转矩阵
其它旋转表示形式
旋转运动学
加上扰动的旋转
姿 态
转换矩阵
机器人学的符号惯例
弗莱纳参考系
传感器模型
透视相机
立体相机
距离-方位角-俯仰角模型
惯性测量单元
总 结
习 题
矩阵李群
几何学
特殊正交群和特殊欧几里得群
李代数
指数映射
伴 随
Baker-Campbell-Hausdorff
距离、体积与积分
插 值
齐次坐标点
微积分和优化
公式摘要
运动学
旋 转
位 姿
旋转线性化
线性化位姿
概率与统计
高斯随机变量和概率分布函数
旋转向量的不确定性
位姿组合
位姿融合
非线性相机模型中的不确定性传播
总 结
习 题
第三部分 应用
位姿估计问题
点云对准
问题描述
单位四元数解法
旋转矩阵解法
变换矩阵解法
点云跟踪
问题描述
运动先验
测量模型
EKF 解法
批量式最大后验解法
位姿图松弛化
问题定义
批量式最大似然解法
初始化
利用稀疏性
边的例子
位姿和点的估计问题
光束平差法
问题描述
测量模型
最大似然解
利用稀疏性
插值的例子
同时定位与地图构建
问题描述
批量式最大后验的解
利用稀疏性
例 子
连续时间的估计
运动先验
原问题
对问题的简化
同时轨迹估计与地图构建
问题建模
观测模型
批量式最大后验解
稀疏性分析
插 值
后 记
参考文献
机器人学的状态估计
作者
Timothy D. Barfoot (tim.barfoot@utoronto.ca)
译者
⾼翔 (gao.xiang.thu@gmail.com)
颜沁睿 (castoryan1991@gmail.com)
刘富强 (liufuqiang_robot@hotmail.com)
郭⽟峰 (taylorguo@126.com)
秦超 (qinchaom4a1@163.com)
谢晓佳 (zerosmemories@gmail.com)
郑帆 (fzheng@link.cuhk.edu.hk)
吴博 (wubo.neu@gmail.com)
张明 (nkzimmon@gmail.com)
左星星 (zuojiaxing1994@foxmail.com)
范帝楷 (fandikai@outlook.com)
张明明 (robotsming@gmail.com)
最后更新 2018 年 1 ⽉ 15 ⽇
第一章 引 言
.
.
.
.
1.1 状态估计简史 . .
.
1.2 传感器，测量和问题定义 .
.
1.3 本书组织结构 . .
.
1.4 与其它教程的关系 .
.
.
.
.
.
.
.
第一部分 状态估计机理
第二章 概率论基础
2.1 概率密度函数 . .
.
2.2 ⾼斯概率密度函数 .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
. .
.
.
. .
DRAFT目 录
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
. .
.
.
2.2.1 定 义 . .
.
.
2.2.2
.
.
2.2.3 联合⾼斯概率密度函数，分解与推断 . .
.
.
.
2.2.4 统计独⽴性，不相关性 . .
.
.
.
.
2.2.5 ⾼斯分布随机变量的线性变换 .
. .
.
. .
.
2.2.6 ⾼斯概率密度函数的归⼀化积 .
.
. .
2.2.7
Sherman-Morrison-Woodbury 等式 .
.
2.2.8 ⾼斯分布随机变量的⾮线性变换 .
. .
.
.
2.2.9 ⾼斯分布的⾹农信息 . .
.
.
. .
2.2.10 联合⾼斯概率密度函数的互信息 .
.
2.2.11 ⾼斯概率密度函数的克拉美罗下界 .
. .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
2.1.1 定 义 . .
.
.
.
.
2.1.2 贝叶斯公式及推断 . .
.
.
2.1.3 矩 . .
.
.
.
2.1.4 样本均值和样本⽅差 . .
.
.
2.1.5 统计独⽴性与不相关性 . .
.
.
2.1.6 归⼀化积 . .
.
2.1.7 ⾹农和互信息 .
.
.
2.1.8 克拉美罗下界和费歇尔信息量 .
.
.
.
.
.
Isserlis 定理 .
. .
.
.
. .
.
.
. .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
5
5
6
8
8
.
.
.
.
10
11
. 11
. 11
. 12
. 13
. 14
. 14
. 15
. 15
. 16
. 17
. 17
. 18
. 20
. 21
. 22
. 23
. 24
. 26
. 29
. 31
. 31
. 33
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
2.3 ⾼斯过程 . .
2
2.4 总 结 .
2.5 习 题 .
. .
. .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
第三章 线性高斯系统的状态估计
3.3 离散时间的递归滤波算法 .
. .
3.4 连续时间的批量估计问题 .
.
.
3.5 总 结 .
3.6 习 题 .
.
3.2 离散时间的递归平滑算法 .
.
3.1 离散时间的批量估计问题 .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
. .
. .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
. .
.
.
. .
. .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
3.1.1 问题定义 . .
.
3.1.2 最⼤后验估计 .
3.1.3 贝叶斯推断 .
.
.
3.1.4 存在性，唯⼀性与能观性 . .
.
3.1.5 MAP 的协⽅差 .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
. .
3.2.1 利⽤批量优化结论中的稀疏结构 .
.
.
.
3.2.2 Cholesky 平滑算法 . .
.
. .
.
3.2.3 Rauch-Tung-Striebel 平滑算法 .
.
.
.
.
.
.
.
.
.
.
3.3.1 批量优化结论的分解 . .
.
.
.
3.3.2 通过 MAP 推导卡尔曼滤波 . .
.
.
. .
3.3.3 通过贝叶斯推断推导卡尔曼滤波 .
3.3.4 从增益最优化的⾓度来看卡尔曼滤波 . .
.
.
3.3.5 关于卡尔曼滤波的讨论 . .
.
.
3.3.6 误差动态过程 .
.
.
.
.
3.3.7 存在性，唯⼀性以及能观性 . .
.
.
.
.
.
.
3.4.1 ⾼斯过程回归 .
.
. .
3.4.2 ⼀种稀疏的⾼斯过程先验⽅法 .
.
.
3.4.3 线性时不变情况 .
.
3.4.4 与批量离散时间情况的关系 . .
.
.
.
.
.
.
.
.
DRAFT
.
4.1.1 全贝叶斯估计 .
4.1.2 最⼤后验估计 .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
. .
. .
. .
. .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
. .
. .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
. .
4.1 引 ⾔ .
第四章 非线性非高斯系统的状态估计
.
.
. .
. .
. .
.
.
. .
. .
. .
.
.
.
4.2 离散时间的递归估计问题 .
.
.
.
.
.
.
.
.
.
4.2.1 问题定义 . .
.
.
.
4.2.2 贝叶斯滤波 .
.
.
4.2.3 扩展卡尔曼滤波 .
4.2.4 ⼴义⾼斯滤波 .
.
.
4.2.5 迭代扩展卡尔曼滤波 . .
4.2.6 从 MAP ⾓度看 IEKF . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
. 34
. 34
36
. 36
. 36
. 37
. 41
. 44
. 46
. 48
. 48
. 49
. 52
. 54
. 54
. 59
. 63
. 64
. 65
. 65
. 66
. 67
. 67
. 70
. 76
. 80
. 80
. 81
83
. 83
. 84
. 85
. 87
. 87
. 88
. 90
. 93
. 94
. 95
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
. .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
sigmapoint 卡尔曼滤波 . .
4.2.7 其它将 PDF 传⼊⾮线性函数的⽅法 .
.
4.2.8 粒⼦滤波 . .
.
4.2.9
.
.
.
4.2.10 迭代 sigmapoint 卡尔曼滤波 . .
.
.
4.2.11 ISPKF 与后验均值 .
4.2.12 滤波器分类 .
.
.
.
.
.
4.3 离散时间的批量估计问题 .
.
.
.
.
.
.
.
.
.
.
.
.
4.4 连续时间的批量估计问题 .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
4.4.1 运动模型 . .
4.4.2 观测模型 . .
.
4.4.3 贝叶斯推断 .
.
4.4.4 算法总结 . .
.
.
.
.
. .
. .
. .
. .
. .
. .
.
.
. .
.
.
.
.
. .
.
.
.
.
.
.
4.5 总 结 .
4.6 习 题 .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
. .
. .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
4.3.1 最⼤后验估计 .
4.3.2 贝叶斯推断 .
.
4.3.3 最⼤似然估计 .
4.3.4 讨 论 . .
.
DRAFT
.
5.1.1 偏差对于卡尔曼滤波器的影响 .
.
5.1.2 未知的输⼊偏差 .
5.1.3 未知的测量偏差 .
.
.
.
.
.
.
.
.
.
.
5.3.1 随机采样⼀致性 .
.
5.3.2 M-估计 . .
.
.
.
.
5.3.3 协⽅差估计 .
.
.
.
.
.
.
.
5.2.1 外部数据关联 .
5.2.2 内部数据关联 .
.
.
.
. .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
. .
. .
.
.
. .
. .
.
.
. .
.
.
. .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
. .
. .
.
.
.
.
. .
.
.
5.4 总 结 .
5.5 习 题 .
5.1 处理输⼊和测量的偏差 .
.
5.2 数据关联 . .
.
5.3 处理外点 . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
第五章 偏差，匹配和外点
.
.
.
.
第二部分 三维空间运动机理
第六章 三维几何学基础
6.1 向量和参考系 . .
6.1.1 参考系 . .
.
6.1.2 点 积 . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
. 96
. 102
. 104
. 109
. 110
. 111
. 112
. 112
. 118
. 120
. 124
. 125
. 126
. 128
. 128
. 129
. 129
. 130
132
. 132
. 132
. 135
. 137
. 139
. 139
. 140
. 140
. 141
. 142
. 144
. 146
. 146
147
148
. 148
. 148
. 149
6.2 旋 转 .
6.3 姿 态 .
6.4 传感器模型 . .
6.5 总 结 .
6.6 习 题 .
第七章 矩阵李群
7.1 ⼏何学 . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
. .
. .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
6.1.3 叉 积 . .
.
.
.
.
.
.
.
.
.
.
.
6.2.1 旋转矩阵 . .
.
6.2.2 基本旋转矩阵 .
. .
6.2.3 其它旋转表⽰形式 . .
6.2.4 旋转运动学 .
. .
.
. .
6.2.5 加上扰动的旋转 .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
6.3.1 转换矩阵 . .
.
6.3.2 机器⼈学的符号惯例 . .
.
6.3.3 弗莱纳参考系 .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
6.4.1 透视相机 . .
6.4.2 ⽴体相机 . .
.
6.4.3 距离-⽅位⾓-俯仰⾓模型 . .
.
6.4.4 惯性测量单元 .
.
.
.
.
. .
.
.
.
.
.
.
. .
.
.
.
.
. .
. .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
DRAFT
.
7.1.1 特殊正交群和特殊欧⼏⾥得群 .
.
.
7.1.2 李代数 . .
.
.
7.1.3 指数映射 . .
.
.
7.1.4 伴 随 . .
.
.
.
7.1.5 Baker-Campbell-Hausdorff . .
7.1.6 距离、体积与积分 . .
.
.
.
.
.
.
7.1.7 插 值 . .
.
.
.
.
. .
7.1.8 齐次坐标点 .
.
7.1.9 微积分和优化 .
. .
.
.
.
.
.
.
.
7.1.10 公式摘要 . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
. .
.
.
. .
.
.
.
.
.
.
.
7.3.1 ⾼斯随机变量和概率分布函数 .
7.3.2 旋转向量的不确定性 . .
.
.
.
7.3.3 位姿组合 . .
.
.
.
.
7.2.1 旋 转 . .
7.2.2 位 姿 . .
.
7.2.3 旋转线性化 .
7.2.4 线性化位姿 .
.
.
.
. .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
. .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
. 150
. 151
. 151
. 152
. 153
. 159
. 163
. 166
. 167
. 168
. 170
. 172
. 173
. 179
. 181
. 182
. 184
. 184
186
. 186
. 186
. 188
. 190
. 196
. 200
. 207
. 209
. 214
. 215
. 222
. 222
. 222
. 225
. 227
. 231
. 232
. 233
. 236
. 238
7.2 运动学 . .
.
.
7.3 概率与统计 . .
.
.
.
.
.
.
.
.
7.3.4 位姿融合 . .
.
7.3.5 ⾮线性相机模型中的不确定性传播 .
.
.
. .
. .
7.4 总 结 .
7.5 习 题 .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
. .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
第三部分 应用
第八章 位姿估计问题
8.1 点云对准 . .
8.2 点云跟踪 . .
8.3 位姿图松弛化 . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
. .
. .
. .
.
.
.
.
.
.
.
.
.
.
.
.
8.1.1 问题描述 . .
.
8.1.2 单位四元数解法 .
.
8.1.3 旋转矩阵解法 .
.
8.1.4 变换矩阵解法 .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
8.2.1 问题描述 . .
.
8.2.2 运动先验 . .
.
8.2.3 测量模型 . .
8.2.4 EKF 解法 . .
.
8.2.5 批量式最⼤后验解法 . .
.
.
8.3.1 问题定义 . .
.
8.3.2 批量式最⼤似然解法 . .
.
8.3.3 初始化 . .
.
.
8.3.4 利⽤稀疏性 .
8.3.5 边的例⼦ . .
.
.
DRAFT
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
. .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
9.1 光束平差法 . .
第九章 位姿和点的估计问题
.
.
.
9.1.1 问题描述 . .
9.1.2 测量模型 . .
.
9.1.3 最⼤似然解 .
9.1.4 利⽤稀疏性 .
9.1.5 插值的例⼦ .
.
.
.
.
.
.
9.2 同时定位与地图构建 .
.
.
.
.
.
.
.
.
9.2.1 问题描述 . .
.
9.2.2 批量式最⼤后验的解 . .
9.2.3 利⽤稀疏性 .
.
.
.
9.2.4 例 ⼦ . .
.
.
.
.
.
.
. .
. .
. .
. .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
. .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
. 244
. 246
. 253
. 254
256
257
. 257
. 257
. 258
. 261
. 273
. 275
. 275
. 276
. 277
. 278
. 280
. 284
. 284
. 285
. 287
. 288
. 289
292
. 292
. 293
. 293
. 296
. 300
. 302
. 305
. 306
. 306
. 308
. 308
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
310
. 310
. 310
. 314
. 315
. 315
. 316
. 317
. 318
. 318
. 320
321
第十章 连续时间的估计
.
.
10.1 运动先验 . .
参考文献
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
10.1.1 原问题 . .
.
10.1.2 对问题的简化 .
.
.
.
10.2 同时轨迹估计与地图构建 .
.
.
.
.
.
.
. .
. .
.
.
10.2.1 问题建模 . .
10.2.2 观测模型 . .
.
.
10.2.3 批量式最⼤后验解 . .
10.2.4 稀疏性分析 .
. .
.
.
.
10.2.5 插 值 . .
10.2.6 后 记 . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
DRAFT
DRAFT
相关推荐
2023年江西萍乡中考道德与法治真题及答案.doc
2012年重庆南川中考生物真题及答案.doc
2013年江西师范大学地理学综合及文艺理论基础考研真题.doc
2020年四川甘孜小升初语文真题及答案I卷.doc
2020年注册岩土工程师专业基础考试真题及答案.doc
2023-2024学年福建省厦门市九年级上学期数学月考试题及答案.doc
2021-2022学年辽宁省沈阳市大东区九年级上学期语文期末试题及答案.doc
2022-2023学年北京东城区初三第一学期物理期末试卷及答案.doc
2018上半年江西教师资格初中地理学科知识与教学能力真题及答案.doc
2012年河北国家公务员申论考试真题及答案-省级.doc
2020-2021学年江苏省扬州市江都区邵樊片九年级上学期数学第一次质量检测试题及答案.doc
2022下半年黑龙江教师资格证中学综合素质真题及答案.doc
