SLAM中状态估计优化退化问题(degraded optimization problem)浅析和源码实现

参考论文 On degeneracy of optimization-based state estimation problems发表在2016 ICRA上

 

原理非常直观，用线性代数的基础知识。用Guassian-Newton， 增量方程的Hessian matrix包含了求解该优化问题的退化信息。必须要求其为正定矩阵。

求解增量方程时，可以将Hessian matrix的eigen value， eigen vector先解出来。较小的eigen vector对应的eigen value就代表类退化方向。这样Hessian不为正定也可以求解了，退化的方向可由其他传感器补充。从而构建一个松耦合串连式多传感器位姿估计框架。

论文的算法流程为：

下面给出我用eigen的算法实现，具体实施如下：

一般我们解为 x_delta = -(JT*J).inv() *JT*b, 将x_delta， JT*J放到modifysolution即可。

//input: 
//    eigThreshlod: for eigen value
//    matAtA: Hessian matrix for the normal equation
//    matX: original solution
//output:
//    matX: modified solution
bool modifySolution(double eigThreshlod , Eigen::M