非线性优化

$$\begin{cases} \bold{x}_k = f(\bold{x}_{k-1},\bold{u}_k) + \bold{w}_k \qquad{运动方程} \\ \bold{z}_{k,j} = h(\bold{y}_j,\bold{x}_k) + \bold{v}_{k,j} \qquad{观测方程} \end{cases}$$

$\bold{x}_k$ 表示相机位姿，$\bold{u}_k$表示传感器读数，$\bold{w}_k$表示该过程中加入的噪声

观测方程表示在$\bold{x}_k$位置看到某个路标点$\bold{y}_j$产生的一个观测数据$\bold{z}_{k,j}$，其中$\bold{v}_{k,j}$是这次观测的噪声

观测方程

$$sz_{k,j} = \bold{K}(\bold{R}_k\bold{y}_j + \bold{t}_k)$$

$s$为像素点的距离,也是$(\bold{R}_k\bold{y}_j + \bold{t}_k)$的第三个分量(相机坐标系下的$Z$)