Reference : https://docs.openvins.com/fej.html

self-use-note 自用笔记

background

在VINS中 关注 observability (能观性) 和 consistency (一致性) 可以作为研究系统退化的切入点

Naive EKF-based VINS estimators have been shown to be inconsistent due to spurious information gain along unobservable directions and have required the creation of "observability aware" filters which explicitly address the inaccurate information gains causing filter over-confidence (the estimated uncertainty is smaller than the true)

由于沿不可观测方向存在虚假的信息增益，基于 EKF 的非直接 VINS 估计器已被证明是不一致的，因此需要创建 "可观测性感知 "滤波器，以明确解决造成滤波器过度置信（估计的不确定性小于真实的不确定性）的不准确信息增益问题。

一致性是滤波优化的必要条件,错误的协方差会产生错误的增益。

First-estimates Jacobian (FEJ) 方法被提出，用于:

  • 保证VINS的observability
  • 提升consistency

EKF Linearized Error-State System

openvins用到的经典的pvq模型:

\[\begin{align*} \mathbf{x}_k &= \begin{bmatrix} {}_G^{I_{k}} \bar{q}{}^{\top} & {}^G\mathbf{p}_{I_k}^{\top} & {}^G\mathbf{v}_{I_k}^{\top} & {}^G\mathbf{p}_{f}^{\top} \end{bmatrix}^{\top} \end{align*}\]

经典的 error-state update:

\[\begin{align*} \tilde{\mathbf{x}}_{k|k-1} &= \mathbf{\Phi}_{(k,k-1)}~\tilde{\mathbf{x}}_{k-1|k-1} + \mathbf{G}_{k}\mathbf{w}_{k} \\ \tilde{\mathbf{z}}_{k} &= \mathbf{H}_{k}~\tilde{\mathbf{x}}_{k|k-1}+\mathbf{n}_{k} \end{align*}\]

状态转移矩阵 \({\Phi}_{(k,k-1)}\):

\[\begin{align*} \mathbf{\Phi}_{(k,k-1)}&= \begin{bmatrix} {}^{I_{k}}_{I_{k-1}}\mathbf R & \mathbf 0_{3\times3} & \mathbf 0_{3\times3} & \mathbf 0_{3\times3} \\[1em] -{}^{I_{k-1}}_{G}\mathbf{R}^\top \lfloor \boldsymbol\alpha(k,k-1) \times\rfloor & \mathbf I_{3\times3} & (t_{k}-t_{k-1})\mathbf I_{3\times3} & \mathbf 0_{3\times3} \\[1em] -{}^{I_{k-1}}_{G}\mathbf{R}^\top \lfloor \boldsymbol\beta(k,k-1) \times\rfloor & \mathbf 0_{3\times3} & \mathbf I_{3\times3} & \mathbf 0_{3\times3} \\[1em] \mathbf 0_{3\times3} & \mathbf 0_{3\times3} & \mathbf 0{3\times3} & \mathbf I_{3\times3} \end{bmatrix} \end{align*}\] \[\begin{align*} \boldsymbol\alpha(k,k-1) &= \int_{t_{k-1}}^{k} \int_{t_{k-1}}^{s} {}^{I_{k-1}}_{\tau}\mathbf R (\mathbf a(\tau)-\mathbf b_a - \mathbf w_a) d\tau ds \\ \boldsymbol\beta(k,k-1) &= \int_{t_{k-1}}^{t_k} {}^{I_{k-1}}_{\tau}\mathbf R (\mathbf a(\tau)-\mathbf b_a - \mathbf w_a) d\tau \end{align*}\]

测量雅各比(measurement Jacobian Matrix) \({H}_{k}\):

\[\begin{align*} \mathbf{H}_{k} &= \mathbf H_{proj,k}~\mathbf H_{state,k} \\ &= \begin{bmatrix} \frac{1}{Iz} & 0 & \frac{-{}^Ix}{({}^Iz)^2} \\ 0 & \frac{1}{Iz} & \frac{-{}^Iy}{({}^Iz)^2} \\ \end{bmatrix} \begin{bmatrix} \lfloor {}^{I_k}_{G}\mathbf{R}({}^{G}\mathbf{p}_f-{}^{G}\mathbf{p}_{I_k}) \times\rfloor & -{}^{I_k}_{G}\mathbf{R} & \mathbf 0_{3\times3} & {}^{I_k}_{G}\mathbf{R} \end{bmatrix} \\ &= \mathbf H_{proj,k}~ {}^{I_k}_{G}\mathbf{R} \begin{bmatrix} \lfloor ({}^{G}\mathbf{p}_f-{}^{G}\mathbf{p}_{I_k}) \times\rfloor {}^{I_k}_{G}\mathbf{R}^\top & -\mathbf I_{3\times3} & \mathbf 0_{3\times3} & \mathbf I_{3\times3} \end{bmatrix} \end{align*}\]

Linearized System Observability

能观性矩阵 \(\mathcal{O}\): 若 \(\mathcal{O}\) 列满秩，则完全能观。它的零空间则描述了无法用给定测量恢复的不可观状态子空间

\[\begin{align*} \mathcal{O}= \begin{bmatrix} \mathbf{H}_{0}\mathbf{\Phi}_{(0,0)} \\ \mathbf{H}_{1}\mathbf{\Phi}_{(1,0)} \\ \mathbf{H}_{2}\mathbf{\Phi}_{(2,0)} \\ \vdots \\ \mathbf{H}_{k}\mathbf{\Phi}_{(k,0)} \\ \end{bmatrix} \end{align*}\]

他的零空间 \(\mathcal{N}\) 为4dof，对应于YAW和global xyz translation:

\[\begin{align*} \mathcal{N} &= \begin{bmatrix} {}^{I_{0}}_{G}\mathbf{R}{}^G\mathbf{g} & \mathbf 0_{3\times3} \\ -\lfloor {}^{G}\mathbf{p}_{I_0} \times\rfloor{}^G\mathbf{g} & \mathbf{I}_{3\times3} \\ -\lfloor {}^{G}\mathbf{v}_{I_0} \times\rfloor{}^G\mathbf{g} & \mathbf{0}_{3\times3} \\ -\lfloor {}^{G}\mathbf{p}_{f} \times\rfloor{}^G\mathbf{g} & \mathbf{I}_{3\times3} \\ \end{bmatrix} \end{align*}\]

First Estimate Jacobians

It has been showed that standard EKF based-VINS, which always computes the state translation matrix and the measurement Jacobian using the current state estimates, has the global yaw orientation appear to be observable and has an incorrectly reduced 3dof nullspace dimention. This causes the filter mistakenly gaining extra information and becoming overconfident in the yaw.

To solve this issue, the First-Estimate Jacobains (FEJ)methodology can be applied. It evaluates the linearized system state transition matrix and Jacobians at the same estimate (i.e., the first estimates) over all time periods and thus ensures that the 4dof unobservable VINS subspace does not gain spurious information. The application of FEJ is simple yet effective, let us consider how to modify the propagation and update linearizations.

已经表明，基于标准 EKF 的 VINS 始终使用当前状态估计来计算状态转换矩阵和测量雅可比行列式，其全局偏航方向似乎是可观察的，并且具有错误减少的 3dof 零空间维度。 这会导致滤波器错误地获取额外信息并对偏航变得过于自信。

为了解决这个问题，可以应用FEJ方法。 它在所有时间段内以相同的估计（即第一次估计）评估线性化系统状态转移矩阵和雅可比行列式，从而确保 4dof 不可观测的 VINS 子空间不会获得虚假信息。 FEJ 的应用简单而有效，让我们考虑如何修改传播和更新线性化。

Propagation

Guoquan Huang老师提出更加正确的状态转移矩阵更新方式，表明: 不是

\[\begin{align*} \mathbf{\Phi}_{(k+1,k-1)}(\mathbf{x}_{k+1|k},\mathbf{x}_{k-1|k-1}) \neq \mathbf{\Phi}_{(k+1,k)}(\mathbf{x}_{k+1|k},\mathbf{x}_{k|k}) ~ \mathbf{\Phi}_{(k,k-1)}(\mathbf{x}_{k|k-1},\mathbf{x}_{k-1|k-1}) \end{align*}\]

而是

\[\begin{align*} \mathbf{\Phi}_{(k+1,k-1)}(\mathbf{x}_{k+1|k},\mathbf{x}_{k-1|k-1}) = \mathbf{\Phi}_{(k+1,k)}(\mathbf{x}_{k+1|k},\mathbf{x}_{k|k-1}) ~ \mathbf{\Phi}_{(k,k-1)}(\mathbf{x}_{k|k-1},\mathbf{x}_{k-1|k-1}) \end{align*}\]

区别就是在于不同的时间节点线性化评估

Update

指出了一般情况下，nullspace的第一列都是无效的： For example, the feature estimate in each row of are different, thus the nullsace does not hold.

使得能观性矩阵的维数退化成3

需要选择first state estimate

就得到了想要的线性化测量更新函数(linearized measurement update function):

\[\begin{align*} \tilde{\mathbf{z}}_{k+1} = \mathbf{z}_{k+1} - \mathbf{h}(\hat{\mathbf{x}}_k) \simeq \bar{\mathbf{H}}_{k} (\mathbf{x}_{k} -\hat{\mathbf{x}}_k ) + \mathbf{n}_k \end{align*}\]

\(\bar{\mathbf{H}}_{k}\) 是第一估计雅可比行列式，仅包含第一状态估计 \(\bar{\mathbf{x}_{k}}\)

使用FEJ可以保留系统的能观性

虽然这样做会引入线性化误差，但是Guoquan Huang老师等人认为inconsistencies对系统的副作用还是更大。FEJ2[Chuchu Chen, Yulin Yang, Patrick Geneva, and Guoquan Huang. Fej2: A consistent visual-inertial state estimator design. In 2022 International Conference on Robotics and Automation (ICRA), 2022.],OC[Guoquan P Huang, Anastasios I Mourikis, and Stergios I Roumeliotis. Observability-based rules for designing consistent ekf slam estimators. The International Journal of Robotics Research, 29(5):502–528, 2010.]等工作也在试图解决这个问题,在补偿或利用最佳估计的同时，解决确保正确可观测性的问题。

具体来说，位姿采用propagated值而非updated值，landmark使用第一次估计值。First Estimate就是这个直观含义，即采用estimation from the first time。

—- 如何理解SLAM中的First-Estimates Jacobian？ - jing胖的回答 - 知乎 https://www.zhihu.com/question/52869487/answer/132517493

comments 注释