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表示系統輸出矩陣（投影矩陣，不是雅可比！），$?$表示了系統矩陣離散化后的描述形式！注意這是一個時變系統。
$$\mathcal{O}?[\begin{array}{c}{\mathbf{H}}_1\\ {\mathbf{H}}_2{\mathbf{\Phi }}_1\\ ?\\ {\mathbf{H}}_k{\mathbf{\Phi }}_{k?1}?{\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\\ ??c{\hat{\mathbf{v}}}_{t,k+1|k}\times {?}^c\mathbf{g}\\ 0_3\\ ??{}^c{\hat{\mathbf{p}}}_{t,k+1|k}\times {?}^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\\ ??{\hat{q}}_{t,k|k?1}\times {?}^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}& =?{}^G{\hat{\mathbf{v}}}_{l,k|k?1}\times {?}^G\mathbf{g}??{}^G{\hat{\mathbf{v}}}_{l,k+1|k}\times {?}^G\mathbf{g}\\ {\Phi }_{51}\mathbf{C}{\left({}^I{\hat{q}}_{G,k|k?1}\right)}^G\mathbf{g}& =\delta t?{}^G{\hat{\mathbf{v}}}_{l,k|k?1}\times {?}^G\mathbf{g}??{}^G{\hat{\mathbf{p}}}_{l,k+1|k}\times {?}^G\mathbf{g}\end{array}$$

然后論文里面引入了一個最小二乘類似的約束，其中 A=Φ31 或 Φ51

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

$${\mathbf{A}}^{?}=\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& ??{}^c{\hat{\mathbf{q}}}_{?,k|k?1}\times {?}^c\mathbf{g}\\ 0_3& 0_3\\ {\mathbf{I}}_3& ??{}^c{\hat{\mathbf{q}}}_{?,k|k?1}\times {?}^c\mathbf{g}\\ {\mathbf{I}}_3& ??{}^c{\hat{\mathbf{f}}}_{k|k?1}\times {?}^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(?G{\hat{\mathbf{f}}}_{k|k?1}\times ???{}^G{\hat{\mathbf{p}}}_{I,k|k?1}\times ?\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}}^{?}=\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列開始取，對應公式