SLAM（状态估计）中的退化问题

即On Degeneracy of Optimization-based State Estimation Problems论文理解。

一、前言

无论是在激光还是在视觉SLAM中，当遇到特征点稀少的场景（空旷、低纹理等）时，都有可能导致退化。那如何解决呢？一般两种思路，一种是根据环境信息来检测是否退化，检测出退化后采用多传感器互补融合。另一种是在求解优化问题时，检查优化过程中是否发生退化，检测到退化后对求解过程进行修改，zhang ji的论文就是第二种思路。

二、问题描述

论文中是通过对状态空间中约束结构的研究，来估计状态估计优化问题中的退化现象。

定义x为一个n*1的状态向量，状态估计问题通常如下：

无论问题（1）是线性还是非线性的，我们都可以将其转换成一个线性问题。因此论文直接针对线性问题进行研究，定义如下：

给定式（3），计算出是否退化以及退化方向，避免发生退化时，退化方向对解产生错误影响。这里有两个假设：

1.假设考虑传感器的噪声，对矩阵A进行适当的加权。
2. 假设满秩。不退化时，状态估计由传感器真实测量决定。

三、退化估计

1. 退化因子（degeneracy factor）

图中黑线代表式（3）中的约束，蓝色点代表真值解，橙色线经过真值解，是为了验证系统退化性，随机插入的约束。

给定橘色线的约束 ，方向不固定。沿着c的方向移动δd的距离，δxc移动的距离最大的方向即为退化方向。相当于在某个方向给个扰动，哪个方向的扰动使真值产生最大的偏差，哪个方向就是退化方向。

2. 退化因子D是A的函数，与b无关。公式证明见论文，涉及到左右伪逆的计算，不难看懂。

3. 对于线性系统（3），D = λmin + 1,其中.λmin是A^T * A的最小特征值，证明过程用到了Rayleigh quotient。

结论：对于线性系统（3），求解A^T * A的特征值和特征向量，当特征值小于一定阈值时，其所对应的特征向量即为退化方向。

四、退化处理

在求出退化方向之后，需要对相应方向的增量进行处理，避免影响求解。核心思想是，当检测到退化时，退化方向使用预测值（xp），非退化方向使用更新值(xu)，二者相加。

整个处理流程如下：

五、参考

1. 《On Degeneracy of Optimization-based State Estimation Problems》

2.  SLAM中的退化问题_slam退化-CSDN博客

3. LOAM SLAM原理之防止非线性优化解退化-CSDN博客

4. SLAM中状态估计优化退化问题(degraded optimization problem)浅析和源码实现_slam退化问题-CSDN博客