SLAM中的状态估计优化退化问题分析与代码实现

参考文献On degeneracy of optimization-based state estimation problems 发表于2016年ICRA会议。

其核心理念较为清晰，仅需运用线性代数的基本理论即可理解。通过采用Guassian-Newton方法，增量方程中的Hessian矩阵能够反映出该优化问题的退化特性。为确保问题的可解性，必须保证该矩阵具有正定性质。

在求解增量方程过程中，可以首先计算Hessian矩阵的特征值与特征向量。其中，对应较小特征值的特征向量所指示的方向即为退化方向。即便Hessian矩阵不具备正定性，也可以通过这种方式进行求解，并借助其他传感器数据对退化方向进行补充。由此可构建出一种松耦合串联式多传感器位姿估计系统。

论文中所描述的算法流程如下：

以下展示基于eigen库的算法实现方式，具体操作步骤如下：

通常情况下，我们采用 x_delta = -(J TJ).inv() J Tb 的形式进行求解，随后将 x_delta 以及 J TJ 的计算结果传递至 modifyso