概率密度求解定理_SLAM十四讲重要公式定理整理（中）

中篇的章节范围为十四讲中的第10和11讲，其中列出的概念，公式及定理证明可以用作备忘方便理解和查阅。

CH10 后端1

ch10-1状态估计概率解释建模

原运动和观测方程：

重新定义x为k时刻的所有未知量，包含原先的相机位姿x与m个路标点

用Bayes法则，改写0~k时刻的状态分布，其中右侧两项分别为似然和先验：

将先验部分按照第k-1时刻为条件概率展开：

ch10-2 线性系统和KF

预备知识(高斯分布)高维形式的高斯分布概率密度函数：

对于两个独立的高斯分布

矩阵乘法后的高斯分布

随机变量乘积的高斯分布

复合运算 x,y不独立时，复合分布为：

KF推导： ch10-1的先验展开结果中，等式右侧第一部分：

第二部分：

对于线性高斯系统：

KF的目标即为：

假设已知k-1时刻后验状态估计

和它的协方差

根据k时刻的输入和观测数据，确定

的后验分布

后续推导中用上帽子

表示后验，以下帽子

表示先验分布

根据状态方程，可以通过k-1时刻的状态后验值得到k时刻状态的分布(先验)：

由观测方程估计的观测数据(似然)：

根据贝叶斯公式可得：(个人感觉等式右侧应该有个比例系数)

两侧都是高斯分布，讨论指数部分：

比较待求的协方差二次系数：

定义：

下面通过比较一次项系数，求k时刻的状态后验：

对整个KF流程进行总结：预测：

更新，先计算卡尔曼增益K：

然后计算后验概率的分布：

ch10-3 EKF

在k时刻，把

处进行线性化

预测步骤：

根据观测方程：

中间推导同KF，得到EKF的卡尔曼增益

后验概率：

ch10-4 Bundle Adjustment求解从视觉重建中提炼出最优的3D模型和相机参数(内参数和外参数)：从每一个特征点反射出来的几束光线(bundle of light rays)，在我们把相机姿态和特征点空间位置做出最优的调整(adjustment)之后，最后收束到相机光心的过程。投影模型回顾(只考虑径向畸变)

为相机位姿，外参

,

；

为路标，即三维点

，

为观测数据

整体Cost Function:

BA求解 全部待优化变量：

表示代价函数对相机姿态偏导，

表示对路标点位置的偏导，具体形式参见上篇的ch7-7

G-N法的增量方程及H矩阵：

ch10-5 H矩阵稀疏性及边缘化稀疏性起因及图解 对第

个相机位姿和第

个路标

设

只在

处有非零块，对整体

总结：

和

都是对角阵，

和

稀疏还是稠密视具体观测数据而定边缘化(Marginalization)

本质：舒尔消元(Schur trick)

矩阵区域划分图示：

其中

为对角阵，每个对角块维度和相机参数维度相同，对角块个数为相机变量个数，

矩阵为

的三维空间路标点矩阵构成的对角块矩阵。 增量方程运用高斯消元对H进行消元：

上式解得

代入原方程，之后求解

CH11 后端2

ch11-1 Pose Graph的优化

相机位姿用

表示

设立最小二乘误差：

其中两个待优化的变量，

和

，各给一个左扰动后，误差变为

运用：

将扰动移动到到最右：

根据李代数求导法则，求出误差关于两个位姿的雅可比矩阵

误差接近于零时，可以将左右雅可比近似取为

或者

之后记

为所有边的集合，总体目标函数为：