论文阅读：FEJ-VIRO: A Consistent First-Estimate Jacobian Visual-Inertial-Ranging Odometry

FEJ-VIRO: A Consistent First-Estimate Jacobian Visual-Inertial-Ranging Odometry这篇论文是发表在2022年IROS上的一篇文章，这篇文章基于MSCKF进行改进，添加了UWB传感器；提出了一种用于初始化UWB锚点和状态增强的长短滑动窗口策略；推导了视觉惯性紧耦合优化- range估计器的可观测性分析，指出了理想情况下VIRO存在的4个不可观测方向并提出了一种FEJ - VIRO框架，扩展了FEJ技术以保持一致性，从而显著提高了估计精度。

 一、问题背景

本文的应用背景是live operation of robotic systems的情况，要求高稳定性；目的是终端实现自主定位，对计算效率与成本要求高；VIO系统有累计漂移误差。结合超宽带( Ultra Wideband，UWB )测量是一种很有前途的解决方案，可以低成本地部署在室内和室外环境中。本文主要针对UWB锚节点位置未知且在运行过程中需要同时估计的情况进行研究。

二、主要假设

1、假设环境中有三个位置未知的UWB锚点。

2、基于MSCKF的基本假设。

三、理论流程

1、状态估计器设计

a.状态向量

(1)k时刻的状态向量矩阵

(2)k时刻的IMU状态向量

(3)k时刻的UWB锚点在全局帧中的位置表示

(4)IMU位姿短窗

(5)IMU位姿长窗

(（4）、（5）用于估计UWB锚点位置)

b.IMU传播

c.相机测量模型

d.UWB测距更新

（1）基于IMU得到的载体位置，经过变换后得到载体上的UWB传感器位置，将其减去UWB锚点的全局位置得到载体与UWB锚点间的距离：（这里并没有用到UWB传感器的真实测量值）

（2）锚点与锚点之间的距离：

（3）得到雅可比矩阵用于EKF更新

e.UWB和相机时间同步

2、UWB初始化

长短窗内有n个IMU位姿，可以建立如下方程组：（这里我有个问题没解决：这里的dri是否和上面的d（1）的dri不一样，因为如果一样是由IMU得到的话这个方程组就是左右一样了，这里的dri是载体UWB直接的测量值吗？我猜测是和上面不一样的，上面之所以直接用IMU算得的，因为那就是最优值了这里的测量值也是朝那个逼近。）

由于上述方程组总是病态，建立如下优化问题，使用高斯牛顿法求解，从而得到锚点全局位置：

通过线性化估计该初始化方法的协方差矩阵如下 ：

3、一致性分析（这里没太看懂）

定理1：由定理1可知，由于全局平移和偏航旋转是不可观测的，采用视觉-惯性-测距估计器无法实现无漂移状态估计。但是可以通过多传感器融合算法来提高估计精度。

定理2：定理2指出，如果在最新的估计状态下对测距测量模型进行线性化，VIRO系统会获得关于全局偏航方向的虚假信息，从而导致估计器的不一致性。

为了设计一致状态估计器，我们需要迫使估计模型(实际案例)的不可观测子空间与理想情况相同，从而防止虚假信息增益，减少不一致性。在基于EKF的VIO中也存在不一致性问题，并且已经提出了几种设计一致性滤波器的方法，如第一估计雅可比( FEJ )思想[ 27 ]和OC方法[ 28 ]。在本文中，我们将传统的FEJ扩展到VIRO系统中，通过在第一次估计的机器人位姿和UWB锚点位置处评估测量雅克比矩阵。我们遵循[ 27 ]的方法对IMU的传播和视觉更新进行FEJ。随着这些变化，实际情况下的可观测性矩阵O的零空间保持与理想情况下相同。

四、实验方案

基于OpenVINS进行实现。

1、仿真实验

该文扩展了Open - VINS提供的仿真实用程序，额外模拟了3个UWB锚节点的测距测量，噪声密度为0.15 m，偏差为- 0.75 m。为了验证不一致性问题，在三个不同的仿真数据集上测试了所提出的VIRO，并评估了它们的归一化估计误差平方和。

为了进一步评估所提出的长短窗口结构，将其与仅使用短滑动窗口的方法进行UWB初始化的比较。(见图FEJ-VIRO-S)

当在当前状态估计下对测距测量模型进行线性化时，不可观测方向的数量从4个减少到3个，其中关于引力的旋转变为(错误)可观测。为了进一步验证理论，绘制了在三个模拟数据集上测试的VIRO和FEJ - VIRO (如下图)的3 - σ界限。由图可知，在UWB锚点初始化后，VIRO估计的z轴旋转3 - σ (不确定度)迅速减小。然而，已经证明视觉惯性测距估计器关于重力的旋转是不可观测的，因此VIRO对估计的z轴旋转是过度自信的。而过度自信是导致不一致性的主要原因。

2、真实场景实验验证定位效果

使用南洋理工VIRAL数据集，比较了openvins、VINS-Fusion、VIRO、VIRO(FEJ)四种定位方案的效果。

五、创新总结

提出了一种用于初始化UWB锚点和状态增强的长短滑动窗口策略；推导了视觉惯性紧耦合优化- range估计器的可观测性分析，指出了理想情况下VIRO存在的4个不可观测方向提出了一种FEJ - VIRO框架，扩展了FEJ技术以保持一致性，从而显著提高了估计精度。

滤波器的方法计算效率高，稳定性高，适用于针对UWB锚点位置未知且在作业过程中需要同时估计的情况。直接EKF更新操作来融合UWB测量会导致不一致性，这篇文章解决了不一致性的问题。

主要创新点就是两点：UWB锚点估计，FEJ技术保持滤波器融合一致性。