SLAM14讲学习笔记 —— 第六讲 —— 非线性优化

一. 状态估计问题

经典的SLAM/模型由2个方程构成：

如上式所示：上面的为运动方程；下面的为观测方程。
参数：
k:时刻
x_k:k时刻的相机位姿。（用变换矩阵或李代数表示）
u_k:输入（运动传感器的读数）
y_j: 相机位姿为x_k时，观测点的路标点
z_k,j:相机位姿为x_k时，对路标y_j进行了一次观测，对应到图像上的像素位置。
w_k:噪声项
v_k,j:噪声项
f() 、h(): 某函数

本讲主要讲观测方程：

根据第五讲的知识：假设在x_k处对路标进行了一次观测，对应到图像上的像素位置z_k,j，则观测方程可表示：
注1：K为相机内参；s为像素点的距离（现实空间的观测点在相机坐标系下的Z坐标），z_k,j和y_j皆为齐次坐标的形式。其中exp(&^)=T（变换矩阵）,&为六维向量。
&^即为将六维向量转换为四维矩阵。y_j为世界坐标系下的坐标的齐次表示。

注2：通常认为两个噪声项w_k,v_k,j都满足零均值的高斯分布：

我们的工作就是：希望通过带有噪声的数据z和u，推断出位姿x和地图y。这构成了一个状态估计问题。
对机器人的状态估计问题，就是求已知输入数据u和观测数据z的条件下，计算状态x的条件概率分布。即：

当没有测量运动的传感器，只有一张张图象时，即只考虑观测方程时，相当于估计P（x | z)的条件概率分布。根据贝叶斯：

注1：P（x | z)称为后验概率。P（x）称为先验。P（z | x)称为似然。
注2：分母部分与估计量x无关，所以可以略去。
注3：直接求后验很难，但求一个状态最优