非线性优化：状态估计+非线性最小二乘

1.状态估计问题

1.最大后验与最大似然

经典的SLAM模型由一个状态方程和一个运动方程构成，如下：


在这些噪声的影响下，我们希望通过带噪声的数据z和u，推断位姿x和地图y(以及他们的概率分布)，这构成了 一个状态估计问题。

2.非线性最小二乘

首先，考虑一个简单的最小二乘：


注：

2.1 一阶和二阶梯度法

2.2 Gauss-Newton

2.3 Levenberg-Marquadt

暂无代码，后边看情况补充。。