视觉SLAM十四讲学习笔记-非线性优化的状态估计问题
经典SLAM模型的位姿可以由变换矩阵来描述，然后用李代数进行优化。观测方程由相机成像模型给出，其中内参是随相机固定的，而外参则是相机的位姿。由于噪声的存在，运动方程和观测方程的等式必定不是精确成立的。得到的数据通常是受各种未知噪声影响的。即使有高精度的相机，运动方程和观测方程也只能近似成立。所以问题是如何在有噪声的数据中进行准确的状态估计，这需要一定程度的最优化背景知识。
附赠自动驾驶最全的学习资料和量产经验：链接
6.1 状态估计问题
6.1.1 批量状态估计与最大后验估计
经典 SLAM 模型由一个运动方程和一个观测方程构成：
xk是相机的位姿变量，可以由Tk∈SE(3)表达。运动方程与输入的具体形式有关，在视觉SLAM中没有特殊性（和普通的机器人、车辆的情况一样）。观测方程则由针孔模型给定。假设在xk处对路标yj进行了一次观测，对应到图像上的像素位置z_k,j_，那么，观测方程可以表示成
其中K为相机内参，s_为像素点的距离，也是(R_k_y_j +t_k_)的第三个分量。如果使用变换矩阵Tk描述位姿，那么路标点yj必须以齐次坐标来描述，计算完成后要转换为非齐次坐标。
在运动和观测方程中，通常假设两个噪声项wk,_v_k,j满足零均值的高斯分布：
其中N表示高斯分布，0表示零均值，Rk,Qk,j为协方差矩阵。在这些噪声的影响下，希望通过带噪声的数据z和u推断位姿x和地图y（以及它们的概率分布），这构成了一个状态估计问题。
处理这个状态估计问题的方法分成两种。
第一种：由于在SLAM过程中，这些数据是随时间逐渐过来的，所以在直观上，应该持有一个当前时刻的估计状态，然后用新的数据来更新它。这种方式称为增量（incremental）的方法，或者叫滤波器。在SLAM的早期研究，主要使用扩展卡尔曼滤波器（EKF）及其衍生方法来求解。
第二种：是把数据累加起来一并处理，这种方式称为批量（batch）的方法。例如
标签：方程,非线性,笔记,观测,相机,估计,SLAM,位姿 From： https://blog.csdn.net/liuphahaha/article/details/141863994