【MSCKF && FEJ】SLAM中FEJ的含义,做法和原因,FEJ（First Estimate Jacobians）

最新推荐文章于 2025-12-07 19:40:17 发布

原创最新推荐文章于 2025-12-07 19:40:17 发布 · 1k 阅读

16 ·

CC 4.0 BY-SA版权

文章标签：

#人工智能 #vr #mr #xr

MSCKF 专栏收录该内容

16 篇文章

订阅专栏

**FEJ（First Estimate Jacobians）**是一个在基于滤波器的SLAM（尤其是EKF-SLAM）中非常重要的概念，用于解决一个关键的“不一致性”问题。

1. 含义（What is FEJ?）

FEJ，即 “首次估计雅可比”，它的核心思想是：

对于一个状态变量，在整个滤波器更新过程中，其雅可比矩阵应该在它第一次被估计出来的时刻（线性化点）进行计算，并且之后保持不变。

换句话说，系统在计算某个状态变量的观测模型雅可比矩阵时，永远使用该变量第一次被估计出来时的值，而不是使用当前迭代中最新、最优的估计值。

2. 原因：为什么要用FEJ？（Why FEJ?）

要理解FEJ，必须先理解它要解决的问题——SLAM系统的“不一致性”。

问题的根源：线性化点不一致

在EKF-SLAM中，我们通过线性化非线性的运动和观测模型来传递协方差（不确定性）。线性化需要在某个线性化点（即状态的当前最佳估计值）上进行。

一个理想的情况是：如果我们能在一个“真实”的线性化点上计算所有雅可比矩阵，那么线性化误差是最小的。

但现实情况是：

当我们观测到一个旧的地标（一个之前已经加入到状态向量中的地标）时，我们会有一个观测方程，它同时依赖于当前的机器人位姿 (x_k) 和这个旧的地标位置 (m_j)。
在EKF更新步骤中，我们需要计算这个观测方程的雅可比矩阵 (H)。这需要对 (x_k) 和 (m_j) 分别求导。
问题来了：我们应该在哪个线性化点上计算 (H)？
- 选项A：在 (x_k) 和 (m_j) 各自的“最优当前估计值” 上计算。
- 选项B：在 (x_k) 和 (m_j) 第一次被共同估计出来的时刻的值 上计算。

如果我们选择选项A（即标准的EKF做法），就会引入严重的问题。

一个简化的例子：

在 (t_1) 时刻，机器人第一次观测到地标 (m_j)，并将其初始化加入状态向量。此时，机器人位姿 (x_1) 和地标 (m_j) 的估计是高度相关的（因为 (m_j) 的位置是根据 (x_1) 和观测值计算出来的）。
到了 (t_k) 时刻，机器人再次观测到 (m_j)。此时，机器人位姿已经更新为 (x_k)，地标 (m_j) 的位置也经过了多次更新。
在标准EKF中，你会用 最新的 (x_k) 和 最新的 (m_j) 来计算雅可比矩阵 (H_{x_k}) 和 (H_{m_j})。

这会导致什么？

(x_k) 和 (m_j) 的最新值都是在考虑了后续所有观测信息后得到的最优估计。
然而，这些后续的观测信息本身也依赖于之前对 (m_j) 的估计。这造成了信息被重复利用。
从图优化的角度看，相当于在信息矩阵中人为地添加了一些本不存在的链接（或称为“环”），错误地增加了系统的信息量。
其直接后果是，滤波器会过度自信——它计算出的协方差矩阵会远小于真实的误差。这意味着滤波器认为自己的估计非常准，但实际误差可能很大。这种“自信”会导致滤波器在未来拒绝那些本可以修正它错误的观测信息，最终导致估计结果发散。

FEJ的解决方案：
FEJ规定，在 (t_k) 时刻计算对地标 (m_j) 的雅可比 (H_{m_j}) 时，不使用 (m_j) 的最新估计值，而是使用它在 (t_1) 时刻第一次被估计出来的值。对于机器人位姿 (x_k)，则使用其当前值。这样就保证了线性化点的一致性，切断了信息重复利用的途径，使得系统协方差能够更真实地反映估计的不确定性。

3. 做法：如何实现FEJ？（How to Implement FEJ?）

在实践中，实现FEJ需要遵循以下步骤：

状态初始化：当一个新地标首次被观测到时，按照标准流程将其初始化并加入到状态向量和协方差矩阵中。记录下此时机器人位姿和该地标位置的估计值，作为该地标的“首次估计”。
后续观测更新：
- 当机器人再次观测到一个已经存在的地标时，进入观测更新步骤。
- 在计算观测模型的雅可比矩阵 (H) 时：
  - 对于机器人当前位姿 (x_k) 的部分 (H_x)，使用 (x_k) 的最新先验估计值进行计算。
  - 对于地标位置 (m_j) 的部分 (H_m)，不使用 (m_j) 的最新估计值，而是从存储的记录中取出它在第一次被估计时的值进行计算。
- 使用这个由“最新位姿”和“旧地标”组合计算出的雅可比矩阵 (H) 来进行EKF的更新步骤。
保持不变：对于同一个地标，在其整个生命周期内，所有涉及它的观测更新，其雅可比矩阵 (H_m) 部分都固定使用第一次的线性化点。

注意：FEJ主要应用于旧地标的观测更新。对于新地标的初始化，以及运动模型的雅可比计算，通常不受此规则影响。

总结

方面	标准EKF	使用FEJ的EKF
核心思想	始终使用状态的最新估计值作为线性化点。	对每个状态变量，固定使用其第一次被估计的值作为线性化点。
雅可比计算	$f(\text{最新位姿}， \text{最新地标})$	$f(\text{最新位姿}， \text{首次地标})$
问题	线性化点不一致，导致信息重复利用，系统过度自信（不一致）。	解决了信息重复利用问题，提高了系统的一致性。
优点	实现简单。	显著改善滤波器的 consistency，协方差更能反映真实误差。
缺点	系统容易发散。	由于线性化点可能不是最优的，会引入更大的线性化误差，可能影响精度。

总而言之，FEJ是一种用“精度”换取“一致性”的策略。 它通过接受一个可能不是最理想的线性化点，来避免EKF-SLAM系统中一个根本性的缺陷，从而防止滤波器过度自信和发散。它是基于滤波器的SLAM能够稳定工作的关键技巧之一。在现代基于图优化的SLAM中，由于线性化是在每次迭代的最新点上进行的，并且会反复迭代直至收敛，因此不存在FEJ所解决的这个问题。

下面来结合MSCKF梳理FEJ，地理解为什么FEJ如此重要，以及MSCKF是如何通过其独特的架构来“巧妙地规避”了对FEJ的强烈需求。

1. 核心含义：问题根源的一致性

FEJ（First Estimate Jacobians） 的根本目的是解决线性化点不一致导致的估计误差与协方差不一致问题。

在标准EKF-SLAM中：状态向量包含当前位姿和所有路标点。当一个旧路标被重新观测时，计算雅可比矩阵需要使用状态的最新值。但该路标的最新值已经包含了之前观测的信息，导致信息在滤波器中被重复利用，使得协方差被人为地缩小，滤波器变得“过度自信”，最终发散。
FEJ的解决方案：强制规定，对于任何一个状态变量（尤其是路标），在其整个生命周期内，计算其雅可比矩阵时，永远只使用它第一次被估计时的值。这确保了线性化点的一致性，避免了信息重复利用。

2. MSCKF的动机：从根本上规避问题

MSCKF的诞生，正是为了克服标准EKF-SLAM的固有缺陷，其中就包括上述的一致性难题。

MSCKF的核心思想是：不将路标点纳入状态向量。

标准EKF-SLAM状态向量： $X = [x_{robot}, m_1, m_2, ..., m_n]$
MSCKF状态向量： $X = [x_{robot}^{k}, x_{robot}^{k-1}, ..., x_{robot}^{k-N}]$ （仅包含一系列历史相机位姿）

这个根本性的改变，带来了革命性的优势。

3. 做法与原因：MSCKF如何工作及为何与FEJ相关

MSCKF的工作流程：

状态传播：使用IMU数据，通过动力学模型预测（传播）当前机器人位姿的均值和协方差。这与标准EKF相同。
特征跟踪：当相机采集到图像时，提取特征点并进行跨帧跟踪。MSCKF不会立即将这些特征点初始化为状态向量中的路标。
延迟更新：当一个特征点被跟踪了若干帧后（例如，从第 $t_i$ 帧到第 $t_j$ 帧），并且即将丢失（移出视野或跟踪失败）时，MSCKF才利用所有这些观测信息。
- 此时，我们有一个多视图几何约束：同一個3D点（虽然我们不知道它的确切位置）在一系列已知位姿（状态向量中的历史位姿）下的投影位置。
几何约束与边缘化：
- MSCKF通过三角化得到一个该特征点的3D位置初始估计（注意，这只是为了构造约束，并不会将其加入状态）。
- 关键的一步来了：它利用所有观测构建一个关于这个3D点和一系列历史相机位姿的几何残差。
- 然后，它通过Schur补将这个3D点边缘化掉。边缘化之后，我们就得到了一个纯粹存在于历史相机位姿之间的约束关系。
EKF更新：将这个由特征点边缘化产生的、纯位姿间的约束，作为一个虚拟的“观测”，用于更新MSCKF状态向量中的所有相关历史位姿。

为什么MSCKF与FEJ的关系变得不同了？

核心原因：MSCKF在更新时，线性化点天然就是一致的。

在标准EKF-SLAM中：问题在于一个路标（状态）和一个当前位姿（状态）的线性化点来自不同时刻的估计。
在MSCKF中：
- 更新步骤所约束的所有变量（即那些历史位姿 $x_{i}, x_{i+1}, ..., x_{j}$ 在发生观测的那一刻，都已经是状态向量的一部分了。
- 当MSCKF执行第4步的边缘化，并准备进行第5步的更新时，它会固定使用这些历史位姿的当前最新估计值（即先验估计）作为线性化点，来构造整个观测模型和雅可比矩阵。
- 由于这些位姿在状态向量中，并且我们是在同一个滤波器周期、使用同一套先验估计来线性化所有与之相关的变量，因此线性化点天然就是一致的。

一个生动的比喻：

标准EKF-SLAM：像一个不断修改旧档案的秘书。当有新证据提到“张三”时，秘书不仅更新对“张三”的最新描述，还会回头去修改最初关于“张三”的档案记录。这导致了信息混乱和过度自信。
FEJ：规定秘书在后续所有文件中，凡提及“张三”，必须引用最初档案里的原始描述，不允许修改。这解决了混乱，但可能因为描述过时而引入误差。
MSCKF：秘书根本不建立“张三”的独立档案。他只记录“李四在A地看到了张三”、“王五在B地看到了张三”这类事件。当需要推断时，他一次性综合所有目击报告（李四、王五的证词），来修正对“A地”、“B地”等位置的认知。所有线性化都基于李四、王五当前位置的最新认知，一致性自然保持。

总结对比

方面	标准EKF-SLAM + FEJ	MSCKF
状态向量	当前位姿 + 所有地图点	仅一系列历史位姿
问题	路标与位姿线性化点不一致，导致不一致性。	天然规避了此问题。
解决方案	算法规则：强制使用路标的第一次估计值计算雅可比。	架构设计：通过不将路标纳入状态，并在更新时一次性处理所有相关观测，保证线性化点一致。
FEJ的角色	必需品。没有FEJ，滤波器大概率会发散。	非必需品。MSCKF的更新步骤本身就满足了一致性要求。在一些严格的数学分析中，为了达到最优的一致性，对MSCKF中位姿的线性化也可能引入类似FEJ的思想，但其必要性和紧迫性远低于标准EKF-SLAM。
优势	能够维持一个全局地图。	高一致性、高计算效率（状态向量维度小）、更鲁棒。
劣势	状态向量庞大，计算复杂度高，且必须依赖FEJ等技巧来维持稳定。	不维持全局地图，只有位姿轨迹，地图信息是隐式的。

结论：

MSCKF通过其独特的、不将路标点状态化的架构，从根本上改变了问题的格局。它将SLAM中的不一致性问题，从一个持续存在的、需要FEJ这种“补丁”来修复的顽疾，转变为一个在局部更新中可以自然满足一致性条件的、更易于管理的问题。因此，可以说MSCKF是一种比“标准EKF-SLAM + FEJ”更优雅、更强大的解决方案。