视觉SLAM笔记(三)-非线性优化

SLAM问题中，机器人自身的状态估计主要有两种方式: 基于滤波器的方法(Kalman filter, Particle filter等)和基于非线性优化的方法，目前主流的SLAM方案均使用了非线性优化方法

状态估计

经典SLAM模型主要由运动模型和观测模型组成，其方程如下
$$\begin{gathered} x_k=f\left(x_{k-1},u_k\right)+w_k \\ {z}_{k,j}=h\left(y_j,x_k\right)+v_{k,j} \end{gathered}$$
在传统的基于滤波器的方案中其状态估计分为两个步骤: 运动更新与观测更新，从概率学的角度看，是在已知输入数据$u$和观测数据$z$的条件下，计算状态$x$的条件概率分布， x = { x 1 , . . . , x N , y 1 , . . . , y M } x=\lbrace x_1,...,x_N,y_1,...,y_M \rbrace x={ x1​,...,xN​,y1​,...,yM​}，包括了机器人自身位姿以及环境中的路标点，利用贝叶斯法则:
$$P(x|z)=\frac{P(z|x)P(x)}{P(z)}$$
其中$P(x|z)$为后验概率，$P(z|x)$为似然，$P(x)$为先验，此时的目标为求解一个$x$使得后验概率最大
$$x_{MAP}^{\ast }=\arg \max P(x|z)=\arg \max P(z|x)P(x)$$
未知先验的情况下，求解最大似然问题，即在什么状态下最可能产生当前的观测数据
$$x_{MLE}^{\ast }=\arg \max P(z|x)$$
对上述的观测模型$z_{k,j}=h(y_j,x_k$