slam‘s degenerate problem

1.问题描述

对于slam中的状态估计问题，通常可以描述为一个最小二乘问题

一般来说，f(x)为非线性的方程，但是我们可以通过线性化的方法即求雅可比矩阵

给定初值通过不断的迭代，直至收敛求得最优值，即可以研究这样一个线性问题

通常A是一个x×n的矩阵，x为约束方程个数，n为向量x的维数，也即此线性方程组为超定方程组。

我们想研究状态空间中约束的结构来评估基于优化的状态估计问题的简并性。

以最简单的例子为例

黑线代表约束方程，围成区域为可行解区域，对于约束较好的方程组，所得解在各个方向上都比较稳定。

 对于这样的约束方程，黑线基本平行，所得解在橙色方向上约束较好，而在蓝色方向上会因为x较小的扰动产生很大的变化，因此蓝色方向为退化方向。

2.退化感知

我们想测量对于上述方程组所得可行解对于微小扰动的敏感度，以此来测量方程组的约束条件是否良好，是否在某些方向上出现退化。

 添加一个方程  经过x0点，并且与向量c垂直。沿着向量c方向对约束方程增加一个微小扰动，求得x在c方向(360°范围内)上的最大位移

最大化确定最不稳定方向，定义一个退化因子

D的物理意义在于D越大说明解的稳定性越好，D越小说明解的稳定性越差

对于线性化问题，D只与A相关与b无关，并且, 其中为的最小特征值，证明见论文。

通过实验确定阈值判断退化与否，

 假如最小的m个特征值被判断为退化反向，可以构建三个矩阵，退化矩阵，非退化矩阵，整个空间

 n为空间维数。

设xp为预测的位姿(可以由匀速模型或者惯导得到)，xu为最小二乘解求得位姿。最终解为

其中xp’和xu‘为预测位姿在退化方向的投影，求得位姿在非退化方向投影。