MSCKF4讲：后端理论推导（下）

  写在前面，为了防止偏航角被错误的可观，作者对状态转移矩阵和观测矩阵进行了一定的修改，使其满足可观性要求。

6 可观测性分析与约束

  论文使用这种约束方法：OC-EKF

  论文 Observability-constrained vision-aided inertial navigation

  线性系统理论能观测能控概念

1 能观测：状态量（如位姿、偏差）是否可以由输出（如观测的预测值）反应

2 能控：状态量是否受到输入影响

6.1 为什么要做能观性分析

  真实VIO系统不能观的维度是4（平移和yaw角），而实际MSCKF不能观的维度变成了3，绕重力轴的旋转（yaw角）被错误地能观了，从而产生了不一致性Inconsistency，系统误认为yaw角具有更多的信息从而将yaw对应的协方差设得比较小，最终导致VIO估计精度的下降。能观性分析就是为了能让MSCKF系统不可观的维度与真实系统一致，从而提高VIO精度。

补充：为什么vio系统不能观的维度是4？

​ 对于平移来说，如果系统没有全局信息，就很难确定相机在世界坐标系中的绝对位置。在滚动（roll）和俯仰（pitch）发生变化时，重力会在测量中引入一些不确定性，但是对于偏航来说，由于重力一直指向下，说白了没有这个方向的投影分量，它并不提供关于相机绝对朝向的信息。因此，偏航角在这种情况下被认为是不可观测的

补充：为什么yaw被错误的能观测了？

​ 计算能观性矩阵会用到状态转移矩阵 和观测矩阵 ，这两个Jacobian的计算依赖状态值。如果使用状态真实值计算Jacobian，系统不能观维度为4，与真实系统一致；如果使用状态估计值，由于使用了不同时刻的估计量，能观性阵中会出现一个扰动项 ΔΓ，扰动项的存在使得yaw角对应那一维的零空间向量不再成立，从而导致yaw角被错误地能观了。

6.2 关于零空间解释

  Ax=0，x就是矩阵A的零空间。

  矩阵A的零度也就是不能观测的维度=n-秩。零度即零空间维度。

1 如果零空间维度为0，意味着矩阵的原像空间中只有一个样本可以被映射到矩阵像空间中的零点。（全0）
2 如果零空间维度为1：意味着矩阵的原像空间中有一条直线上的点经过矩阵算子后会被映射到像空间中的零点。
3 如果零空间维度为2：意味着矩阵的原像空间中有一个平面上的点经过矩阵算子后会被映射到像空间中的零点。
4 如果零空间维度为k：意味着矩阵的原像空间中有一个k维子空间，该k维子空间中的点经过矩阵算子后会被映射到像空间中的零点。

6.3 可观测性分析

  系统的可观测性属性影响估计器的一致性。在使用线性化估计器（如EKF）时，对评估系统和测量雅可比矩阵进行线性化的错误会改变估计器获取信息的方向。

  如果这些信息沿着不可观测的方向，将导致更大的误差、更小的不确定性和不一致性。就是说，如果由不可观变成可以观测，就会导致

  线性系统理论中提到，一个系统能否完全可观，看其能观测性判别矩阵$Q_O$是否满秩

  放到这里，就是如下形式，H表示系统输出矩阵（投影矩阵，不是雅可比！），$\varphi$表示了系统矩阵离散化后的描述形式！注意这是一个时变系统。
$$\mathcal{O}\triangleq [\begin{array}{c}{\mathbf{H}}_1\\ {\mathbf{H}}_2{\mathbf{\Phi }}_1\\ ⋮\\ {\mathbf{H}}_k{\mathbf{\Phi }}_{k-1}\cdots {\mathbf{\Phi }}_1\end{array}]$$

  这个矩阵对应的零空间如下

零空间：零空间的秩表示了不可观测的维度。很明显这个零空间的维度是4（两个全0行），刚好对应vio系统的不可观维度4，即平移与yaw。

$$\mathcal{O}{\mathbf{N}}_1=0,{\mathbf{N}}_1=[\begin{array}{cc}{0}_3& R_{bv,1}G\mathbf{g}\\ 0_3& 0_3\\ 0_3& -{(}^G{\mathbf{v}}_{b,1}{)}^{\wedge G}\mathbf{g}\\ 0_3& 0_3\\ {\mathbf{I}}_3& -(t_{wb,1}{)}^{\wedge G}\mathbf{g}\\ {\mathbf{I}}_3& -(p_w{)}^{\wedge G}\mathbf{g}\end{array}]=\left[\begin{array}{ccc}{\mathbf{N}}_{t,1}& |& {\mathbf{N}}_{r,1}\end{array}\right]$$

​

  但是，随着状态增多，能观测性矩阵维度大了，其中${\mathrm{H}}_k{\Phi }_{k,1}={\mathrm{H}}_k{\Phi }_{k,k-1}\dots {\Phi }_{2,1}{\Phi }_1$递推过程中，形式和理想的形式不一致了。就是能观测性矩阵形式变了，那么原来的零空间的形式肯定不再满足。0空间的最后一列不成立，导致可观测性多了一维（yaw）。

  论文中为了纠正这个问题，即仍然用原来零空间的形式，那么就只能强制修改能观测性矩阵的形式。

6.4 可观测性约束

  针对上述问题，修改状态转移矩阵 Φ和观测矩阵 H。本质上不需要关注0空间在每一时刻是否一样，只要保证每一个时刻的0空间维度都是4即可！也就是要满足下面两个式子！
$${\mathbf{N}}_k={\Phi }_{k-1}{\mathbf{N}}_{k-1},{\mathbf{H}}_k{\mathbf{N}}_k=0$$

① 状态转移矩阵Φ

$${\mathbf{N}}_k={\Phi }_{k-1}{\mathbf{N}}_{k-1}$$

  展开
$${\mathbf{N}}_{r,k+1}={\mathbf{\Phi }}_k{\mathbf{N}}_{r,k}\to \left[\begin{array}{c}\mathbf{C}{\left({\hat{q}}_{c,k+1|k}\right)}^c\mathbf{g}\\ 0_3\\ -\lfloor c{\hat{\mathbf{v}}}_{t,k+1|k}\times {\rfloor }^c\mathbf{g}\\ 0_3\\ -\lfloor {}^c{\hat{\mathbf{p}}}_{t,k+1|k}\times {\rfloor }^c\mathbf{g}\end{array}\right]=\left[\begin{array}{cccccc}{\Phi }_{11}& {\Phi }_{12}& 0_3& 0_3& 0_3& 0_3\\ 0_3& {\mathbf{I}}_3& 0_3& 0_3& 0_3& \\ {\Phi }_{31}& {\Phi }_{32}& {\mathbf{I}}_3& 0_{34}& 0_3& \\ 0_3& 0_3& 0_3& {\mathbf{I}}_3& 0_3& \\ 0_{51}& {\Phi }_{52}& \delta t{\mathbf{I}}_3& {\Phi }_{54}& {\mathbf{I}}_3& \end{array}\right]\left[\begin{array}{c}\mathbf{C}{\left({\hat{q}}_{c,k|k-1}\right)}^c\mathbf{g}\\ 0_3\\ -\lfloor {\hat{q}}_{t,k|k-1}\times {\rfloor }^c\mathbf{g}\\ 0_3\\ 0_3\end{array}\right].$$
我们把这个矩阵乘出来，能得到3个等式（主要是修改Φ11，31，51）：

第1行：可以直接拿出来

$$\mathbf{C}{\left({}^{\prime }{\hat{\bar{q}}}_{G,k+1|k}\right)}^c\mathbf{g}={\Phi }_{11}\mathbf{C}{\left({}^{\prime }{\hat{\bar{q}}}_{G,k|k-1}\right)}^c\mathbf{g}\to {\Phi }_{11}=\mathbf{C}\left({}^{\iota ,k+1|k}{\hat{\tilde{q}}}_{\iota ,k|k-1}\right).$$

第3、5行：论文中提到线性相关，无法直接求，或者说由多个解（等式中包含了反对
称矩阵导致实际右侧如果按照第⼀个式子那么算就会导致得到矩阵是线性相关的）

$$\begin{array}{rl}{\Phi }_{31}\mathbf{C}{\left({\hat{q}}_{c,k|k-1}\right)}^G\mathbf{g}& =\lfloor {}^G{\hat{\mathbf{v}}}_{l,k|k-1}\times {\rfloor }^G\mathbf{g}-\lfloor {}^G{\hat{\mathbf{v}}}_{l,k+1|k}\times {\rfloor }^G\mathbf{g}\\ {\Phi }_{51}\mathbf{C}{\left({}^I{\hat{q}}_{G,k|k-1}\right)}^G\mathbf{g}& =\delta t\lfloor {}^G{\hat{\mathbf{v}}}_{l,k|k-1}\times {\rfloor }^G\mathbf{g}-\lfloor {}^G{\hat{\mathbf{p}}}_{l,k+1|k}\times {\rfloor }^G\mathbf{g}\end{array}$$

然后论文里面引入了一个最小二乘类似的约束，其中 A=Φ31 或 Φ51

$$\underset{{\mathbf{A}}^{\ast }}{\min }{\Vert {\mathbf{A}}^{\ast }-\mathbf{A}\Vert }_{\mathcal{F}}^2,\text{s.t. }{\mathbf{A}}^{\ast }\mathbf{u}=\mathbf{w}$$

$${\mathbf{A}}^{\ast }=\mathbf{A}-(\mathbf{Au}-\mathbf{w}){\left({\mathbf{u}}^T\mathbf{u}\right)}^{-1}{\mathbf{u}}^T$$

// 5. Observability-constrained VINS 可观性约束// Modify the transition matrix// 5.1 修改phi_11// imu_state.orientation_null为上一个imu数据递推后保存的// 这块可能会有疑问，因为当上一个imu假如被观测更新了，// 导致当前的imu状态是由更新后的上一个imu状态递推而来，但是这里的值是没有更新的，这个有影响吗// 答案是没有的，因为我们更改了phi矩阵，保证了零空间// 并且这里必须这么处理，因为如果使用更新后的上一个imu状态构建上一时刻的零空间// 就破坏了上上一个跟上一个imu状态之间的0空间// Ni-1 = phi_[i-2] * Ni-2// Ni = phi_[i-1] * Ni-1^// 如果像上面这样约束，那么中间的0空间就“崩了”Matrix3d R_kk_1 = quaternionToRotation(imu_state.orientation_null);Phi.block<3, 3>(0, 0) =quaternionToRotation(imu_state.orientation) * R_kk_1.transpose();// 5.2 修改phi_31Vector3d u = R_kk_1 * IMUState::gravity;RowVector3d s = (u.transpose() * u).inverse() * u.transpose();Matrix3d A1 = Phi.block<3, 3>(6, 0);Vector3d w1 =skewSymmetric(imu_state.velocity_null - imu_state.velocity) * IMUState::gravity;Phi.block<3, 3>(6, 0) = A1 - (A1 * u - w1) * s;// 5.3 修改phi_51Matrix3d A2 = Phi.block<3, 3>(12, 0);Vector3d w2 =skewSymmetric(dtime * imu_state.velocity_null + imu_state.position_null -imu_state.position) *IMUState::gravity;Phi.block<3, 3>(12, 0) = A2 - (A2 * u - w2) * s;

② 对观测矩阵H–观测的雅可比矩阵

$${\mathbf{H}}_{cam}\left[\begin{array}{ccccc}{\mathbf{H}}_{{\theta }_G}& 0_{3\times 9}& {\mathbf{H}}_{{\mathbf{p}}_l}& |& {\mathbf{H}}_{\mathbf{f}}\end{array}\right]\left[\begin{array}{cc}{0}_3& \mathbf{C}{\left({\iota }_{G,k|k-1}^{\hat{q}}\mathbf{g}\right)}^c\mathbf{g}\\ 0_3& 0_3\\ 0_3& -\lfloor {}^c{\hat{\mathbf{q}}}_{\ell ,k|k-1}\times {\rfloor }^c\mathbf{g}\\ 0_3& 0_3\\ {\mathbf{I}}_3& -\lfloor {}^c{\hat{\mathbf{q}}}_{\ell ,k|k-1}\times {\rfloor }^c\mathbf{g}\\ {\mathbf{I}}_3& -\lfloor {}^c{\hat{\mathbf{f}}}_{k|k-1}\times {\rfloor }^c\mathbf{g}\end{array}\right]=\left[\begin{array}{cc}0& 0\end{array}\right].$$

  还是上面同样的计算方法，这部分细节后续在考虑吧，先把代码对应上即可。

  下面代码u就是对于了这个大矩阵（右），A就是对位姿的雅可比矩阵（左）。约束限制就是$Au=0=w$。所以下面公式里的w其实是0.
$${\mathbf{H}}_{cam}\left[\begin{array}{cc}{\mathbf{H}}_{{\mathbf{\theta }}_G}& {\mathbf{H}}_{{\mathbf{p}}_l}\end{array}\right]\left[\begin{array}{c}\mathbf{C}{\left({\hat{\mathbf{q}}}_{G,k|k-1}\right)}^G\mathbf{g}\\ {\left(\lfloor G{\hat{\mathbf{f}}}_{k|k-1}\times \rfloor -\lfloor {}^G{\hat{\mathbf{p}}}_{I,k|k-1}\times \rfloor \right)}^G\mathbf{g}\end{array}\right]=0.$$

${\mathbf{H}}_{cam}$对应代码dz_dpc，即残差对相机系点${}^{C}P$

${\mathbf{H}}_{{\theta }_G}$对应代码dpc_dxc，即相机系点${}^{C}P$对旋转的雅可比

${\mathbf{H}}_{{\mathbf{p}}_l}$对应代码dpc_dxc，即相机系点${}^{C}P$对平移的雅可比

${\mathbf{H}}_{\mathbf{f}}$对应代码dpc_dpg，即相机系点${}^{C}P$对路标点${}^{W}P$的雅可比

$${\mathbf{A}}^{\ast }=\mathbf{A}-(\mathbf{Au}-\mathbf{w}){\left({\mathbf{u}}^T\mathbf{u}\right)}^{-1}{\mathbf{u}}^T$$

解决来之后，根据下面公式依次对应新的雅可比。代码中是双目，所以行维都是4.

$${\mathbf{H}}_{cam}{\mathbf{H}}_{{\theta }_G}={\mathbf{A}}_{1:2,1:3}^{\prime },{\mathbf{H}}_{cam}{\mathbf{H}}_{{\mathbf{p}}_l}={\mathbf{A}}_{1:2,4:6}^{\prime },{\mathbf{H}}_{cam}{\mathbf{H}}_{\mathbf{f}}=-{\mathbf{A}}_{1:2,4:6}^{\prime }$$

    // Modifty the measurement Jacobian to ensure// observability constrain.// 6. OCMatrix<double, 4, 6> A = H_x;Matrix<double, 6, 1> u = Matrix<double, 6, 1>::Zero();u.block<3, 1>(0, 0) = quaternionToRotation(cam_state.orientation_null) * IMUState::gravity;u.block<3, 1>(3, 0) =skewSymmetric(p_w - cam_state.position_null) * IMUState::gravity;H_x = A - A * u * (u.transpose() * u).inverse() * u.transpose();H_f = -H_x.block<4, 3>(0, 3);//4*3大小，从0行3列开始取，对应公式