06 关于边缘化、FEJ、可观性的讨论（1）

06 一致性、弱可观与接触约束：从 FEJ 到 Contact Constraint 的秩分析

这一篇回答四个常见但很容易混在一起的问题：

1. 为什么 FEJ 会改善 consistency？
2. 为什么边缘化会破坏可观性？
3. 为什么某个状态只是弱可观，而不是完全可观或完全不可观？
4. 额外拓展（VINS-mono并没有这个内容）：腿式里程计中，为什么 Contact Constraint 能提高秩？

这四个问题的共同核心是：

$$\text{测量到底给状态空间的哪些方向提供了信息？}$$

如果某个方向没有信息，它应该留在零空间里；如果某个方向有一点信息但很弱，它会对应很小的奇异值或特征值；如果新约束提供了独立信息，它就可能提高观测矩阵或信息矩阵的秩。

1. 先统一几个概念

设系统状态为：

$$\mathbf{x}\in {\mathbb{R}}^n$$

其中：

• $\mathbf{x}$ 表示需要估计的全部状态；
• $n$ 表示状态维度。

一个测量残差写成：

$$\mathbf{r}(\mathbf{x})=\mathbf{z}-h(\mathbf{x})$$

其中：

• $\mathbf{r}(\mathbf{x})$ 表示残差；
• $\mathbf{z}$ 表示传感器测量；
• $h(\mathbf{x})$ 表示由状态预测测量的函数。

在当前线性化点 $\bar{\mathbf{x}}$ 处做一阶近似：

$$\mathbf{r}(\bar{\mathbf{x}}⊞\delta \mathbf{x})\approx \mathbf{r}(\bar{\mathbf{x}})+\mathbf{J}\delta \mathbf{x}$$

其中：

• $\bar{\mathbf{x}}$ 表示线性化点；
• $\delta \mathbf{x}$ 表示局部状态扰动；
• $⊞$ 表示流形上的加法；
• $\mathbf{J}$ 表示残差对局部扰动的 Jacobian。

加权最小二乘的二次项为：

$${\Vert \mathbf{r}\Vert }_{\mathbf{\Omega }}^2={\mathbf{r}}^{\top }\mathbf{\Omega r}$$

其中：

• $\mathbf{\Omega }$ 表示信息矩阵；
• $\mathbf{\Omega }={\mathbf{\Sigma }}^{-1}$；
• $\mathbf{\Sigma }$ 表示测量噪声协方差。

线性化后，对状态增量的近似 Hessian 为：

$$\mathbf{H}={\mathbf{J}}^{\top }\mathbf{\Omega J}$$

如果有多个残差，则：

$$\mathbf{H}=\sum _i{\mathbf{J}}_i^{\top }{\mathbf{\Omega }}_i{\mathbf{J}}_i$$

其中：

• ${\mathbf{J}}_i$ 表示第 $i$ 个残差的 Jacobian；
• ${\mathbf{\Omega }}_i$ 表示第 $i$ 个残差的信息矩阵。

这个 $\mathbf{H}$ 可以理解为局部信息矩阵。某个方向 $\mathbf{n}$ 上的信息量可以看：

$${\mathbf{n}}^{\top }\mathbf{Hn}$$

其中 $\mathbf{n}$ 表示状态空间中的一个扰动方向。

如果：

$${\mathbf{n}}^{\top }\mathbf{Hn}=0$$

则该方向没有被当前线性化测量约束。由于 $\mathbf{H}$ 是半正定矩阵，上式等价于：

$$\mathbf{Hn}=0$$

也等价于：

$${\mathbf{J}}_i\mathbf{n}=0\text{对所有残差 }i$$

这就是零空间与不可观方向的联系。

2. 可观、不可观、弱可观

2.1 不可观

如果存在非零方向 $\mathbf{n}$，使得所有测量对这个方向都没有响应：

$$\mathbf{Jn}=0$$

那么 $\mathbf{n}$ 是不可观方向。

例如在没有 GPS、磁力计、已知地图的 VIO 中，全局平移和全局 yaw 通常不可观：

$$\dim (\mathcal{N})=4$$

其中：

• $\mathcal{N}$ 表示不可观子空间；
• $4$ 表示三维全局平移加一维全局 yaw。

2.2 可观

如果某个方向 $\mathbf{n}$ 满足：

$$\mathbf{Jn}\ne 0$$

说明沿着这个方向改变状态会改变测量预测，因此测量对这个方向有信息。

从信息矩阵看，就是：

$${\mathbf{n}}^{\top }\mathbf{Hn}>0$$

2.3 弱可观

弱可观不是“完全不可观”。它表示这个方向确实有信息，但信息很弱。

对 Jacobian 做奇异值分解：

$$\mathbf{J}=\mathbf{US}{\mathbf{V}}^{\top }$$

其中：

• $\mathbf{U}$ 表示左奇异向量矩阵；
• $\mathbf{S}$ 表示奇异值对角矩阵；
• $\mathbf{V}$ 表示右奇异向量矩阵。

如果某个方向对应的奇异值为零：

$${\sigma }_i=0$$

该方向不可观。

如果某个方向对应的奇异值很小但非零：

$$0<{\sigma }_i\ll 1$$

该方向弱可观。

对应到 Hessian：

$$\mathbf{H}={\mathbf{J}}^{\top }\mathbf{J}=\mathbf{V}{\mathbf{S}}^{\top }\mathbf{S}{\mathbf{V}}^{\top }$$

所以 Hessian 的特征值近似为：

$${\lambda }_i={\sigma }_i^2$$

弱可观方向就是：

$$0<{\lambda }_i\ll 1$$

直观解释是：

$$\text{这个方向不是完全测不到，但测得很软、很慢、很容易受噪声影响。}$$

3. Consistency 到底是什么意思

Consistency 通常翻译为“估计一致性”。它关心的是：

$$\text{估计器对自身不确定性的判断是否合理？}$$

如果一个方向本来不可观，估计器就不应该在这个方向上变得非常自信。

设估计误差为：

$$\tilde{\mathbf{x}}=\mathbf{x}-\hat{\mathbf{x}}$$

其中：

• $\mathbf{x}$ 表示真实状态；
• $\hat{\mathbf{x}}$ 表示估计状态；
• $\tilde{\mathbf{x}}$ 表示估计误差。

设估计器给出的协方差为：

$$\mathbf{P}=\mathbb{E}\left[\tilde{\mathbf{x}}{\tilde{\mathbf{x}}}^{\top }\right]$$

现在取状态空间中的一个方向：

$$\mathbf{n}$$

其中 $\mathbf{n}$ 可以理解为“我们关心的某个状态扰动方向”。例如在 VIO 中，$\mathbf{n}$ 可以表示全局平移方向，也可以表示全局 yaw 方向。

如果估计误差为 $\tilde{\mathbf{x}}$，那么误差在方向 $\mathbf{n}$ 上的投影是一个标量：

$$e_n={\mathbf{n}}^{\top }\tilde{\mathbf{x}}$$

其中：

• $e_n$ 表示估计误差沿方向 $\mathbf{n}$ 的分量；
• 如果 $e_n$ 很大，说明状态在这个方向上误差很大；
• 如果 $e_n$ 很小，说明状态在这个方向上估计得比较准。

这个标量误差的方差为：

$$\mathrm{Var}(e_n)=\mathbb{E}\left[e_n^2\right]$$

代入 $e_n={\mathbf{n}}^{\top }\tilde{\mathbf{x}}$：

$$\mathrm{Var}(e_n)=\mathbb{E}\left[\left({\mathbf{n}}^{\top }\tilde{\mathbf{x}}\right)\left({\mathbf{n}}^{\top }\tilde{\mathbf{x}}\right)\right]$$

把第二项改写成：

$${\mathbf{n}}^{\top }\tilde{\mathbf{x}}={\tilde{\mathbf{x}}}^{\top }\mathbf{n}$$

于是：

$$\mathrm{Var}(e_n)=\mathbb{E}\left[{\mathbf{n}}^{\top }\tilde{\mathbf{x}}{\tilde{\mathbf{x}}}^{\top }\mathbf{n}\right]$$

因为 $\mathbf{n}$ 不是随机变量，可以移到期望外面：

$$\mathrm{Var}(e_n)={\mathbf{n}}^{\top }\mathbb{E}\left[\tilde{\mathbf{x}}{\tilde{\mathbf{x}}}^{\top }\right]\mathbf{n}$$

再利用协方差定义：

$$\mathbf{P}=\mathbb{E}\left[\tilde{\mathbf{x}}{\tilde{\mathbf{x}}}^{\top }\right]$$

得到：

$${\mathbf{n}}^{\top }\mathbf{Pn}$$

也就是：

$$\mathrm{Var}(e_n)={\mathbf{n}}^{\top }\mathbf{Pn}$$

这就是 ${\mathbf{n}}^{\top }\mathbf{Pn}$ 的含义：它表示估计误差在方向 $\mathbf{n}$ 上的方差。

如果方向 $\mathbf{n}$ 是不可观方向，系统没有真实测量信息来约束它。因此估计器不应该让：

$${\mathbf{n}}^{\top }\mathbf{Pn}$$

变得过小。因为过小意味着估计器认为“我在这个方向上很确定”，而这和不可观性矛盾。

从信息矩阵角度看：

$$\mathbf{P}\approx {\mathbf{H}}^{-1}$$

这个式子来自高斯近似。线性化后的最小二乘代价可以写成：

$$E(\delta \mathbf{x})=\frac{1}{2}\delta {\mathbf{x}}^{\top }\mathbf{H}\delta \mathbf{x}$$

其中：

• $E(\delta \mathbf{x})$ 表示局部二次能量；
• $\delta \mathbf{x}$ 表示状态扰动；
• $\mathbf{H}$ 表示 Hessian 或信息矩阵。

对应的高斯分布为：

$$p(\delta \mathbf{x})\propto \exp \left(-\frac{1}{2}\delta {\mathbf{x}}^{\top }\mathbf{H}\delta \mathbf{x}\right)$$

而一个高斯分布也可以写成：

$$p(\delta \mathbf{x})\propto \exp \left(-\frac{1}{2}\delta {\mathbf{x}}^{\top }{\mathbf{P}}^{-1}\delta \mathbf{x}\right)$$

对比这两个式子，就得到：

$$\mathbf{H}\approx {\mathbf{P}}^{-1}$$

也就是：

$$\mathbf{P}\approx {\mathbf{H}}^{-1}$$

严格来说，如果系统存在不可观方向，$\mathbf{H}$ 是奇异的，不能直接求普通逆。这时可以理解为在可观子空间上求逆，或者使用伪逆、阻尼、gauge fixing 后的近似协方差。这里写 $\mathbf{P}\approx {\mathbf{H}}^{-1}$，表达的是“信息越大，协方差越小”的局部高斯近似关系。

为了看清楚方向 $\mathbf{n}$ 上的信息和方差的关系，假设 $\mathbf{n}$ 是单位方向：

$$\Vert \mathbf{n}\Vert =1$$

并且它是 $\mathbf{H}$ 的一个特征方向：

$$\mathbf{Hn}=\lambda \mathbf{n}$$

其中 $\lambda$ 表示该方向上的信息强度。沿着这个方向取扰动：

$$\delta \mathbf{x}=s\mathbf{n}$$

其中 $s$ 表示沿方向 $\mathbf{n}$ 移动的标量幅度。代入二次能量：

$$E(s\mathbf{n})=\frac{1}{2}{s}^2{\mathbf{n}}^{\top }\mathbf{Hn}$$

由于 $\mathbf{Hn}=\lambda \mathbf{n}$ 且 ${\mathbf{n}}^{\top }\mathbf{n}=1$：

$$E(s\mathbf{n})=\frac{1}{2}\lambda s^2$$

这说明 $\lambda$ 越大，沿方向 $\mathbf{n}$ 偏离一点点的代价越大，优化器越“确信”这个方向不能乱动。

如果 $\lambda >0$，对应协方差在这个方向上的大小近似为：

$${\mathbf{n}}^{\top }\mathbf{Pn}\approx \frac{1}{\lambda }$$

如果 $\lambda$ 很大：

$$\lambda \uparrow \Rightarrow {\mathbf{n}}^{\top }\mathbf{Pn}\downarrow$$

如果 $\lambda =0$：

$$E(s\mathbf{n})=0$$

表示沿这个方向怎么移动都没有二次代价。这个方向就是不可观方向，理论上它对应无限大不确定性，或者在数值系统中表现为非常大的协方差。

因此，如果某个本来不可观的方向 $\mathbf{n}$ 被错误地加入了信息：

$${\mathbf{n}}^{\top }\mathbf{Hn}>0$$

那么对应协方差会变小：

$${\mathbf{n}}^{\top }\mathbf{Pn}\downarrow$$

这就叫不一致：系统误以为自己知道了其实传感器没有告诉它的东西。

在 VIO 中，这种不一致常表现为：

• 全局位置方向协方差过小；
• 全局 yaw 方向被错误约束；
• bias 或尺度在激励不足时被估计得过度自信；
• 轨迹短期看起来平滑，但长期误差或漂移模式异常。

4. 为什么 FEJ 会改善 Consistency

FEJ 的全称是 First Estimate Jacobian。核心做法是：

$$\text{在 first estimate 处计算 Jacobian，并固定使用。}$$

设某个残差为：

$$\mathbf{r}(\mathbf{x})$$

普通重线性化会在每次迭代的当前估计 ${\mathbf{x}}^{(t)}$ 处计算：

$${\mathbf{J}}^{(t)}=\frac{\partial \mathbf{r}}{\partial \delta \mathbf{x}}{|}_{{\mathbf{x}}^{(t)}}$$

FEJ 则使用第一次估计 $\bar{\mathbf{x}}$：

$${\mathbf{J}}_{FEJ}=\frac{\partial \mathbf{r}}{\partial \delta \mathbf{x}}{|}_{\bar{\mathbf{x}}}$$

后续即使当前状态变成 ${\mathbf{x}}^{(t)}$，仍然使用：

$${\mathbf{J}}_{FEJ}$$

但残差中的状态差分仍然可以变化：

$$\delta \mathbf{x}={\mathbf{x}}^{(t)}\boxminus \bar{\mathbf{x}}$$

4.1 真正的问题不是“Jacobian 变了”，而是“零空间变了”

假设真实非线性系统有一个不可观变换：

$${\mathcal{T}}_{\alpha }(\mathbf{x})$$

其中：

• ${\mathcal{T}}_{\alpha }$ 表示对状态施加某个不可观变换；
• $\alpha$ 表示该变换的参数。

不可观意味着：

$$\mathbf{r}(\mathbf{x})=\mathbf{r}({\mathcal{T}}_{\alpha }(\mathbf{x}))$$

对 $\alpha$ 求导，可以得到对应的不可观方向：

$$\mathbf{N}(\mathbf{x})=\frac{\partial {\mathcal{T}}_{\alpha }(\mathbf{x})}{\partial \alpha }{|}_{\alpha =0}$$

其中 $\mathbf{N}(\mathbf{x})$ 是不可观子空间基矩阵。

理想线性化应该满足：

$$\mathbf{J}(\mathbf{x})\mathbf{N}(\mathbf{x})=0$$

注意这里的零空间 $\mathbf{N}(\mathbf{x})$ 依赖当前线性化点 $\mathbf{x}$。

如果不同因子在不同点线性化：

$${\mathbf{J}}_1({\mathbf{x}}_1),{\mathbf{J}}_2({\mathbf{x}}_2)$$

那么它们各自满足的零空间可能是：

$$\mathbf{N}({\mathbf{x}}_1),\mathbf{N}({\mathbf{x}}_2)$$

这两个零空间不一定完全相同。

组合后的信息矩阵为：

$$\mathbf{H}={\mathbf{J}}_1^{\top }{\mathbf{\Omega }}_1{\mathbf{J}}_1+{\mathbf{J}}_2^{\top }{\mathbf{\Omega }}_2{\mathbf{J}}_2$$

如果不存在一个共同方向 $\mathbf{n}$ 同时满足：

$${\mathbf{J}}_1\mathbf{n}=0$$

$${\mathbf{J}}_2\mathbf{n}=0$$

那么原本应该不可观的方向就可能被约束住。

FEJ 的作用，就是让相关 Jacobian 尽量在同一个 first estimate 上定义，从而共享同一个零空间：

$${\mathbf{J}}_1(\bar{\mathbf{x}})\mathbf{N}(\bar{\mathbf{x}})\approx 0$$

$${\mathbf{J}}_2(\bar{\mathbf{x}})\mathbf{N}(\bar{\mathbf{x}})\approx 0$$

这样组合后仍然有：

$$\mathbf{HN}(\bar{\mathbf{x}})\approx 0$$

4.2 FEJ 改善 Consistency 的本质

FEJ 改善 consistency，不是因为它让线性化更精确。恰恰相反，固定 Jacobian 有时会牺牲非线性精度。

它改善 consistency 的原因是：

$$\text{FEJ 更好地保持了不可观方向的零空间结构。}$$

换句话说：

$$\text{系统本来不知道的东西，FEJ 不让线性化过程假装知道。}$$

这会减少：

$${\mathbf{n}}^{\top }\mathbf{Hn}>0$$

在不可观方向 $\mathbf{n}$ 上错误出现的概率。

因此 FEJ 的收益主要是：

• 避免不可观方向被错误约束；
• 避免协方差在不可观方向上过度收缩；
• 降低估计器过度自信；
• 提升长期一致性。

FEJ 的代价是：

• 如果 first estimate 很差，固定 Jacobian 会降低局部精度；
• 如果状态远离 first estimate，线性模型误差会增大；
• 它主要解决一致性，不保证每个数据集上误差最小。

4.3 FEJ 是缓解，不是完美消除影响

这里要避免一个误解：FEJ 不是让 Jacobian 更“实时”、更“精确”。它恰恰是不实时更新某些 Jacobian。

普通重线性化使用：

$$\mathbf{J}=\mathbf{J}({\mathbf{x}}^{current})$$

其中 ${\mathbf{x}}^{current}$ 表示当前估计状态。它的优点是更贴近当前非线性函数；缺点是历史 prior、视觉因子、IMU 因子可能在不同线性化点上拥有不同零空间。

FEJ 使用：

$$\mathbf{J}=\mathbf{J}(\bar{\mathbf{x}})$$

其中 $\bar{\mathbf{x}}$ 表示 first estimate。它的优点是零空间结构更稳定；缺点是当：

$${\mathbf{x}}^{current}\ne \bar{\mathbf{x}}$$

并且二者差距变大时，线性化误差也会变大。

所以 FEJ 的核心取舍是：

$$\text{牺牲一部分当前线性化精度}⟺\text{换取更好的零空间一致性}$$

更具体地说，FEJ 不是通过“人为增大协方差”来改善一致性，而是通过让不可观方向尽量继续满足：

$$\mathbf{J}(\bar{\mathbf{x}})\mathbf{N}(\bar{\mathbf{x}})\approx 0$$

从而避免总信息矩阵在不可观方向上错误出现：

$${\mathbf{N}}^{\top }\mathbf{HN}>0$$

如果 first estimate 很差，或者后续状态离 first estimate 太远，那么 FEJ 固定下来的 Jacobian 也会带来误差。也就是说：

$$\text{FEJ 能缓解 inconsistency}\ne \text{FEJ 能完全消除 inconsistency}$$

它只是把主要风险从：

$$\text{不可观方向被错误约束}$$

转移成：

$$\text{固定 Jacobian 带来的线性化误差}$$

在 VIO 中，这个取舍常常是值得的，因为不可观方向上的虚假信息会让系统长期过度自信，而 FEJ 的线性化误差可以通过较好的初始化、较短滑窗、频繁更新窗口来控制。

4.4 严格 FEJ 和 FEJ-like 的具体区别

前面一直在说 FEJ，但工程里经常会看到两种不同做法：

$$\text{strict FEJ}\text{和}\text{FEJ-like}$$

二者的区别不在于“有没有固定某个 Jacobian”，而在于：

$$\text{是否让所有影响可观性的相关 Jacobian 都使用 first estimate。}$$

严格 FEJ 要维护两份状态

对每一个进入估计器的状态块 ${\mathbf{x}}_i$，严格 FEJ 会维护两份值：

$${\mathbf{x}}_i^{cur}\text{和}{\bar{\mathbf{x}}}_i^{FEJ}$$

其中：

• ${\mathbf{x}}_i^{cur}$ 表示当前估计值，会随着优化不断更新；
• ${\bar{\mathbf{x}}}_i^{FEJ}$ 表示 first estimate，也就是该状态第一次进入系统时保存下来的估计值；
• 下标 $i$ 表示第 $i$ 个状态块，例如第 $i$ 帧位姿、速度、bias，或者第 $i$ 个特征点参数；
• 上标 $cur$ 表示 current estimate；
• 上标 $FEJ$ 表示用于计算 FEJ Jacobian 的固定参考值。

当一个新状态第一次进入系统时，保存：

$${\bar{\mathbf{x}}}_i^{FEJ}\leftarrow {\mathbf{x}}_i^{init}$$

其中 ${\mathbf{x}}_i^{init}$ 表示该状态的初始估计。

之后优化过程中，只更新当前值：

$${\mathbf{x}}_i^{cur}\leftarrow {\mathbf{x}}_i^{cur}⊞\delta {\mathbf{x}}_i$$

但 first estimate 不再改变：

$${\bar{\mathbf{x}}}_i^{FEJ}\leftarrow {\bar{\mathbf{x}}}_i^{FEJ}$$

其中：

• $\delta {\mathbf{x}}_i$ 表示优化求出来的局部状态增量；
• $⊞$ 表示流形上的加法，例如位置可以直接相加，旋转则需要通过指数映射更新。

对 VIO 状态来说，一帧 IMU 状态通常包含：

$${\mathbf{x}}_i=\left[{\mathbf{R}}_i,{\mathbf{p}}_i,{\mathbf{v}}_i,{\mathbf{b}}_{a_i},{\mathbf{b}}_{g_i}\right]$$

其中：

• ${\mathbf{R}}_i$ 表示第 $i$ 帧机体系到世界系的旋转；
• ${\mathbf{p}}_i$ 表示第 $i$ 帧机体系在世界系下的位置；
• ${\mathbf{v}}_i$ 表示第 $i$ 帧速度；
• ${\mathbf{b}}_{a_i}$ 表示第 $i$ 帧加速度计 bias；
• ${\mathbf{b}}_{g_i}$ 表示第 $i$ 帧陀螺仪 bias。

严格 FEJ 会同时保存：

$${\mathbf{x}}_i^{cur}=\left[{\mathbf{R}}_i^{cur},{\mathbf{p}}_i^{cur},{\mathbf{v}}_i^{cur},{\mathbf{b}}_{a_i}^{cur},{\mathbf{b}}_{g_i}^{cur}\right]$$

以及：

$${\bar{\mathbf{x}}}_i^{FEJ}=\left[{\bar{\mathbf{R}}}_i^{FEJ},{\bar{\mathbf{p}}}_i^{FEJ},{\bar{\mathbf{v}}}_i^{FEJ},{\bar{\mathbf{b}}}_{a_i}^{FEJ},{\bar{\mathbf{b}}}_{g_i}^{FEJ}\right]$$

如果特征点用逆深度 ${\lambda }_j$ 表示，也会保存：

$${\lambda }_j^{cur}\text{和}{\bar{\lambda }}_j^{FEJ}$$

其中 ${\lambda }_j$ 表示第 $j$ 个特征点的逆深度。

普通重线性化怎么做

设第 $k$ 个因子的残差为：

$${\mathbf{r}}_k({\mathbf{x}}_{{\mathcal{S}}_k})$$

其中：

• ${\mathbf{r}}_k$ 表示第 $k$ 个残差；
• ${\mathcal{S}}_k$ 表示这个因子连接的状态块集合；
• ${\mathbf{x}}_{{\mathcal{S}}_k}$ 表示该因子依赖的所有状态。

普通 Gauss-Newton 或 Levenberg-Marquardt 会在当前状态处线性化：

$${\mathbf{r}}_k\left({\mathbf{x}}_{{\mathcal{S}}_k}^{cur}⊞\delta {\mathbf{x}}_{{\mathcal{S}}_k}\right)\approx {\mathbf{r}}_k\left({\mathbf{x}}_{{\mathcal{S}}_k}^{cur}\right)+{\mathbf{J}}_k\left({\mathbf{x}}_{{\mathcal{S}}_k}^{cur}\right)\delta {\mathbf{x}}_{{\mathcal{S}}_k}$$

也就是残差值和 Jacobian 都围绕当前状态：

$${\mathbf{r}}_k^{cur}={\mathbf{r}}_k({\mathbf{x}}^{cur})$$

$${\mathbf{J}}_k^{cur}={\frac{\partial {\mathbf{r}}_k}{\partial \delta \mathbf{x}}|}_{\mathbf{x}={\mathbf{x}}^{cur}}$$

这种做法局部精度高，但不同因子会随着状态更新在不同位置反复重线性化，零空间结构可能不断变化。

严格 FEJ 怎么做

严格 FEJ 的关键做法是：

$$\text{残差值可以用当前状态计算，Jacobian 用 first estimate 计算。}$$

也就是：

$${\mathbf{r}}_k^{cur}={\mathbf{r}}_k\left({\mathbf{x}}_{{\mathcal{S}}_k}^{cur}\right)$$

但 Jacobian 使用：

$${\mathbf{J}}_k^{FEJ}={\frac{\partial {\mathbf{r}}_k}{\partial \delta {\mathbf{x}}_{{\mathcal{S}}_k}}|}_{{\mathbf{x}}_{{\mathcal{S}}_k}={\bar{\mathbf{x}}}_{{\mathcal{S}}_k}^{FEJ}}$$

于是严格 FEJ 的线性化模型是：

$${\mathbf{r}}_k\left({\mathbf{x}}^{cur}⊞\delta \mathbf{x}\right)\approx {\mathbf{r}}_k\left({\mathbf{x}}^{cur}\right)+{\mathbf{J}}_k^{FEJ}\delta \mathbf{x}$$

注意，这个式子不是严格意义上的 Taylor 展开，因为残差值和 Jacobian 不是在同一个点计算的。它是一个 consistency-preserving approximation，也就是为了保持一致性而采用的近似。

整体优化问题写成：

$$\underset{\delta \mathbf{x}}{\min }\sum _k{\Vert {\mathbf{r}}_k\left({\mathbf{x}}^{cur}\right)+{\mathbf{J}}_k^{FEJ}\delta \mathbf{x}\Vert }_{{\mathbf{\Omega }}_k}^2$$

其中 ${\mathbf{\Omega }}_k$ 表示第 $k$ 个因子的信息矩阵。

求解出 $\delta \mathbf{x}$ 后，只更新当前状态：

$${\mathbf{x}}^{cur}\leftarrow {\mathbf{x}}^{cur}⊞\delta \mathbf{x}$$

但仍然保持：

$${\bar{\mathbf{x}}}^{FEJ}\text{ 不变}$$

下一轮优化时，重复同样的规则：

$${\mathbf{r}}_k\text{ 用 }{\mathbf{x}}^{cur}\text{ 算}$$

$${\mathbf{J}}_k\text{ 用 }{\bar{\mathbf{x}}}^{FEJ}\text{ 算}$$

新进来的因子如何计算 Jacobian

严格 FEJ 中，“新因子”的 Jacobian 不是随便固定，也不是说新因子自己有一个 first estimate。更准确地说：

$$\text{新因子的 Jacobian 由它连接的变量的 first estimate 决定。}$$

设新加入的因子为：

$${\mathbf{r}}_k={\mathbf{r}}_k\left({\mathbf{x}}_{{\mathcal{S}}_k}\right)$$

其中：

• ${\mathbf{r}}_k$ 表示新加入的第 $k$ 个残差；
• ${\mathcal{S}}_k$ 表示这个因子连接的变量索引集合；
• ${\mathbf{x}}_{{\mathcal{S}}_k}$ 表示这个因子依赖的所有变量。

严格 FEJ 计算该因子的 Jacobian 时，先检查它连接的每个变量是否已经有 first estimate：

$${\bar{\mathbf{x}}}_s^{FEJ},s\in {\mathcal{S}}_k$$

其中 $s$ 表示该因子连接的某一个变量索引。

变量的 first estimate 来源分几种情况：

• 如果 ${\mathbf{x}}_s$ 是旧状态，它在更早进入滑窗时已经保存了 ${\bar{\mathbf{x}}}_s^{FEJ}$；
• 如果 ${\mathbf{x}}_s$ 是刚加入的新帧，就在新帧初始化并进入估计器时立刻保存 ${\bar{\mathbf{x}}}_s^{FEJ}$；
• 如果 ${\mathbf{x}}_s$ 是刚初始化的新特征点，就在特征点第一次被参数化时保存 ${\bar{\lambda }}_s^{FEJ}$ 或 ${\bar{\mathbf{P}}}_s^{FEJ}$；
• 如果某个变量还没有初始化，就不能稳定地构造这个变量对应的 FEJ Jacobian，通常要延迟加入该因子，或者等变量完成初始化。

因此，新因子的 FEJ Jacobian 是：

$${\mathbf{J}}_k^{FEJ}={\frac{\partial {\mathbf{r}}_k}{\partial \delta {\mathbf{x}}_{{\mathcal{S}}_k}}|}_{{\mathbf{x}}_s={\bar{\mathbf{x}}}_s^{FEJ},s\in {\mathcal{S}}_k}$$

而它的残差值仍然可以用当前状态计算：

$${\mathbf{r}}_k^{cur}={\mathbf{r}}_k\left({\mathbf{x}}_{{\mathcal{S}}_k}^{cur}\right)$$

最终进入线性系统的是：

$${\mathbf{r}}_k\approx {\mathbf{r}}_k^{cur}+{\mathbf{J}}_k^{FEJ}\delta {\mathbf{x}}_{{\mathcal{S}}_k}$$

这句话非常重要：

$$\text{新因子新不新不关键，关键是它连接的变量各自有没有 first estimate。}$$

新视觉因子的例子

假设一个新视觉重投影因子连接：

$${\mathbf{x}}_i,{\mathbf{x}}_j,{\lambda }_l$$

其中：

• ${\mathbf{x}}_i$ 表示特征点的锚定帧状态；
• ${\mathbf{x}}_j$ 表示当前观测帧状态；
• ${\lambda }_l$ 表示第 $l$ 个特征点的逆深度。

如果第 $i$ 帧是旧帧，那么它早已有：

$${\bar{\mathbf{x}}}_i^{FEJ}$$

如果第 $j$ 帧是刚进来的新帧，则在它进入滑窗时保存：

$${\bar{\mathbf{x}}}_j^{FEJ}\leftarrow {\mathbf{x}}_j^{init}$$

如果特征点 ${\lambda }_l$ 是刚初始化的，则保存：

$${\bar{\lambda }}_l^{FEJ}\leftarrow {\lambda }_l^{init}$$

于是这个新视觉因子的 Jacobian 在严格 FEJ 下为：

$${\mathbf{J}}_{ij,l}^{FEJ}={\frac{\partial {\mathbf{r}}_{ij,l}}{\partial \left[\delta {\mathbf{x}}_i,\delta {\mathbf{x}}_j,\delta {\lambda }_l\right]}|}_{\begin{array}{c}{\mathbf{x}}_i={\bar{\mathbf{x}}}_i^{FEJ},\\ {\mathbf{x}}_j={\bar{\mathbf{x}}}_j^{FEJ},\\ {\lambda }_l={\bar{\lambda }}_l^{FEJ}\end{array}}$$

但残差值可以用当前状态：

$${\mathbf{r}}_{ij,l}^{cur}={\mathbf{r}}_{ij,l}\left({\mathbf{x}}_i^{cur},{\mathbf{x}}_j^{cur},{\lambda }_l^{cur}\right)$$

所以这个新因子加入系统时，线性化形式是：

$${\mathbf{r}}_{ij,l}\approx {\mathbf{r}}_{ij,l}^{cur}+{\mathbf{J}}_{ij,l}^{FEJ}\left[\begin{array}{c}\delta {\mathbf{x}}_i\\ \delta {\mathbf{x}}_j\\ \delta {\lambda }_l\end{array}\right]$$

新 IMU 因子的例子

IMU 预积分因子通常连接相邻两帧：

$${\mathbf{x}}_i,{\mathbf{x}}_{i+1}$$

若第 $i+1$ 帧刚加入系统，则先保存：

$${\bar{\mathbf{x}}}_{i+1}^{FEJ}\leftarrow {\mathbf{x}}_{i+1}^{init}$$

严格 FEJ 下，新 IMU 因子的 Jacobian 为：

$${\mathbf{J}}_{imu,i}^{FEJ}={\frac{\partial {\mathbf{r}}_{imu,i}}{\partial \left[\delta {\mathbf{x}}_i,\delta {\mathbf{x}}_{i+1}\right]}|}_{\begin{array}{c}{\mathbf{x}}_i={\bar{\mathbf{x}}}_i^{FEJ},\\ {\mathbf{x}}_{i+1}={\bar{\mathbf{x}}}_{i+1}^{FEJ}\end{array}}$$

而 IMU 残差值可以用：

$${\mathbf{r}}_{imu,i}^{cur}={\mathbf{r}}_{imu,i}\left({\mathbf{x}}_i^{cur},{\mathbf{x}}_{i+1}^{cur}\right)$$

所以新 IMU 因子同样遵循：

$$\text{残差看当前状态，Jacobian 看 first estimate。}$$

用流程表示就是：

1. 新变量进入系统，保存 first estimate；
2. 新因子进入系统，找到它连接的变量集合 ${\mathcal{S}}_k$；
3. 用 ${\bar{\mathbf{x}}}_{{\mathcal{S}}_k}^{FEJ}$ 计算 Jacobian；
4. 用 ${\mathbf{x}}_{{\mathcal{S}}_k}^{cur}$ 计算残差；
5. 求解增量后只更新 current estimate，不更新 first estimate。

多个变量的因子怎么处理

假设一个视觉重投影因子连接：

$${\mathbf{x}}_i,{\mathbf{x}}_j,{\lambda }_l$$

其中：

• ${\mathbf{x}}_i$ 表示特征点第一次观测所在帧的状态；
• ${\mathbf{x}}_j$ 表示当前观测帧的状态；
• ${\lambda }_l$ 表示第 $l$ 个特征点的逆深度。

普通重线性化会用：

$${\mathbf{J}}_{k,i}^{cur}={\frac{\partial {\mathbf{r}}_k}{\partial \delta {\mathbf{x}}_i}|}_{\begin{array}{c}{\mathbf{x}}_i={\mathbf{x}}_i^{cur},\\ {\mathbf{x}}_j={\mathbf{x}}_j^{cur},\\ {\lambda }_l={\lambda }_l^{cur}\end{array}}$$

$${\mathbf{J}}_{k,j}^{cur}={\frac{\partial {\mathbf{r}}_k}{\partial \delta {\mathbf{x}}_j}|}_{\begin{array}{c}{\mathbf{x}}_i={\mathbf{x}}_i^{cur},\\ {\mathbf{x}}_j={\mathbf{x}}_j^{cur},\\ {\lambda }_l={\lambda }_l^{cur}\end{array}}$$

严格 FEJ 则使用：

$${\mathbf{J}}_{k,i}^{FEJ}={\frac{\partial {\mathbf{r}}_k}{\partial \delta {\mathbf{x}}_i}|}_{\begin{array}{c}{\mathbf{x}}_i={\bar{\mathbf{x}}}_i^{FEJ},\\ {\mathbf{x}}_j={\bar{\mathbf{x}}}_j^{FEJ},\\ {\lambda }_l={\bar{\lambda }}_l^{FEJ}\end{array}}$$

$${\mathbf{J}}_{k,j}^{FEJ}={\frac{\partial {\mathbf{r}}_k}{\partial \delta {\mathbf{x}}_j}|}_{\begin{array}{c}{\mathbf{x}}_i={\bar{\mathbf{x}}}_i^{FEJ},\\ {\mathbf{x}}_j={\bar{\mathbf{x}}}_j^{FEJ},\\ {\lambda }_l={\bar{\lambda }}_l^{FEJ}\end{array}}$$

也就是说，只要这个因子的 Jacobian 依赖某些状态，严格 FEJ 就用这些状态的 first estimate 来计算。

IMU 预积分因子也是同理。若 IMU 因子连接：

$${\mathbf{x}}_i,{\mathbf{x}}_{i+1}$$

严格 FEJ 会用：

$${\bar{\mathbf{x}}}_i^{FEJ},{\bar{\mathbf{x}}}_{i+1}^{FEJ}$$

来计算该 IMU 因子的相关 Jacobian，而不是用当前迭代中的：

$${\mathbf{x}}_i^{cur},{\mathbf{x}}_{i+1}^{cur}$$

边缘化时严格 FEJ 怎么做

边缘化会把历史因子压缩成 prior。严格 FEJ 下，形成边缘化 prior 时使用的 Jacobian 也应来自 first estimate：

$${\mathbf{J}}_m^{FEJ}={\mathbf{J}}_m\left({\bar{\mathbf{x}}}^{FEJ}\right)$$

然后构造 Hessian：

$${\mathbf{H}}^{FEJ}={\left({\mathbf{J}}^{FEJ}\right)}^{\top }\mathbf{\Omega }{\mathbf{J}}^{FEJ}$$

再通过 Schur 补消去要边缘化的变量，得到 prior：

$${\mathbf{H}}_{prior}^{FEJ}={\mathbf{H}}_{rr}^{FEJ}-{\mathbf{H}}_{rm}^{FEJ}{\left({\mathbf{H}}_{mm}^{FEJ}\right)}^{-1}{\mathbf{H}}_{mr}^{FEJ}$$

其中：

• 下标 $m$ 表示 marginalized variables，也就是要删除的变量；
• 下标 $r$ 表示 remaining variables，也就是保留下来的变量；
• ${\mathbf{H}}_{mm}^{FEJ}$、${\mathbf{H}}_{mr}^{FEJ}$、${\mathbf{H}}_{rm}^{FEJ}$、${\mathbf{H}}_{rr}^{FEJ}$ 是按变量分块后的 FEJ Hessian。

边缘化之后，prior 被固定下来。保留下来的变量继续维护：

$${\mathbf{x}}_r^{cur}\text{和}{\bar{\mathbf{x}}}_r^{FEJ}$$

后续使用 prior 时，仍然不要把 ${\bar{\mathbf{x}}}_r^{FEJ}$ 更新成当前值，否则就失去了 strict FEJ 的意义。

为什么严格 FEJ 更能保持零空间

如果每个相关因子都使用同一组 first estimate 计算 Jacobian，那么对同一个不可观子空间：

$$\mathbf{N}\left({\bar{\mathbf{x}}}^{FEJ}\right)$$

理想情况下每个因子都满足：

$${\mathbf{J}}_k^{FEJ}\mathbf{N}\left({\bar{\mathbf{x}}}^{FEJ}\right)\approx 0$$

总信息矩阵为：

$${\mathbf{H}}^{FEJ}=\sum _k{\left({\mathbf{J}}_k^{FEJ}\right)}^{\top }{\mathbf{\Omega }}_k{\mathbf{J}}_k^{FEJ}$$

于是：

$${\mathbf{H}}^{FEJ}\mathbf{N}\left({\bar{\mathbf{x}}}^{FEJ}\right)\approx 0$$

这表示不可观方向仍然尽量留在零空间里。

而如果只有 marginalization prior 使用旧线性化点，但普通视觉因子和 IMU 因子继续使用当前状态重线性化，就会变成：

$${\mathbf{J}}_{prior}={\mathbf{J}}_{prior}\left(\bar{\mathbf{x}}\right)$$

$${\mathbf{J}}_{vision}={\mathbf{J}}_{vision}\left({\mathbf{x}}^{cur}\right)$$

$${\mathbf{J}}_{imu}={\mathbf{J}}_{imu}\left({\mathbf{x}}^{cur}\right)$$

这些 Jacobian 对应的零空间可能不是同一个，组合起来就可能出现：

$${\mathbf{N}}^{\top }\mathbf{HN}>0$$

这就是 FEJ-like 和 strict FEJ 在一致性上的差别。

VINS-Mono 为什么更像 FEJ-like

VINS-Mono 的边缘化 prior 会固定在边缘化发生时的线性化点上。这个 prior 后续不会重新根据当前状态恢复成原始历史因子再重新 Schur 补，所以它具有 FEJ 的影子：

$${\mathbf{J}}_{prior}\text{ 被固定}$$

但是，VINS-Mono 的普通视觉重投影因子、IMU 预积分因子在滑窗优化过程中仍然会根据当前状态重新计算残差和 Jacobian：

$${\mathbf{J}}_{vision}={\mathbf{J}}_{vision}\left({\mathbf{x}}^{cur}\right)$$

$${\mathbf{J}}_{imu}={\mathbf{J}}_{imu}\left({\mathbf{x}}^{cur}\right)$$

所以它不是严格意义上的：

$$\forall k,{\mathbf{J}}_k={\mathbf{J}}_k\left({\bar{\mathbf{x}}}^{FEJ}\right)$$

更准确地说：

$$\text{VINS-Mono}\approx \text{marginalization-prior FEJ-like}$$

它的工程取舍是：

$$\text{固定 prior 保留一部分一致性}+\text{普通因子重线性化保持局部精度}$$

这个取舍很实用，但不等价于严格 FEJ。严格 FEJ 的 consistency 更强，代价是 Jacobian 更不贴近当前状态，可能牺牲更多局部精度，并且需要为所有状态和特征维护 first estimate。

严格 FEJ 对 first estimate 的依赖

严格 FEJ 还有一个很现实的风险：

$$\text{如果 first estimate 很差，严格 FEJ 的 Jacobian 会长期在一个较差的点上计算。}$$

设真实非线性残差为：

$$\mathbf{r}(\mathbf{x})$$

普通重线性化在当前状态 ${\mathbf{x}}^{cur}$ 处使用：

$$\mathbf{r}\left({\mathbf{x}}^{cur}⊞\delta \mathbf{x}\right)\approx \mathbf{r}\left({\mathbf{x}}^{cur}\right)+\mathbf{J}\left({\mathbf{x}}^{cur}\right)\delta \mathbf{x}$$

如果 ${\mathbf{x}}^{cur}$ 已经接近真实状态，那么这个线性模型通常比较精确。

严格 FEJ 使用：

$$\mathbf{r}\left({\mathbf{x}}^{cur}⊞\delta \mathbf{x}\right)\approx \mathbf{r}\left({\mathbf{x}}^{cur}\right)+\mathbf{J}\left({\bar{\mathbf{x}}}^{FEJ}\right)\delta \mathbf{x}$$

如果 first estimate 和当前状态差很多：

$$\Vert {\mathbf{x}}^{cur}\boxminus {\bar{\mathbf{x}}}^{FEJ}\Vert \text{ 很大}$$

那么：

$$\mathbf{J}\left({\bar{\mathbf{x}}}^{FEJ}\right)\not\approx \mathbf{J}\left({\mathbf{x}}^{cur}\right)$$

此时线性模型可能明显不准。

这会带来几个后果：

• 优化方向可能变差；
• 收敛速度可能变慢；
• 局部精度可能下降；
• 强非线性因子更容易受影响，例如视觉重投影、逆深度、外参、时间偏移；
• 如果 first estimate 偏差很大，严格 FEJ 可能保持了零空间一致性，却牺牲了较多 accuracy。

所以严格 FEJ 不是“更准”的方法。它更准确的定位是：

$$\text{FEJ 主要改善 consistency，不保证最小化每一轮的 nonlinear residual error。}$$

也可以说：

$$\text{FEJ 防的是估计器过度自信，不是防所有估计误差。}$$

这也是 VINS-Mono 更偏向 FEJ-like 的重要原因。它选择：

$$\text{marginalization prior 固定}+\text{普通视觉 / IMU 因子继续重线性化}$$

也就是用一部分 consistency，换取更好的局部优化精度和工程稳定性。

把三种做法放在一起看：

因此，严格 FEJ 的优缺点可以压缩成一句话：

$$\text{严格 FEJ 用 accuracy 的一部分代价，换 consistency 的提升。}$$

4.5 prior 只约束旧变量，为什么会影响最新变量

这里还有一个很容易困惑的点：

$$\text{边缘化 prior 的 Jacobian 往往只作用在一部分旧变量上。}$$

那么它为什么会影响最新帧？最新帧明明不在这个 prior 里面。

答案是：

$$\text{prior 直接约束旧变量；相对因子把旧变量和新变量连起来；所以新变量会被间接约束。}$$

一个最小例子

先看一维位置问题。假设窗口里有两个位置：

$$p_0,p_1$$

其中：

• $p_0$ 表示旧位置；
• $p_1$ 表示新位置。

假设唯一真实测量是二者之间的相对位移：

$$
z =
p_1-p_0

• n
  $$

其中：

• $z$ 表示测得的相对位移；
• $n$ 表示测量噪声。

对应残差可以写成：

$$r_{01}=(p_1-p_0)-z$$

对状态：

$$\mathbf{x}=\left[\begin{array}{c}{p}_0\\ p_1\end{array}\right]$$

线性化后，Jacobian 是：

$${\mathbf{J}}_{01}=\left[\begin{array}{cc}-1& 1\end{array}\right]$$

因为：

$$\frac{\partial r_{01}}{\partial p_0}=-1,\frac{\partial r_{01}}{\partial p_1}=1$$

这个系统没有绝对位置测量，所以全局平移不可观。全局平移方向是：

$$\mathbf{n}=\left[\begin{array}{c}1\\ 1\end{array}\right]$$

它的含义是：

$$p_0\leftarrow p_0+s,p_1\leftarrow p_1+s$$

其中 $s$ 表示任意平移量。

沿这个方向移动时，相对位移不变：

$$(p_1+s)-(p_0+s)=p_1-p_0$$

用 Jacobian 表示就是：

$${\mathbf{J}}_{01}\mathbf{n}=\left[\begin{array}{cc}-1& 1\end{array}\right]\left[\begin{array}{c}1\\ 1\end{array}\right]=0$$

所以相对因子不会约束全局平移方向。

如果 prior 错误锚定旧变量

现在假设边缘化之后，系统产生了一个作用在旧变量 $p_0$ 上的 prior：

$$r_p=p_0-{\bar{p}}_0$$

其中 ${\bar{p}}_0$ 表示 prior 形成时 $p_0$ 的线性化参考值。

这个 prior 的 Jacobian 是：

$${\mathbf{J}}_p=\left[\begin{array}{cc}1& 0\end{array}\right]$$

注意，它确实没有直接约束 $p_1$。

但是沿全局平移方向：

$${\mathbf{J}}_p\mathbf{n}=\left[\begin{array}{cc}1& 0\end{array}\right]\left[\begin{array}{c}1\\ 1\end{array}\right]=1\ne 0$$

这说明 prior 对全局平移方向产生了响应。

如果 prior 的权重是 ${\omega }_p$，那么 prior 对应的信息矩阵是：

$${\mathbf{H}}_p={\mathbf{J}}_p^{\top }{\omega }_p{\mathbf{J}}_p=\left[\begin{array}{cc}{\omega }_p& 0\\ 0& 0\end{array}\right]$$

沿全局平移方向的二次代价是：

$$\frac{1}{2}{s}^2{\mathbf{n}}^{\top }{\mathbf{H}}_p\mathbf{n}=\frac{1}{2}{s}^2{\omega }_p$$

这个值大于零，表示系统认为整体平移会增加代价。于是本来不可观的全局平移方向被错误约束了。

重点是：

$$\text{prior 没有直接约束 }{p}_1,\text{但它约束了整体平移方向中的 }{p}_0\text{ 分量。}$$

而真正的全局平移不是“只移动 $p_1$”，而是：

$$\left[\begin{array}{c}{p}_0\\ p_1\end{array}\right]\leftarrow \left[\begin{array}{c}{p}_0\\ p_1\end{array}\right]+s\left[\begin{array}{c}1\\ 1\end{array}\right]$$

只要 $p_0$ 这一部分被错误锚定，整个全局平移方向就不再自由。

为什么新变量也被间接锚住

从另一个角度看，如果 $p_0$ 被 prior 锚住，而 $p_1$ 想单独平移：

$$p_1\leftarrow p_1+s,p_0\text{ 不动}$$

相对残差会变成：

$$r_{01}^{new}=\left(p_1+s-p_0\right)-z=r_{01}+s$$

这会被相对因子惩罚。

因此：

$$\text{prior 锚住 }{p}_0+\text{相对因子约束 }{p}_1-p_0\Rightarrow p_1\text{ 也被间接锚住}$$

这就是“最新变量没有直接出现在 prior 中，却仍然被 prior 影响”的原因。

写成一般矩阵形式

把状态分成两部分：

$$\mathbf{x}=\left[\begin{array}{c}{\mathbf{x}}_a\\ {\mathbf{x}}_b\end{array}\right]$$

其中：

• ${\mathbf{x}}_a$ 表示被边缘化 prior 直接连接的旧变量；
• ${\mathbf{x}}_b$ 表示没有被 prior 直接连接的新变量。

假设 prior 只作用在 ${\mathbf{x}}_a$ 上：

$${\mathbf{r}}_p\approx {\mathbf{r}}_{p0}+{\mathbf{J}}_p\delta {\mathbf{x}}_a$$

嵌入完整状态后，它的信息矩阵具有块结构：

$${\mathbf{H}}_p=\left[\begin{array}{cc}{\mathbf{H}}_{aa}^p& 0\\ 0& 0\end{array}\right]$$

其中：

• ${\mathbf{H}}_{aa}^p={\mathbf{J}}_p^{\top }{\mathbf{\Omega }}_p{\mathbf{J}}_p$；
• ${\mathbf{\Omega }}_p$ 表示 prior 的信息矩阵；
• 右下角的 $0$ 表示 prior 不直接约束 ${\mathbf{x}}_b$。

如果旧变量和新变量之间还有相对因子：

$${\mathbf{r}}_{ab}=\mathbf{h}({\mathbf{x}}_a,{\mathbf{x}}_b)-{\mathbf{z}}_{ab}$$

线性化后：

$${\mathbf{r}}_{ab}\approx {\mathbf{r}}_{ab0}+{\mathbf{J}}_a\delta {\mathbf{x}}_a+{\mathbf{J}}_b\delta {\mathbf{x}}_b$$

对应的信息矩阵会产生交叉块：

$${\mathbf{H}}_{ab}={\mathbf{J}}_a^{\top }{\mathbf{\Omega }}_{ab}{\mathbf{J}}_b$$

所以总信息矩阵近似为：

$$\mathbf{H}=\left[\begin{array}{cc}{\mathbf{H}}_{aa}^p& 0\\ 0& 0\end{array}\right]+\left[\begin{array}{cc}{\mathbf{J}}_a^{\top }{\mathbf{\Omega }}_{ab}{\mathbf{J}}_a& {\mathbf{J}}_a^{\top }{\mathbf{\Omega }}_{ab}{\mathbf{J}}_b\\ {\mathbf{J}}_b^{\top }{\mathbf{\Omega }}_{ab}{\mathbf{J}}_a& {\mathbf{J}}_b^{\top }{\mathbf{\Omega }}_{ab}{\mathbf{J}}_b\end{array}\right]$$

设完整系统的某个不可观方向为：

$$\mathbf{N}=\left[\begin{array}{c}{\mathbf{N}}_a\\ {\mathbf{N}}_b\end{array}\right]$$

其中：

• ${\mathbf{N}}_a$ 表示不可观方向在旧变量 ${\mathbf{x}}_a$ 上的分量；
• ${\mathbf{N}}_b$ 表示不可观方向在新变量 ${\mathbf{x}}_b$ 上的分量。

如果相对因子正确保持这个不可观方向，那么应该有：

$${\mathbf{J}}_a{\mathbf{N}}_a+{\mathbf{J}}_b{\mathbf{N}}_b=0$$

也就是说，相对因子不会惩罚整体 gauge 运动。

但是如果 prior 错误约束了旧变量上的不可观分量：

$${\mathbf{H}}_{aa}^p{\mathbf{N}}_a\ne 0$$

那么完整方向就不再是总信息矩阵的零空间。令扰动为：

$$\delta \mathbf{x}=s\mathbf{N}$$

其中 $s$ 表示沿不可观方向移动的幅度。沿这个方向的二次代价为：

$$\frac{1}{2}\delta {\mathbf{x}}^{\top }\mathbf{H}\delta \mathbf{x}=\frac{1}{2}{s}^2{\mathbf{N}}^{\top }\mathbf{HN}=\frac{1}{2}{s}^2{\mathbf{N}}_a^{\top }{\mathbf{H}}_{aa}^p{\mathbf{N}}_a>0$$

上式右边只来自 prior 对旧变量的约束。相对因子本身可以仍然满足零响应，但因为 prior 已经把 ${\mathbf{N}}_a$ 锚住，完整的不可观方向 $\mathbf{N}$ 也被破坏了。

回到 VIO 滑窗

在 VINS 这类滑窗 VIO 中，最新帧通常通过以下因子和历史状态连接：

• IMU 预积分因子连接相邻帧状态；
• 视觉重投影因子连接观测帧、特征点和相机外参；
• 边缘化 prior 连接窗口中保留下来的旧状态；
• 多帧特征跟踪让不同时间的位姿共享几何约束。

所以边缘化 prior 不需要直接连接最新帧，也可以影响最新帧。影响路径通常是：

$$\text{marginalization prior}\to \text{旧保留状态}\to \text{IMU / 视觉相对因子}\to \text{最新状态}$$

如果最新状态完全不和旧状态相连，那么它当然不会被这个 prior 约束。但那样它也不属于同一个滑窗估计问题；在正常 VIO 后端里，最新帧必然通过 IMU 和视觉约束接入历史图。

因此，更准确的说法是：

$$\text{边缘化 prior 直接约束的是旧变量，但它约束的是整个连通图的 gauge 自由度。}$$

FEJ 要避免的正是这种情况：不要让 prior 在旧变量分量 ${\mathbf{N}}_a$ 上错误产生信息，否则最新变量会通过相对因子被一起间接锚住。