SLAM图优化poseGraph-(2)优化推导

1.误差函数

其中：f(x)是状态函数，比如机器人的里程计状态，Zi是雷达观测或雷达与地图的匹配结果，两者误差为e。

2.待优化的目标函数

目标函数是优化的目标，一般构建为最小二乘目标函数。

其中：中间的sigma是观测的信息矩阵。

3.优化方法(GN为例)

(1) 线性化
目标函数为非线性系统，需要线性化才能求解。

其中Jacobian矩阵：

(2)目标函数展开

其中，记：
…
我们的目标是求解delta_x，即位姿调整量。
由于F（x+delta_x）是关于delta_x的二次函数，所以直接对delta_x求导。
（3）迭代公式

得到状态增量：

由迭代公式x=x+delta_x，直到收敛为止，优化结束。

4.slam中对应公式

（1）误差函数

xi和xj是i和j两点的位姿，转化为Ri和ti,Rj和tj, 观测Zij,转化为 Rij和tij。那么误差函数如下：

（2）对应的Jacobian矩阵



（3）H矩阵和b向量及其更新

H更新：


b更新:

（4）求解delta_x