VINS_FUSION的global融合思想

VINS_FUSION的global融合思想

---

使用全局融合的原因

• 局部观测存在累计漂移,室外大场景难以运行
• 局部观测数据缺乏全局约束,导致在不同的位置进行建图和定位都会得到不同坐标系下的结果,难以进行全局结果的观测

GPS的缺点

• 数据不平滑,噪声过大
• 输出频率低(1hz)

融合的目的

• 局部精确
• 全局零漂

算法架构

观测和状态约束关系

• state: 状态量,位姿,速度,bias等
• Local Factor: 局部观测约束,VIO相对位姿变换, 计算的是相邻两帧之间位姿的残差

GPS 融合思路

• 经纬度坐标系转换为大地坐标系(东北天坐标系ENU:local East North Up)
• 第一帧的gps点为坐标系原点
• 求最小二乘
  $\begin{array}{rl}{X}^{\ast }& ={\mathrm{argmax}}_x\prod _{t=0}^n\prod _{k\in S}exp(-\frac{1}{2}\Vert z_t^k-h_t^k(X)){\Vert }_{{\Omega }_t^k}^2)\\ & ={\mathrm{argmin}}_x\sum _{t=0}^n\sum _{k\in S}\Vert z_t^k-h_t^k(X)){\Vert }_{{\Omega }_t^k}^2.\end{array}$
• 注意这里的马氏距离要进行一个分解才可以进行最小二乘
  ∥ r ∥ Ω 2 = r T Ω − 1 r \|r\|^2_Ω = r^T\Omega^{-1}r ∥r∥Ω2​=r