MSCKF（五）——Observability-Constrainted方法

文章目錄

• • Reference
  • Notation
  • IMU狀态傳遞方程
  • • MSCKF2.0的傳遞方程
    • MSCKF1.0的傳遞方程
    • OC-KF的系統狀态傳遞方程
    • • 第一組
      • 第三組
      • 第二組
      • • 小結
    • 相鄰時刻的狀态轉移矩陣
    • • t1 時刻的狀态轉移矩陣
      • t2 時刻的狀态轉移矩陣
      • t1 到 t2 時刻的狀态轉移矩陣
  • 視覺部分的觀測方程
  • 理想情況下能觀性的分析
  • • 在 t 時刻，系統零空間是如何的
    • 相鄰時刻間系統的零空間是如何傳播的
    • t 時刻傳播過來的零空間是否是觀測矩陣的零空間
    • 小結
  • OC-KF對于零空間的處理
  • • OC-KF想做什麼？
    • 修改狀态轉移矩陣 Φ ^ ( t , t − 1 ) \hat{\Phi}(t, t-1) Φ^(t,t−1)
    • 修改觀測矩陣 H ^ ( t ) \hat{\mathbf{H}}(t) H^(t)
    • 小結
  • OC-KF方法在MSCKF中的應用
  • 總結

Reference

1. Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight. 開源S-MSCKF的論文；
2. Observability-constrained Vision-aided Inertial Navigation. OC-EKF的論文；
3. https://zhuanlan.zhihu.com/p/304889273. 關于FEJ的總結；
4. High-Precision, Consistent EKF-based Visual-Inertial Odometry. MSCKF2.0 論文；
5. https://blog.csdn.net/wubaobao1993/article/details/109299097. 關于MSCKF1.0預測部分的總結；

Notation

依舊先來說清楚文中的Notation是如何表示的：

1. l l l 節點的理想值表示為 x l \mathrm{x}_{l} xl​；在 k 時刻的估計值表示為 x ^ l ( k ) \mathrm{\hat{x}}_{l}^{(k)} x^l(k)​；其誤差狀态表示為 x ~ l \mathrm{\tilde{x}}_{l} x~l​；
2. l l l 節點的估計值在不同時刻的關系為： x ^ l ( k + n ) = ( k + n ) x ~ l ( k ) + x ^ l ( k ) \mathbf{\hat{x}}^{(k+n)}_{l}={}^{(k+n)}\mathbf{\tilde{x}}^{(k)}_{l}+\mathbf{\hat{x}}^{(k)}_{l} x^l(k+n)​=(k+n)x~l(k)​+x^l(k)​，特别的，對于旋轉有： G l q ^ ( k + n ) ≈ ( I − ⌊ ( k + n ) θ l ( k ) ⌋ × ) G l q ^ ( k ) {}^{l}_{G}\mathbf{\hat{q}}^{(k+n)}\approx (\mathbf{I}-\lfloor {}^{(k+n)}\theta^{(k)}_{l}\rfloor_{\times}){}^{l}_{G}\mathbf{\hat{q}}^{(k)} Gl​q^​(k+n)≈(I−⌊(k+n)θl(k)​⌋×​)Gl​q^​(k)；

IMU狀态傳遞方程

INS系統的重中之重，還是先來推導IMU的狀态傳遞方程。為了和參考【3】的推導保持一緻，這裡的狀态變量也定為 X = [ G I q G p I G v I ] \mathbf{X}=\left[{}^{I}_{G}\mathrm{q}\quad {}^{G}p_{I}\quad {}^{G}v_{I} \right] X=[GI​qGpI​GvI​]，忽略零偏的部分。

MSCKF2.0的傳遞方程

在參考【3】中，作者從理論意義上推導了IMU的傳遞方程，推得的狀态轉移矩陣如下：

[ G I l + 1 θ ~ ( l ) G p ~ I l + 1 ( l ) G v ~ I l + 1 ( l ) ] = [ I l I l + 1 R ( l ) 0 0 − ( G I l R ( l ) ) T [ y ^ l ( l ) ] × I I Δ t − ( G I l R ( l ) ) T [ s ^ l ( l ) ] × 0 I ] [ G I l θ ~ ( l ) G p ~ I l ( l ) G v ~ I l ( l ) ] + [ I l I l + 1 θ ~ ( l ) ( G I l R ( l ) ) T [ y ~ l ( l ) ] ( G I l R ( l ) ) T [ s ~ l ( l ) ] ] (1) \begin{bmatrix} {}^{I_{l+1}}_{G}\tilde{\theta}^{(l)} \\ {}^{G}\tilde{p}_{I_{l+1}}^{(l)} \\ {}^{G}\tilde{v}_{I_{l+1}}^{(l)} \end{bmatrix} = \begin{bmatrix} {}^{I_{l+1}}_{I_{l}}R^{(l)} & \mathbf{0} & \mathbf{0} \\ -({}^{I_l}_{G}R^{(l)})^{T}\left[\hat{\mathrm{y}}^{(l)}_{l}\right]_{\times} & \mathbf{I} & \mathbf{I}\Delta{t} \\ -({}^{I_l}_{G}R^{(l)})^{T}\left[\hat{\mathrm{s}}^{(l)}_{l}\right]_{\times} & \mathbf{0} & \mathbf{I} \end{bmatrix} \begin{bmatrix} {}^{I_{l}}_{G}\tilde{\theta}^{(l)} \\ {}^{G}\tilde{p}_{I_{l}}^{(l)} \\ {}^{G}\tilde{v}_{I_{l}}^{(l)} \end{bmatrix} + \begin{bmatrix} {}^{I_{l+1}}_{I_{l}}\tilde{\theta}^{(l)} \\ ({}^{I_l}_{G}R^{(l)})^{T}\left[\tilde{\mathrm{y}}^{(l)}_{l}\right] \\ ({}^{I_l}_{G}R^{(l)})^{T}\left[\tilde{\mathrm{s}}^{(l)}_{l}\right] \end{bmatrix} \tag{1} ⎣⎢⎡​GIl+1​​θ~(l)Gp~​Il+1​(l)​Gv~Il+1​(l)​​⎦⎥⎤​=⎣⎢⎢⎡​Il​Il+1​​R(l)−(GIl​​R(l))T[y^​l(l)​]×​−(GIl​​R(l))T[s^l(l)​]×​​0I0​0IΔtI​⎦⎥⎥⎤​⎣⎢⎡​GIl​​θ~(l)Gp~​Il​(l)​Gv~Il​(l)​​⎦⎥⎤​+⎣⎢⎢⎡​Il​Il+1​​θ~(l)(GIl​​R(l))T[y~​l(l)​](GIl​​R(l))T[s~l(l)​]​⎦⎥⎥⎤​(1)

其中 − ( G I l R ( l ) ) T [ y ^ l ( l ) ] × -({}^{I_l}_{G}R^{(l)})^{T}\left[\hat{\mathrm{y}}^{(l)}_{l}\right]_{\times} −(GIl​​R(l))T[y^​l(l)​]×​和 − ( G I l R ( l ) ) T [ s ^ l ( l ) ] × -({}^{I_l}_{G}R^{(l)})^{T}\left[\hat{\mathrm{s}}^{(l)}_{l}\right]_{\times} −(GIl​​R(l))T[s^l(l)​]×​可以了解為是旋轉誤差作用于位移和速度的杆臂，具有一定的實體意義；

其實在實際的MSCKF2.0中，傳遞方程并不是公式（1）所示的形式，而是将旋轉部分從矩陣中去掉了，具體可見參考【4】

MSCKF1.0的傳遞方程

在MSCKF1.0的理論中，IMU的誤差狀态傳遞方程主要由運動方程求得，如下：

X ~ ˙ I M U = F X ~ I M U + G n I M U (2) \dot{\tilde{\mathbf{X}}}_{\mathrm{IMU}}=\mathbf{F} \tilde{\mathbf{X}}_{\mathrm{IMU}}+\mathbf{G} \mathbf{n}_{\mathrm{IMU}} \tag{2} X~˙IMU​=FX~IMU​+GnIMU​(2)

其中：

F = [ − ⌊ ω ^ × ⌋ 0 3 × 3 0 3 × 3 0 3 × 3 0 3 I 3 × 3 − ( G I l R ) T ⌊ a m ^ × ⌋ 0 3 × 3 0 3 × 3 ] \mathbf{F}=\left[\begin{array}{ccc} -\lfloor\hat{\boldsymbol{\omega}} \times\rfloor & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\ \mathbf{0}_{3 \times 3} & \mathbf{0}_{3} & \mathbf{I}_{3 \times 3} \\ -({}_{G}^{I_l}R)^{T}\lfloor\hat{\mathbf{a_m}} \times\rfloor & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\ \end{array}\right] F=⎣⎡​−⌊ω^×⌋03×3​−(GIl​​R)T⌊am​^​×⌋​03×3​03​03×3​​03×3​I3×3​03×3​​⎦⎤​

由于此處的G對于能觀性的分析沒有實質性的用處，這裡不作分析；

将微分方程轉為離散的形式：

X ~ ( t l + 1 ) = Φ ( t l + 1 , t l ) X ~ ( t l ) + ∫ t l t l + 1 Φ ( t l + 1 , τ ) G ( τ ) n ( τ ) d τ (3) \boldsymbol{\tilde{X}}\left(t_{l+1}\right)=\boldsymbol{\Phi}\left(t_{l+1}, t_{l}\right) \boldsymbol{\tilde{X}}\left(t_{l}\right)+\int_{t_{l}}^{t_{l+1}} \boldsymbol{\Phi}\left(t_{l+1}, \tau\right) \boldsymbol{G}(\tau) \boldsymbol{n}(\tau) \mathrm{d} \tau \tag{3} X~(tl+1​)=Φ(tl+1​,tl​)X~(tl​)+∫tl​tl+1​​Φ(tl+1​,τ)G(τ)n(τ)dτ(3)

其中：

{ Φ ˙ ( t l + 1 , t l ) = F ( t ) Φ ( t l + 1 , t l ) Φ ( t l + 1 , t l ) = exp ⁡ ( ∫ t l t l + 1 F ( t ) d t ) (4) \begin{cases} \dot{\boldsymbol{\Phi}}\left(t_{l+1}, t_{l}\right) = \boldsymbol{F}(t)\boldsymbol{\Phi}\left(t_{l+1}, t_{l}\right) \\ \boldsymbol{\Phi}\left(t_{l+1}, t_{l}\right)=\exp(\int_{t_{l}}^{t_{l+1}} \boldsymbol{F}(t) \mathrm{d} t) \end{cases} \tag{4} {Φ˙(tl+1​,tl​)=F(t)Φ(tl+1​,tl​)Φ(tl+1​,tl​)=exp(∫tl​tl+1​​F(t)dt)​(4)

OC-KF的系統狀态傳遞方程

在OC-KF中，IMU部分的傳遞方程使用的是MSCKF1.0中的微分方程的形式，這裡先來推導狀态轉移矩陣的閉式解。請讀者耐心看完理想情況下的推導過程，因為整個OC-KF的核心思路就是由該推導啟發的。

Notation：

以下均表示理想情況下的狀态傳遞

由公式（4）的第一行可以列出如下公式，這裡把時間跨度直接認為是 t，初始的時間為 t0：

[ Φ ˙ 11 ( t ) Φ ˙ 12 ( t ) Φ ˙ 13 ( t ) Φ ˙ 21 ( t ) Φ ˙ 22 ( t ) Φ ˙ 23 ( t ) Φ ˙ 31 ( t ) Φ ˙ 32 ( t ) Φ ˙ 33 ( t ) ] = [ − ⌊ ω ( t ) ⌋ × 0 3 × 3 0 3 × 3 0 3 × 3 0 3 I 3 × 3 − ( t G R ) T ⌊ a m ( t ) ⌋ × 0 3 × 3 0 3 × 3 ] [ Φ 11 ( t ) Φ 12 ( t ) Φ 13 ( t ) Φ 21 ( t ) Φ 22 ( t ) Φ 23 ( t ) Φ 31 ( t ) Φ 32 ( t ) Φ 33 ( t ) ] [ Φ 11 ( t 0 ) Φ 12 ( t 0 ) Φ 13 ( t 0 ) Φ 21 ( t 0 ) Φ 22 ( t 0 ) Φ 23 ( t 0 ) Φ 31 ( t 0 ) Φ 32 ( t 0 ) Φ 33 ( t 0 ) ] = [ I 0 0 0 I 0 0 0 I ] (5) \begin{aligned} \begin{bmatrix} \dot{\Phi}_{11}(t) & \dot{\Phi}_{12}(t) & \dot{\Phi}_{13}(t) \\ \dot{\Phi}_{21}(t) & \dot{\Phi}_{22}(t) & \dot{\Phi}_{23}(t) \\ \dot{\Phi}_{31}(t) & \dot{\Phi}_{32}(t) & \dot{\Phi}_{33}(t) \\ \end{bmatrix} &=\left[\begin{array}{ccc} -\lfloor{\boldsymbol{\omega}(t)}\rfloor_{\times} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\ \mathbf{0}_{3 \times 3} & \mathbf{0}_{3} & \mathbf{I}_{3 \times 3} \\ -({}^{G}_{t}R)^{T}\lfloor{\mathbf{a_m}(t)}\rfloor_{\times} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\ \end{array}\right] \begin{bmatrix} {\Phi}_{11}(t) & {\Phi}_{12}(t) & {\Phi}_{13}(t) \\ {\Phi}_{21}(t) & {\Phi}_{22}(t) & {\Phi}_{23}(t) \\ {\Phi}_{31}(t) & {\Phi}_{32}(t) & {\Phi}_{33}(t) \\ \end{bmatrix} \\ \begin{bmatrix} {\Phi}_{11}(t_0) & {\Phi}_{12}(t_0) & {\Phi}_{13}(t_0) \\ {\Phi}_{21}(t_0) & {\Phi}_{22}(t_0) & {\Phi}_{23}(t_0) \\ {\Phi}_{31}(t_0) & {\Phi}_{32}(t_0) & {\Phi}_{33}(t_0) \\ \end{bmatrix} &= \begin{bmatrix} \mathbf{I} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{I} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{I} \\ \end{bmatrix} \end{aligned} \tag{5} ⎣⎡​Φ˙11​(t)Φ˙21​(t)Φ˙31​(t)​Φ˙12​(t)Φ˙22​(t)Φ˙32​(t)​Φ˙13​(t)Φ˙23​(t)Φ˙33​(t)​⎦⎤​⎣⎡​Φ11​(t0​)Φ21​(t0​)Φ31​(t0​)​Φ12​(t0​)Φ22​(t0​)Φ32​(t0​)​Φ13​(t0​)Φ23​(t0​)Φ33​(t0​)​⎦⎤​​=⎣⎡​−⌊ω(t)⌋×​03×3​−(tG​R)T⌊am​(t)⌋×​​03×3​03​03×3​​03×3​I3×3​03×3​​⎦⎤​⎣⎡​Φ11​(t)Φ21​(t)Φ31​(t)​Φ12​(t)Φ22​(t)Φ32​(t)​Φ13​(t)Φ23​(t)Φ33​(t)​⎦⎤​=⎣⎡​I00​0I0​00I​⎦⎤​​(5)

公式（5）可以引出如下的幾組公式：

{ Φ ˙ 11 ( t ) = − ⌊ w ^ ⌋ × Φ 11 ( t ) Φ ˙ 12 ( t ) = − ⌊ w ^ ⌋ × Φ 12 ( t ) Φ ˙ 13 ( t ) = − ⌊ w ^ ⌋ × Φ 13 ( t ) { Φ ˙ 21 ( t ) = Φ 31 ( t ) Φ ˙ 22 ( t ) = Φ 32 ( t ) Φ ˙ 23 ( t ) = Φ 33 ( t ) { Φ ˙ 31 ( t ) = − ( I t G R ) T ⌊ a m ⌋ × Φ 11 ( t ) Φ ˙ 32 ( t ) = − ( I t G R ) T ⌊ a m ⌋ × Φ 12 ( t ) Φ ˙ 33 ( t ) = − ( I t G R ) T ⌊ a m ⌋ × Φ 13 ( t ) (6) \begin{cases} \dot{\Phi}_{11}(t)=-\lfloor \hat{\mathcal{w}} \rfloor_{\times}\Phi_{11}(t) \\ \dot{\Phi}_{12}(t)=-\lfloor \hat{\mathcal{w}} \rfloor_{\times}\Phi_{12}(t) \\ \dot{\Phi}_{13}(t)=-\lfloor \hat{\mathcal{w}} \rfloor_{\times}\Phi_{13}(t) \\ \end{cases} \quad \begin{cases} \dot{\Phi}_{21}(t)=\Phi_{31}(t) \\ \dot{\Phi}_{22}(t)=\Phi_{32}(t) \\ \dot{\Phi}_{23}(t)=\Phi_{33}(t) \\ \end{cases} \quad \begin{cases} \dot{\Phi}_{31}(t)=-({}^{G}_{I_t}R)^{T}\lfloor{\mathbf{a_m}}\rfloor_{\times} \Phi_{11}(t) \\ \dot{\Phi}_{32}(t)=-({}^{G}_{I_t}R)^{T}\lfloor{\mathbf{a_m}}\rfloor_{\times} \Phi_{12}(t) \\ \dot{\Phi}_{33}(t)=-({}^{G}_{I_t}R)^{T}\lfloor{\mathbf{a_m}}\rfloor_{\times} \Phi_{13}(t) \\ \end{cases} \tag{6} ⎩⎪⎨⎪⎧​Φ˙11​(t)=−⌊w^⌋×​Φ11​(t)Φ˙12​(t)=−⌊w^⌋×​Φ12​(t)Φ˙13​(t)=−⌊w^⌋×​Φ13​(t)​⎩⎪⎨⎪⎧​Φ˙21​(t)=Φ31​(t)Φ˙22​(t)=Φ32​(t)Φ˙23​(t)=Φ33​(t)​⎩⎪⎨⎪⎧​Φ˙31​(t)=−(It​G​R)T⌊am​⌋×​Φ11​(t)Φ˙32​(t)=−(It​G​R)T⌊am​⌋×​Φ12​(t)Φ˙33​(t)=−(It​G​R)T⌊am​⌋×​Φ13​(t)​(6)

下面就是一組一組的展開一下：

第一組

容易看出，第一組的閉式解其實都是exp函數，是以：

{ Φ 11 ( t ) = e x p ( − ⌊ w ⌋ × ( t − t 0 ) ) Φ 11 ( t 0 ) Φ 12 ( t ) = e x p ( − ⌊ w ⌋ × ( t − t 0 ) ) Φ 12 ( t 0 ) Φ 13 ( t ) = e x p ( − ⌊ w ⌋ × ( t − t 0 ) ) Φ 13 ( t 0 ) \begin{cases} \Phi_{11}(t)=exp(-\lfloor {\mathcal{w}} \rfloor_{\times}(t-t_0))\Phi_{11}(t_0) \\ \Phi_{12}(t)=exp(-\lfloor {\mathcal{w}} \rfloor_{\times}(t-t_0))\Phi_{12}(t_0) \\ \Phi_{13}(t)=exp(-\lfloor {\mathcal{w}} \rfloor_{\times}(t-t_0))\Phi_{13}(t_0) \\ \end{cases} ⎩⎪⎨⎪⎧​Φ11​(t)=exp(−⌊w⌋×​(t−t0​))Φ11​(t0​)Φ12​(t)=exp(−⌊w⌋×​(t−t0​))Φ12​(t0​)Φ13​(t)=exp(−⌊w⌋×​(t−t0​))Φ13​(t0​)​

由于初始的狀态轉移矩陣為機關矩陣，是以 Φ 12 ( t 0 ) \Phi_{12}(t_0) Φ12​(t0​)和 Φ 13 ( t 0 ) \Phi_{13}(t_0) Φ13​(t0​)均為0， Φ 11 ( t 0 ) = I \Phi_{11}(t_0)=\mathbf{I} Φ11​(t0​)=I，于是：

{ Φ 11 ( t , t 0 ) = e x p ( ∫ t 0 t ( − ⌊ ω ( t ) ⌋ × ) d t ) = t 0 t R Φ 12 ( t , t 0 ) = 0 Φ 13 ( t , t 0 ) = 0 (7) \begin{cases} \Phi_{11}(t, t_{0})=exp(\int_{t_0}^{t}(-\lfloor {\omega}(t)\rfloor_{\times})dt)={}_{t_0}^{t}R \\ \Phi_{12}(t, t_{0})=\mathbf{0} \\ \Phi_{13}(t, t_{0})=\mathbf{0} \\ \end{cases} \tag{7} ⎩⎪⎨⎪⎧​Φ11​(t,t0​)=exp(∫t0​t​(−⌊ω(t)⌋×​)dt)=t0​t​RΦ12​(t,t0​)=0Φ13​(t,t0​)=0​(7)

第三組

由于第二組中的 Φ ˙ 21 \dot{\Phi}_{21} Φ˙21​與 Φ 31 \Phi_{31} Φ31​相關，是以這裡先推導第三組的情況：

{ Φ 31 ( t ) = ∫ t 0 t − ( t G R ) T ⌊ a m ( t ) ⌋ × t 0 t R d t Φ 32 ( t ) = I × Φ 32 ( t 0 ) = 0 Φ 33 ( t ) = I × Φ 33 ( t 0 ) = I (8A) \begin{cases} \Phi_{31}(t)=\int_{t_0}^{t} -({}^{G}_{t}R)^{T}\lfloor {\mathbf{a_m}}(t)\rfloor_{\times} {}_{t_0}^{t}R dt \\ \Phi_{32}(t)= \mathbf{I} \times \Phi_{32}(t_0) = \mathbf{0} \\ \Phi_{33}(t)= \mathbf{I}\times \Phi_{33}(t_0) = \mathbf{I} \\ \end{cases} \tag{8A} ⎩⎪⎨⎪⎧​Φ31​(t)=∫t0​t​−(tG​R)T⌊am​(t)⌋×​t0​t​RdtΦ32​(t)=I×Φ32​(t0​)=0Φ33​(t)=I×Φ33​(t0​)=I​(8A)

重點分析首行元素：

Φ 31 ( t ) = ∫ t 0 t − ( t G R ) T ⌊ a m ( t ) ⌋ × t 0 t R d t = ∫ t 0 t − ( t G R ) T ⌊ G t R ( G a ( t ) + G g ) ⌋ × t 0 t R d t = − ∫ t 0 t ⌊ G a ( t ) + G g ⌋ × t G R t 0 t R d t = − ∫ t 0 t ⌊ G a ( t ) + G g ⌋ × d t ( G t 0 R ) T = − ⌊ G v t − G v t 0 + G g ( t − t 0 ) ⌋ × ( G t 0 R ) T (8B) \begin{aligned} \Phi_{31}(t)&=\int_{t_0}^{t} -({}^{G}_{t}R)^{T}\lfloor {\mathbf{a_m}}(t)\rfloor_{\times} {}_{t_0}^{t}R dt \\ &=\int_{t_0}^{t} -({}^{G}_{t}R)^{T}\lfloor {}_{G}^{t}R ({}^{G} {\mathbf{a}}(t)+{}^{G}\mathbf{g})\rfloor_{\times} {}_{t_0}^{t}R dt \\ &=-\int_{t_0}^{t}\lfloor {}^{G}{\mathbf{a}}(t)+{}^{G}\mathbf{g}\rfloor_{\times} {}_{t}^{G}R {}_{t_0}^{t}R dt \\ &=-\int_{t_0}^{t}\lfloor {}^{G}{\mathbf{a}}(t)+{}^{G}\mathbf{g}\rfloor_{\times} dt ({}^{t_0}_{G}R)^{T} \\ &=-\lfloor {}^{G}v_{t}-{}^{G}v_{t_0}+{}^{G}\mathbf{g}(t-t_0) \rfloor_{\times}({}^{t_0}_{G}R)^{T} \end{aligned} \tag{8B} Φ31​(t)​=∫t0​t​−(tG​R)T⌊am​(t)⌋×​t0​t​Rdt=∫t0​t​−(tG​R)T⌊Gt​R(Ga(t)+Gg)⌋×​t0​t​Rdt=−∫t0​t​⌊Ga(t)+Gg⌋×​tG​Rt0​t​Rdt=−∫t0​t​⌊Ga(t)+Gg⌋×​dt(Gt0​​R)T=−⌊Gvt​−Gvt0​​+Gg(t−t0​)⌋×​(Gt0​​R)T​(8B)

其中：

1. a m \mathbf{a}_{m} am​表示在機體坐标系中的測量值，夾雜重力；
2. G a {}^{G}\mathbf{a} Ga表示在世界坐标系下的加速度，不夾雜重力；
3. 最後一行化簡引入了 G v t = G v t 0 + ∫ G a d t {}^{G}v_{t}={}^{G}v_{t_0}+\int{}^{G}\mathbf{a}dt Gvt​=Gvt0​​+∫Gadt，其中的加速度不夾雜重力；

是以公式（8A）可以重新寫作：

{ Φ 31 ( t ) = − ⌊ G v t − G v t 0 + G g ( t − t 0 ) ⌋ × ( G t 0 R ) T Φ 32 ( t ) = 0 Φ 33 ( t ) = I (8) \begin{cases} \Phi_{31}(t)= -\lfloor {}^{G}v_{t}-{}^{G}v_{t_0}+{}^{G}\mathbf{g}(t-t_0) \rfloor_{\times}({}^{t_0}_{G}R)^{T}\\ \Phi_{32}(t)= \mathbf{0} \\ \Phi_{33}(t)= \mathbf{I} \\ \end{cases} \tag{8} ⎩⎪⎨⎪⎧​Φ31​(t)=−⌊Gvt​−Gvt0​​+Gg(t−t0​)⌋×​(Gt0​​R)TΦ32​(t)=0Φ33​(t)=I​(8)

第二組

将第三組的結果帶入到第二組中

{ Φ 21 ( t ) = − ∫ t 0 t ∫ t 0 t ⌊ G a ( t ) + G g ⌋ × d t d τ ( G t 0 R ) T Φ 22 ( t ) = I × Φ 22 ( t 0 ) = I Φ 23 ( t ) = Δ t × Φ 22 ( t 0 ) = I Δ t (9A) \begin{cases} \Phi_{21}(t)= -\int_{t_0}^{t} \int_{t_0}^{t}\lfloor {}^{G}{\mathbf{a}}(t)+{}^{G}\mathbf{g}\rfloor_{\times} dt d\tau({}^{t_0}_{G}R)^{T}\\ \Phi_{22}(t)= \mathbf{I} \times \Phi_{22}(t_0) = \mathbf{I} \\ \Phi_{23}(t)= \Delta{t}\times \Phi_{22}(t_0) = \mathbf{I}\Delta{t} \\ \end{cases} \tag{9A} ⎩⎪⎨⎪⎧​Φ21​(t)=−∫t0​t​∫t0​t​⌊Ga(t)+Gg⌋×​dtdτ(Gt0​​R)TΦ22​(t)=I×Φ22​(t0​)=IΦ23​(t)=Δt×Φ22​(t0​)=IΔt​(9A)

重點分析首行元素：

Φ 21 ( t ) = − ∫ t 0 t ∫ t 0 t ⌊ G a ^ ( t ) + G g ⌋ × d t d τ ( G t 0 R ) T = − ⌊ G p t − G p t 0 − G v t 0 ( t − t 0 ) + 1 2 G g ( t − t 0 ) 2 ⌋ × ( G t 0 R ) T (9B) \begin{aligned} \Phi_{21}(t) &= -\int_{t_0}^{t} \int_{t_0}^{t}\lfloor {}^{G}\hat{\mathbf{a}}(t)+{}^{G}\mathbf{g}\rfloor_{\times} dt d\tau({}^{t_0}_{G}R)^{T} \\ &=-\lfloor {}^{G}p_{t}-{}^{G}p_{t_0}-{}^{G}v_{t_0}(t-t_0)+\frac{1}{2}{}^{G}\mathbf{g}(t-t_0)^2 \rfloor_{\times}({}^{t_0}_{G}R)^{T} \end{aligned} \tag{9B} Φ21​(t)​=−∫t0​t​∫t0​t​⌊Ga^(t)+Gg⌋×​dtdτ(Gt0​​R)T=−⌊Gpt​−Gpt0​​−Gvt0​​(t−t0​)+21​Gg(t−t0​)2⌋×​(Gt0​​R)T​(9B)

是以公式（9A）可以重新寫作：

{ Φ 21 ( t ) = − ⌊ G p t − G p t 0 − G v t 0 ( t − t 0 ) + 1 2 G g ( t − t 0 ) 2 ⌋ × ( G t 0 R ) T Φ 22 ( t ) = I Φ 23 ( t ) = I Δ t (9) \begin{cases} \Phi_{21}(t)= -\lfloor {}^{G}p_{t}-{}^{G}p_{t_0}-{}^{G}v_{t_0}(t-t_0)+\frac{1}{2}{}^{G}\mathbf{g}(t-t_0)^2 \rfloor_{\times}({}^{t_0}_{G}R)^{T} \\ \Phi_{22}(t)= \mathbf{I} \\ \Phi_{23}(t)= \mathbf{I}\Delta{t} \\ \end{cases} \tag{9} ⎩⎪⎨⎪⎧​Φ21​(t)=−⌊Gpt​−Gpt0​​−Gvt0​​(t−t0​)+21​Gg(t−t0​)2⌋×​(Gt0​​R)TΦ22​(t)=IΦ23​(t)=IΔt​(9)

小結

綜合公式（7）（8）（9）可得：

Φ ( t , t 0 ) = [ t 0 t R 0 0 − ⌊ y ( t ) ⌋ × ( G t 0 R ) T I I Δ t − ⌊ s ( t ) ⌋ × ( G t 0 R ) T 0 I ] (10) \boldsymbol{\Phi}\left(t, t_{0}\right)= \begin{bmatrix} {}_{t_0}^{t}R & 0 & 0 \\ -\lfloor \mathbf{{y}}^{(t)} \rfloor_{\times}({}^{t_0}_{G}R)^{T} & \mathbf{I} & \mathbf{I}\Delta{t} \\ -\lfloor \mathbf{{s}}^{(t)} \rfloor_{\times}({}^{t_0}_{G}R)^{T} & \mathbf{0} & \mathbf{I} \end{bmatrix} \tag{10} Φ(t,t0​)=⎣⎡​t0​t​R−⌊y(t)⌋×​(Gt0​​R)T−⌊s(t)⌋×​(Gt0​​R)T​0I0​0IΔtI​⎦⎤​(10)

需要注意的是這裡的轉移矩陣是從 t0 時刻開始進行遞推的。

相鄰時刻的狀态轉移矩陣

如上一章節可知，t1 時刻和 t2 時刻的狀态轉移矩陣分别如下：

t1 時刻的狀态轉移矩陣

Φ ( t 1 , t 0 ) = [ t 0 t 1 R 0 0 − ⌊ y ( t 1 ) ⌋ × ( G t 0 R ) T I I Δ t 1 − ⌊ s ( t 1 ) ⌋ × ( G t 0 R ) T 0 I ] (11) \boldsymbol{\Phi}\left(t_1, t_{0}\right)= \begin{bmatrix} {}_{t_0}^{t_1}R & 0 & 0 \\ -\lfloor \mathbf{{y}}^{(t_1)} \rfloor_{\times}({}^{t_0}_{G}R)^{T} & \mathbf{I} & \mathbf{I}\Delta{t}_1 \\ -\lfloor \mathbf{{s}}^{(t_1)} \rfloor_{\times}({}^{t_0}_{G}R)^{T} & \mathbf{0} & \mathbf{I} \end{bmatrix} \tag{11} Φ(t1​,t0​)=⎣⎡​t0​t1​​R−⌊y(t1​)⌋×​(Gt0​​R)T−⌊s(t1​)⌋×​(Gt0​​R)T​0I0​0IΔt1​I​⎦⎤​(11)

t2 時刻的狀态轉移矩陣

Φ ( t 2 , t 0 ) = [ t 0 t 2 R 0 0 − ⌊ y ( t 2 ) ⌋ × ( G t 0 R ) T I I Δ t 2 − ⌊ s ( t 2 ) ⌋ × ( G t 0 R ) T 0 I ] (12) \boldsymbol{\Phi}\left(t_2, t_{0}\right)=\begin{bmatrix}{}_{t_0}^{t_2}R & 0 & 0 \\-\lfloor \mathbf{{y}}^{(t_2)} \rfloor_{\times}({}^{t_0}_{G}R)^{T} & \mathbf{I} & \mathbf{I}\Delta{t}_2 \\-\lfloor \mathbf{{s}}^{(t_2)} \rfloor_{\times}({}^{t_0}_{G}R)^{T} & \mathbf{0} & \mathbf{I}\end{bmatrix} \tag{12} Φ(t2​,t0​)=⎣⎡​t0​t2​​R−⌊y(t2​)⌋×​(Gt0​​R)T−⌊s(t2​)⌋×​(Gt0​​R)T​0I0​0IΔt2​I​⎦⎤​(12)

t1 到 t2 時刻的狀态轉移矩陣

結合公式（11）（12）易得：

Φ ( t 2 , t 1 ) = Φ ( t 2 , t 0 ) ( Φ ( t 1 , t 0 ) ) − 1 = [ t 0 t 2 R 0 0 − ⌊ y ( t 2 ) ⌋ × ( G t 0 R ) T I I Δ t 2 − ⌊ s ( t 2 ) ⌋ × ( G t 0 R ) T 0 I ] [ t 0 t 1 R T 0 0 ( ⌊ y ( t 1 ) − s ( t 1 ) Δ t 1 ⌋ × ) ( G t 1 R ) T I − I Δ t 1 ⌊ s ( t 1 ) ⌋ × ( G t 1 R ) T 0 I ] = [ t 1 t 2 R 0 0 − ⌊ y ( t 2 ) − y ( t 1 ) + s ^ ( t 1 ) Δ t 1 − s ^ ( t 1 ) Δ t 2 ⌋ × ( G t 1 R ) T I I ( Δ t 2 − Δ t 1 ） − ⌊ s ( t 2 ) − s ( t 1 ) ⌋ × ( G t 1 R ) T 0 I ] = [ t 1 t 2 R 0 0 − ⌊ G p t 2 − G p t 1 − G v t 1 Δ t 1 2 + 1 2 g ( Δ t 1 2 ) 2 ⌋ × ( G t 1 R ) T I I ( Δ t 1 2 ) − ⌊ G v t 2 − G v t 1 + g Δ t 1 2 ⌋ × ( G t 1 R ) T 0 I ] = [ t 1 t 2 R 0 0 − ⌊ G p t 2 − G p t 1 − G v t 1 Δ t 1 2 + 1 2 g ( Δ t 1 2 ) 2 ⌋ × ( G t 1 R ) T I I ( Δ t 1 2 ) − ⌊ G v t 2 − G v t 1 + g Δ t 1 2 ⌋ × ( G t 1 R ) T 0 I ] (13) \begin{aligned}\Phi(t_2, t_1)&=\Phi(t_2, t_0) (\Phi(t_1, t_0))^{-1} \\&=\begin{bmatrix}{}_{t_0}^{t_2}R & 0 & 0 \\-\lfloor \mathbf{{y}}^{(t_2)} \rfloor_{\times}({}^{t_0}_{G}R)^{T} & \mathbf{I} & \mathbf{I}\Delta{t}_2 \\-\lfloor \mathbf{{s}}^{(t_2)} \rfloor_{\times}({}^{t_0}_{G}R)^{T} & \mathbf{0} & \mathbf{I}\end{bmatrix}\begin{bmatrix}{}_{t_0}^{t_1}R^{T} & 0 & 0 \\(\lfloor \mathbf{{y}}^{(t_1)} - \mathbf{{s}}^{(t_1)}\Delta{t}_1 \rfloor_{\times})({}^{t_1}_{G}R)^{T} & \mathbf{I} & -\mathbf{I}\Delta{t}_1 \\\lfloor \mathbf{{s}}^{(t_1)} \rfloor_{\times}({}^{t_1}_{G}R)^{T} & \mathbf{0} & \mathbf{I}\end{bmatrix} \\&=\begin{bmatrix}{}_{t_1}^{t_2}R & 0 & 0 \\-\lfloor \mathbf{{y}}^{(t_2)}-\mathbf{{y}}^{(t_1)}+\mathbf{\hat{s}}^{(t_1)}\Delta{t}_1- \mathbf{\hat{s}}^{(t_1)}\Delta{t}_2 \rfloor_{\times}({}^{t_1}_{G}R)^{T} & \mathbf{I} & \mathbf{I}(\Delta{t}_2-\Delta{t}_1） \\-\lfloor \mathbf{{s}}^{(t_2)}-\mathbf{{s}}^{(t_1)} \rfloor_{\times}({}^{t_1}_{G}R)^{T} & \mathbf{0} & \mathbf{I}\end{bmatrix} \\&=\begin{bmatrix}{}_{t_1}^{t_2}R & 0 & 0 \\-\lfloor {}^{G}p_{t_2}-{}^{G}p_{t_1}-{}^{G}v_{t_1}\Delta{t}_{1}^{2}+\frac{1}{2}\mathbf{g}(\Delta{t}_{1}^{2})^2 \rfloor_{\times}({}^{t_1}_{G}R)^{T} & \mathbf{I} & \mathbf{I}(\Delta{t}_{1}^{2}) \\-\lfloor {}^{G}v_{t_2}-{}^{G}v_{t_1}+\mathbf{g}\Delta{t}_{1}^{2} \rfloor_{\times}({}^{t_1}_{G}R)^{T} & \mathbf{0} & \mathbf{I}\end{bmatrix} \\&=\begin{bmatrix}{}_{t_1}^{t_2}R & 0 & 0 \\-\lfloor {}^{G}p_{t_2}-{}^{G}p_{t_1}-{}^{G}v_{t_1}\Delta{t}_{1}^{2}+\frac{1}{2}\mathbf{g}(\Delta{t}_{1}^{2})^2 \rfloor_{\times}({}^{t_1}_{G}R)^{T} & \mathbf{I} & \mathbf{I}(\Delta{t}_{1}^{2}) \\-\lfloor {}^{G}v_{t_2}-{}^{G}v_{t_1}+\mathbf{g}\Delta{t}_{1}^{2} \rfloor_{\times}({}^{t_1}_{G}R)^{T} & \mathbf{0} & \mathbf{I}\end{bmatrix}\end{aligned} \tag{13} Φ(t2​,t1​)​=Φ(t2​,t0​)(Φ(t1​,t0​))−1=⎣⎡​t0​t2​​R−⌊y(t2​)⌋×​(Gt0​​R)T−⌊s(t2​)⌋×​(Gt0​​R)T​0I0​0IΔt2​I​⎦⎤​⎣⎡​t0​t1​​RT(⌊y(t1​)−s(t1​)Δt1​⌋×​)(Gt1​​R)T⌊s(t1​)⌋×​(Gt1​​R)T​0I0​0−IΔt1​I​⎦⎤​=⎣⎡​t1​t2​​R−⌊y(t2​)−y(t1​)+s^(t1​)Δt1​−s^(t1​)Δt2​⌋×​(Gt1​​R)T−⌊s(t2​)−s(t1​)⌋×​(Gt1​​R)T​0I0​0I(Δt2​−Δt1​）I​⎦⎤​=⎣⎡​t1​t2​​R−⌊Gpt2​​−Gpt1​​−Gvt1​​Δt12​+21​g(Δt12​)2⌋×​(Gt1​​R)T−⌊Gvt2​​−Gvt1​​+gΔt12​⌋×​(Gt1​​R)T​0I0​0I(Δt12​)I​⎦⎤​=⎣⎡​t1​t2​​R−⌊Gpt2​​−Gpt1​​−Gvt1​​Δt12​+21​g(Δt12​)2⌋×​(Gt1​​R)T−⌊Gvt2​​−Gvt1​​+gΔt12​⌋×​(Gt1​​R)T​0I0​0I(Δt12​)I​⎦⎤​​(13)

可以看到由微分方程推得的狀态轉移矩陣的閉式解和公式（1）基本一緻（感覺推了個寂寞），不過對于噪聲項的部分不太能保證。

視覺部分的觀測方程

下面的所有的下角标 l l l 表示id為 l l l 的相機，不表示時間，這裡暫時不涉及時間，可以認為是理想的觀測模型。

為了分析能觀性，這裡還需要一個步驟就是觀測模型，以單個觀測點 P f j P_{f_j} Pfj​​為例，其觀測模型為：

z l = π ( C l p f j ) + n l C l p f j = I C R l G R ( G p f j − G p I l ) + C p I (14) \begin{aligned} z_l&=\pi({}^{C_l}\mathrm{p}_{f_j})+n_{l} \\ {}^{C_l}\mathrm{p}_{f_j}&={}^{C}_{I}R {}^{G}_{l}R({}^{G}\mathrm{p}_{f_j}-{}^{G}\mathrm{p}_{I_l})+{}^{C}\mathrm{p}_I \end{aligned} \tag{14} zl​Cl​pfj​​​=π(Cl​pfj​​)+nl​=IC​RlG​R(Gpfj​​−GpIl​​)+CpI​​(14)

是以觀測模型為：

H ( I l ∣ l ) = J ( f j ∣ l ) I C R G I l R [ [ G p f j − G p I i ] × ( G I l R ) T ⏟ ∂ e / ∂ θ − I 3 × 3 ⏟ ∂ e / ∂ p 0 3 × 3 ⏟ ∂ e / ∂ v ] H ( f j ∣ l ) = J ( f j ∣ l ) I C R G I l R (15) \begin{aligned} H_{(I_l|l)}&=J_{(f_j|l)} \quad {}^{C}_{I}R \quad {}_{G}^{I_l}R\begin{bmatrix} \underbrace{\left[{}^{G}\mathrm{p}_{f_j}-{}^{G}\mathrm{p}_{I_i}\right]_{\times}({}_{G}^{I_l}R)^{T}}_{{\partial e}/{\partial \theta}} & \underbrace{ -\mathbf{I}_{3\times3}}_{{\partial e}/{\partial \mathrm{p}}} & \underbrace{ \mathbf{0}_{3\times3}}_{{\partial e}/{\partial \mathrm{v}}}\end{bmatrix} \\ H_{(f_j|l)}&=J_{(f_j|l)} \quad {}^{C}_{I}R \quad {}_{G}^{I_l}R \end{aligned} \tag{15} H(Il​∣l)​H(fj​∣l)​​=J(fj​∣l)​IC​RGIl​​R[∂e/∂θ

[Gpfj​​−GpIi​​]×​(GIl​​R)T​​​∂e/∂p

−I3×3​​​​∂e/∂v

03×3​​​​]=J(fj​∣l)​IC​RGIl​​R​(15)

其中：

J ( f j ∣ l ) = 1 Z [ 1 0 − X Z 0 1 − Y Z ] J_{(f_j|l)}=\frac{1}{Z}\begin{bmatrix}1 & 0 & -\frac{X}{Z} \\ 0 & 1 & -\frac{Y}{Z} \end{bmatrix} J(fj​∣l)​=Z1​[10​01​−ZX​−ZY​​]

理想情況下能觀性的分析

OC-KF在做能觀性分析的時候，不像參考【3】中所示的是在能觀性矩陣中抽出一部分進行通用的分析，該方法采用遞推的方式證明了在時刻 t，零空間應該是什麼樣的，且理想狀态下是如何傳播的（propagation），下面就兩個部分進行分析：

在 t 時刻，系統零空間是如何的

假設系統從 t0 時刻開始，那麼該時刻的零空間為：

N t 0 = [ 0 G t 0 R g I − ⌊ G p t 0 ⌋ × g 0 − ⌊ G v t 0 ⌋ × g I − ⌊ G p f j ⌋ × g ] (16) \mathbf{N}_{t_0}=\begin{bmatrix} \begin{array}{c|c} \mathbf{0} & {}^{t_0}_{G}R\mathbf{g} \\ \mathbf{I} & -\lfloor {}^{G}p_{t_0} \rfloor_{\times}\mathbf{g} \\ \mathbf{0} & -\lfloor {}^{G}v_{t_0} \rfloor_{\times}\mathbf{g} \\ \hline \mathbf{I} & -\lfloor {}^{G}p_{f_j} \rfloor_{\times}\mathbf{g} \end{array} \end{bmatrix} \tag{16} Nt0​​=⎣⎢⎢⎡​0I0I​Gt0​​Rg−⌊Gpt0​​⌋×​g−⌊Gvt0​​⌋×​g−⌊Gpfj​​⌋×​g​​​⎦⎥⎥⎤​(16)

根據能觀性矩陣的定義，在 t 時刻，對 f j f_j fj​ 特征點的能觀性矩陣的對應行：

O t = H f j Φ ( t , t 0 ) (17) \mathcal{O}_{t}=\mathbf{H}_{f_j}\Phi(t, t_0) \tag{17} Ot​=Hfj​​Φ(t,t0​)(17)

是以在 t 時刻，系統的零空間滿足：

O t N t = H f j Φ ( t , t 0 ) N t 0 ⏟ p a r t 1 (18) \mathcal{O}_{t}\mathbf{N}_t=\mathbf{H}_{f_j}\underbrace{\Phi(t, t_0)\mathbf{N}_{t_0}}_{part1} \tag{18} Ot​Nt​=Hfj​​part1

Φ(t,t0​)Nt0​​​​(18)

将公式（10）和公式（16）帶入到公式（18）的part1中，可以得到（在系統的傳播或者說預測階段不涉及到觀測部分，是以觀測部分的零空間照抄上去就好了）：

Φ ( t , t 0 ) N t 0 = [ t 0 t R 0 0 − ⌊ y ( t ) ⌋ × ( G t 0 R ) T I I Δ t − ⌊ s ( t ) ⌋ × ( G t 0 R ) T 0 I ] [ 0 G t 0 R g I − ⌊ G p t 0 ⌋ × g 0 − ⌊ G v t 0 ⌋ × g I − ⌊ G p f j ⌋ × g ] = [ 0 G t R g I − ⌊ G p t ⌋ × g 0 − ⌊ G v t ⌋ × g I − ⌊ G p f j ⌋ × g ] (19) \begin{aligned} \Phi(t,t_0)\mathbf{N}_{t_0}&=\begin{bmatrix} {}_{t_0}^{t}R & 0 & 0 \\ -\lfloor \mathbf{{y}}^{(t)} \rfloor_{\times}({}^{t_0}_{G}R)^{T} & \mathbf{I} & \mathbf{I}\Delta{t} \\ -\lfloor \mathbf{{s}}^{(t)} \rfloor_{\times}({}^{t_0}_{G}R)^{T} & \mathbf{0} & \mathbf{I} \end{bmatrix} \begin{bmatrix} \mathbf{0} & {}^{t_0}_{G}R\mathbf{g} \\ \mathbf{I} & -\lfloor {}^{G}p_{t_0} \rfloor_{\times}\mathbf{g} \\ \mathbf{0} & -\lfloor {}^{G}v_{t_0} \rfloor_{\times}\mathbf{g} \\ \hline \mathbf{I} & -\lfloor {}^{G}p_{f_j} \rfloor_{\times}\mathbf{g} \end{bmatrix} \\ &=\begin{bmatrix} \mathbf{0} & {}^{t}_{G}R\mathbf{g} \\ \mathbf{I} & -\lfloor {}^{G}p_{t} \rfloor_{\times}\mathbf{g} \\ \mathbf{0} & -\lfloor {}^{G}v_{t} \rfloor_{\times}\mathbf{g} \\ \hline \mathbf{I} & -\lfloor {}^{G}p_{f_j} \rfloor_{\times}\mathbf{g} \end{bmatrix} \end{aligned} \tag{19} Φ(t,t0​)Nt0​​​=⎣⎡​t0​t​R−⌊y(t)⌋×​(Gt0​​R)T−⌊s(t)⌋×​(Gt0​​R)T​0I0​0IΔtI​⎦⎤​⎣⎢⎢⎡​0I0I​Gt0​​Rg−⌊Gpt0​​⌋×​g−⌊Gvt0​​⌋×​g−⌊Gpfj​​⌋×​g​​⎦⎥⎥⎤​=⎣⎢⎢⎡​0I0I​Gt​Rg−⌊Gpt​⌋×​g−⌊Gvt​⌋×​g−⌊Gpfj​​⌋×​g​​⎦⎥⎥⎤​​(19)

可以看到在理想情況下，系統的零空間從在 t 時刻和初始時刻 t0 的零空間還是頗為相似的。需要注意的是在實際情況下，公式（19）中的變量均是由 t-1 時刻的值預測出來的。

相鄰時刻間系統的零空間是如何傳播的

另一方面 ，t1 到 t2 時刻的系統零空間滿足：

Φ ( t 2 , t 1 ) N t 1 = [ t 1 t 2 R 0 0 − ⌊ G p t 2 − G p t 1 − G v t 1 Δ t 1 2 + 1 2 g ( Δ t 1 2 ) 2 ⌋ × ( G t 1 R ) T I I ( Δ t 1 2 ) − ⌊ G v t 2 − G v t 1 + g Δ t 1 2 ⌋ × ( G t 1 R ) T 0 I ] [ 0 G t 1 R g I − ⌊ G p t 1 ⌋ × g 0 − ⌊ G v t 1 ⌋ × g ] = [ 0 G t 2 R g I − ⌊ G p t 2 ⌋ × g 0 − ⌊ G v t 2 ⌋ × g ] (20) \begin{aligned} \Phi(t_2, t_1)\mathbf{N}_{t_1}&= \begin{bmatrix} {}_{t_1}^{t_2}R & 0 & 0 \\ -\lfloor {}^{G}p_{t_2}-{}^{G}p_{t_1}-{}^{G}v_{t_1}\Delta{t}_{1}^{2}+\frac{1}{2}\mathbf{g}(\Delta{t}_{1}^{2})^2 \rfloor_{\times}({}^{t_1}_{G}R)^{T} & \mathbf{I} & \mathbf{I}(\Delta{t}_{1}^{2}) \\ -\lfloor {}^{G}v_{t_2}-{}^{G}v_{t_1}+\mathbf{g}\Delta{t}_{1}^{2} \rfloor_{\times}({}^{t_1}_{G}R)^{T} & \mathbf{0} & \mathbf{I} \end{bmatrix} \begin{bmatrix} \mathbf{0} & {}^{t_1}_{G}R\mathbf{g} \\ \mathbf{I} & -\lfloor {}^{G}p_{t_1} \rfloor_{\times}\mathbf{g} \\ \mathbf{0} & -\lfloor {}^{G}v_{t_1} \rfloor_{\times}\mathbf{g} \end{bmatrix} \\ &= \begin{bmatrix} \mathbf{0} & {}^{t_2}_{G}R\mathbf{g} \\ \mathbf{I} & -\lfloor {}^{G}p_{t_2} \rfloor_{\times}\mathbf{g} \\ \mathbf{0} & -\lfloor {}^{G}v_{t_2} \rfloor_{\times}\mathbf{g} \\ \end{bmatrix} \end{aligned} \tag{20} Φ(t2​,t1​)Nt1​​​=⎣⎡​t1​t2​​R−⌊Gpt2​​−Gpt1​​−Gvt1​​Δt12​+21​g(Δt12​)2⌋×​(Gt1​​R)T−⌊Gvt2​​−Gvt1​​+gΔt12​⌋×​(Gt1​​R)T​0I0​0I(Δt12​)I​⎦⎤​⎣⎡​0I0​Gt1​​Rg−⌊Gpt1​​⌋×​g−⌊Gvt1​​⌋×​g​⎦⎤​=⎣⎡​0I0​Gt2​​Rg−⌊Gpt2​​⌋×​g−⌊Gvt2​​⌋×​g​⎦⎤​​(20)

t 時刻傳播過來的零空間是否是觀測矩陣的零空間

在 t 時刻，觀測矩陣為：

H t = J f j I C R G t R [ [ G p f j − G p t ] × ( G t R ) T ⏟ ∂ e / ∂ θ − I 3 × 3 ⏟ ∂ e / ∂ p 0 3 × 3 ⏟ ∂ e / ∂ v I 3 × 3 ⏟ ∂ e / ∂ p f j ] (21) \begin{aligned} \mathbf{H}_{t}&=J_{f_j}{}^{C}_{I}R{}_{G}^{t}R\begin{bmatrix} \begin{array}{ccc|c} \underbrace{\left[{}^{G}\mathrm{p}_{f_j}-{}^{G}\mathrm{p}_{t}\right]_{\times}({}_{G}^{t}R)^{T}}_{{\partial e}/{\partial \theta}} & \underbrace{ -\mathbf{I}_{3\times3}}_{{\partial e}/{\partial \mathrm{p}}} & \underbrace{ \mathbf{0}_{3\times3}}_{{\partial e}/{\partial \mathrm{v}}} & \underbrace{\mathbf{I}_{3\times3}}_{\partial e/\partial p_{f_j}} \end{array} \end{bmatrix} \end{aligned} \tag{21} Ht​​=Jfj​​IC​RGt​R[∂e/∂θ

[Gpfj​​−Gpt​]×​(Gt​R)T​​​∂e/∂p

−I3×3​​​​∂e/∂v

03×3​​​​∂e/∂pfj​​

I3×3​​​​​]​(21)

結合公式（19）易得：

H t N t = [ [ G p f j − G p t ] × ( G t R ) T − I 3 × 3 0 3 × 3 I 3 × 3 ] [ 0 G t R g I − ⌊ G p t ⌋ × g 0 − ⌊ G v t ⌋ × g I − ⌊ G p f j ⌋ × g ] = 0 \begin{aligned} \mathbf{H}_{t}\mathbf{N}_{t}&= \begin{bmatrix} \left[{}^{G}\mathrm{p}_{f_j}-{}^{G}\mathrm{p}_{t}\right]_{\times}({}_{G}^{t}R)^{T} & -\mathbf{I}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{I}_{3\times3} \end{bmatrix} \begin{bmatrix} \mathbf{0} & {}^{t}_{G}R\mathbf{g} \\ \mathbf{I} & -\lfloor {}^{G}p_{t} \rfloor_{\times}\mathbf{g} \\ \mathbf{0} & -\lfloor {}^{G}v_{t} \rfloor_{\times}\mathbf{g} \\ \mathbf{I} & -\lfloor {}^{G}p_{f_j} \rfloor_{\times}\mathbf{g} \end{bmatrix} \\ &= \mathbf{0} \end{aligned} Ht​Nt​​=[[Gpfj​​−Gpt​]×​(Gt​R)T​−I3×3​​03×3​​I3×3​​]⎣⎢⎢⎡​0I0I​Gt​Rg−⌊Gpt​⌋×​g−⌊Gvt​⌋×​g−⌊Gpfj​​⌋×​g​⎦⎥⎥⎤​=0​

小結

由上述分析可知，在理想情況下：

1. 性質一：系統的零空間可以通過狀态轉移矩陣傳播到目前時刻 t，且形式與初始零空間相似，由 t 時刻狀态的預測值有關；
2. 性質二：相鄰時刻的零空間可以通過狀态轉移矩陣進行傳播，且零空間的形式也滿足通用形式；
3. 性質三：通過狀态傳遞矩陣傳播到目前時刻 t 的零空間與觀測矩陣正交，亦即優化問題的優化方向與零空間正交，是以優化量不會破壞系統的零空間；

OC-KF對于零空間的處理

Notation:

一下均是從實際情況出發了。

狀态變量中僅有IMU的位姿以及空間中特征點的位置，不像MSCKF有一個滑動視窗記錄所有的相機位姿，是以某個時刻的位姿一旦被優化了，那麼該位姿的觀測方程的線性化點就确定了，後面不會改變，這是和MSCKF的很大的不同；

OC-KF想做什麼？

通過上面對理想情況的分析，OC-KF建立的一個比較理想化的方法，該方法把上述的三個性質使用的淋漓盡緻：

1. 在時刻 t 構成的能觀性矩陣中的項目為：

   O ^ t = H ^ f j Φ ^ ( t , t − 1 ) Φ ^ ( t − 1 , t − 2 ) … Φ ^ ( t 1 , t 0 ) (22) \hat{\mathcal{O}}_{t}=\hat{\mathbf{H}}_{f_j}\hat{\Phi}(t,t-1)\hat{\Phi}(t-1,t-2)\dots\hat{\Phi}(t_1, t_0) \tag{22} O^t​=H^fj​​Φ^(t,t−1)Φ^(t−1,t−2)…Φ^(t1​,t0​)(22)

2. OC-KF認為，在 t 時刻，之前的狀态都已經通過最優化的方法求得了最優解，那麼使用性質一，t0 時刻的零空間可以通過 Φ ^ ( t − 1 , t 0 ) \hat{\Phi}(t-1, t_0) Φ^(t−1,t0​)傳播到 t-1 時刻，寫作：

   N t − 1 = [ 0 G t − 1 R ( t − 2 ) g I − ⌊ G p t − 1 ( t − 2 ) ⌋ × g 0 − ⌊ G v t − 1 ( t − 2 ) ⌋ × g I − ⌊ G p f j ⌋ × g ] = Φ ( t − 1 , t 0 ) ⏟ o p t i m a l ∗ N t 0 (23) \mathbf{N}_{t-1}= \begin{bmatrix} \mathbf{0} & {}^{t-1}_{G}R^{(t-2)}\mathbf{g} \\ \mathbf{I} & -\lfloor {}^{G}p_{t-1}^{(t-2)} \rfloor_{\times}\mathbf{g} \\ \mathbf{0} & -\lfloor {}^{G}v_{t-1}^{(t-2)} \rfloor_{\times}\mathbf{g} \\ \hline \mathbf{I} & -\lfloor {}^{G}p_{f_j} \rfloor_{\times}\mathbf{g} \end{bmatrix} =\underbrace{\Phi(t-1,t_0)}_{optimal^{*}}\mathbf{N}_{t_0} \tag{23} Nt−1​=⎣⎢⎢⎡​0I0I​Gt−1​R(t−2)g−⌊Gpt−1(t−2)​⌋×​g−⌊Gvt−1(t−2)​⌋×​g−⌊Gpfj​​⌋×​g​​⎦⎥⎥⎤​=optimal∗

   Φ(t−1,t0​)​​Nt0​​(23)

   其中 o p t i m a l ∗ optimal^{*} optimal∗表示算法認為之前的處理已經趨于理想情況了；其中的 t-1 時刻的零空間是由狀态轉移矩陣傳播過來的，是以零空間中的變量均是使用 t-2 時刻的值預測出來的；

3. 于 t 時刻的狀态轉移矩陣 Φ ^ ( t , t − 1 ) \hat{\Phi}(t, t-1) Φ^(t,t−1)而言，根據性質二，該狀态轉移矩陣必然可以将零空間 N t − 1 \mathbf{N}_{t-1} Nt−1​傳播為 N t \mathbf{N}_t Nt​，且形式上滿足通用形式：

   N t = [ 0 G t R ( t − 1 ) g I − ⌊ G p t ( t − 1 ) ⌋ × g 0 − ⌊ G v t ( t − 1 ) ⌋ × g I − ⌊ G p f j ⌋ × g ] = Φ ˇ ( t , t − 1 ) N t − 1 = Φ ˇ ( t , t − 1 ) [ 0 G t − 1 R ( t − 2 ) g I − ⌊ G p t − 1 ( t − 2 ) ⌋ × g 0 − ⌊ G v t − 1 ( t − 2 ) ⌋ × g I − ⌊ G p f j ⌋ × g ] (24) \mathbf{N}_{t}= \begin{bmatrix} \mathbf{0} & {}^{t}_{G}R^{(t-1)}\mathbf{g} \\ \mathbf{I} & -\lfloor {}^{G}p_{t}^{(t-1)} \rfloor_{\times}\mathbf{g} \\ \mathbf{0} & -\lfloor {}^{G}v_{t}^{(t-1)} \rfloor_{\times}\mathbf{g} \\ \mathbf{I} & -\lfloor {}^{G}p_{f_j} \rfloor_{\times}\mathbf{g} \end{bmatrix} = \check{\Phi}(t,t-1)\mathbf{N}_{t-1} = \check{\Phi}(t, t-1) \begin{bmatrix} \mathbf{0} & {}^{t-1}_{G}R^{(t-2)}\mathbf{g} \\ \mathbf{I} & -\lfloor {}^{G}p_{t-1}^{(t-2)} \rfloor_{\times}\mathbf{g} \\ \mathbf{0} & -\lfloor {}^{G}v_{t-1}^{(t-2)} \rfloor_{\times}\mathbf{g} \\ \mathbf{I} & -\lfloor {}^{G}p_{f_j} \rfloor_{\times}\mathbf{g} \end{bmatrix}\tag{24} Nt​=⎣⎢⎢⎡​0I0I​Gt​R(t−1)g−⌊Gpt(t−1)​⌋×​g−⌊Gvt(t−1)​⌋×​g−⌊Gpfj​​⌋×​g​⎦⎥⎥⎤​=Φˇ(t,t−1)Nt−1​=Φˇ(t,t−1)⎣⎢⎢⎡​0I0I​Gt−1​R(t−2)g−⌊Gpt−1(t−2)​⌋×​g−⌊Gvt−1(t−2)​⌋×​g−⌊Gpfj​​⌋×​g​⎦⎥⎥⎤​(24)

   其中 t 時刻的零空間是由 t-1 時刻的零空間經由相鄰時間的狀态轉移矩陣傳播過來的，是以也是使用預測值；公式中使用 Φ ˇ \check{\Phi} Φˇ來表示期望的傳遞矩陣。

4. 對于 t 時刻的觀測矩陣 H ^ ( t ) \hat{\mathbf{H}}(t) H^(t)，根據性質三，該觀測矩陣要與零空間 N t \mathbf{N}_t Nt​正交：

H ˇ f j N t = 0 (25) \check{\mathbf{H}}_{f_j}\mathbf{N}_{t} =\mathbf{0} \tag{25} Hˇfj​​Nt​=0(25)

分析到這裡其實就比較明确了，OC-KF的核心就是期望找到合适的狀态轉移矩陣和合适的觀測矩陣，使得零空間的傳播和優化方向與理想情況下的相同。

下面其實就可以針對公式（24）（25）進行分析了。

修改狀态轉移矩陣 Φ ^ ( t , t − 1 ) \hat{\Phi}(t, t-1) Φ^(t,t−1)

實際情況下，t-1 時刻到 t 時刻的狀态轉移矩陣為：

Φ ^ ( t , t − 1 ) = e x p ( F ( t , t − 1 ) Δ t ) = [ Φ ^ 11 0 0 Φ ^ 21 I I Δ t Φ ^ 31 0 I ] (26) \hat{\Phi}(t, t-1)=exp(\mathbf{F}(t, t-1)\Delta{t})= \begin{bmatrix} \hat{\Phi}_{11} & \mathbf{0} & \mathbf{0} \\ \hat{\Phi}_{21} & \mathbf{I} & \mathbf{I}\Delta{t} \\ \hat{\Phi}_{31} & \mathbf{0} & \mathbf{I} \end{bmatrix} \tag{26} Φ^(t,t−1)=exp(F(t,t−1)Δt)=⎣⎡​Φ^11​Φ^21​Φ^31​​0I0​0IΔtI​⎦⎤​(26)

根據公式（24）有：

N t = [ 0 G t R ( t − 1 ) g I − ⌊ G p t ( t − 1 ) ⌋ × g 0 − ⌊ G v t ( t − 1 ) ⌋ × g I − ⌊ G p f j ⌋ × g ] = [ Φ ˇ 11 0 0 Φ ˇ 21 I I Δ t Φ ˇ 31 0 I ] [ 0 G t − 1 R ( t − 2 ) g I − ⌊ G p t − 1 ( t − 2 ) ⌋ × g 0 − ⌊ G v t − 1 ( t − 2 ) ⌋ × g I − ⌊ G p f j ⌋ × g ] (27) \mathbf{N}_{t}= \begin{bmatrix} \mathbf{0} & {}^{t}_{G}R^{(t-1)}\mathbf{g} \\ \mathbf{I} & -\lfloor {}^{G}p_{t}^{(t-1)} \rfloor_{\times}\mathbf{g} \\ \mathbf{0} & -\lfloor {}^{G}v_{t}^{(t-1)} \rfloor_{\times}\mathbf{g} \\ \hline \mathbf{I} & -\lfloor {}^{G}p_{f_j} \rfloor_{\times}\mathbf{g} \end{bmatrix}= \begin{bmatrix} \check{\Phi}_{11} & \mathbf{0} & \mathbf{0} \\ \check{\Phi}_{21} & \mathbf{I} & \mathbf{I}\Delta{t} \\ \check{\Phi}_{31} & \mathbf{0} & \mathbf{I} \end{bmatrix} \begin{bmatrix} \mathbf{0} & {}^{t-1}_{G}R^{(t-2)}\mathbf{g} \\ \mathbf{I} & -\lfloor {}^{G}p_{t-1}^{(t-2)} \rfloor_{\times}\mathbf{g} \\ \mathbf{0} & -\lfloor {}^{G}v_{t-1}^{(t-2)} \rfloor_{\times}\mathbf{g} \\ \hline \mathbf{I} & -\lfloor {}^{G}p_{f_j} \rfloor_{\times}\mathbf{g} \end{bmatrix} \tag{27} Nt​=⎣⎢⎢⎡​0I0I​Gt​R(t−1)g−⌊Gpt(t−1)​⌋×​g−⌊Gvt(t−1)​⌋×​g−⌊Gpfj​​⌋×​g​​⎦⎥⎥⎤​=⎣⎡​Φˇ11​Φˇ21​Φˇ31​​0I0​0IΔtI​⎦⎤​⎣⎢⎢⎡​0I0I​Gt−1​R(t−2)g−⌊Gpt−1(t−2)​⌋×​g−⌊Gvt−1(t−2)​⌋×​g−⌊Gpfj​​⌋×​g​​⎦⎥⎥⎤​(27)

這裡依舊隻考慮IMU狀态量，也就是矩陣橫線下面特征點部分不考慮。

把公式（27）各項展開有：

{ G t R ( t − 1 ) g = Φ ˇ 11 G t − 1 R ( t − 2 ) g − ⌊ G p t ( t − 1 ) ⌋ × g = Φ ˇ 21 G t − 1 R ( t − 2 ) g − ⌊ G p t − 1 ( t − 2 ) ⌋ × g − ⌊ G v t − 1 ( t − 2 ) Δ t ⌋ × g − ⌊ G v t ( t − 1 ) ⌋ × g = Φ ˇ 31 G t − 1 R ( t − 2 ) g − ⌊ G v t − 1 ( t − 2 ) ⌋ × g (28) \begin{aligned} \begin{cases} {}^{t}_{G}R^{(t-1)}\mathbf{g} &= \check{\Phi}_{11} {}^{t-1}_{G}R^{(t-2)}\mathbf{g} \\ -\lfloor {}^{G}p_{t}^{(t-1)} \rfloor_{\times}\mathbf{g} &= \check{\Phi}_{21}{}^{t-1}_{G}R^{(t-2)}\mathbf{g}-\lfloor {}^{G}p_{t-1}^{(t-2)} \rfloor_{\times}\mathbf{g} - \lfloor {}^{G}v_{t-1}^{(t-2)} \Delta{t} \rfloor_{\times}\mathbf{g}\\ -\lfloor {}^{G}v_{t}^{(t-1)} \rfloor_{\times}\mathbf{g} &= \check{\Phi}_{31}{}^{t-1}_{G}R^{(t-2)}\mathbf{g}-\lfloor {}^{G}v_{t-1}^{(t-2)} \rfloor_{\times}\mathbf{g} \end{cases} \end{aligned} \tag{28} ⎩⎪⎨⎪⎧​Gt​R(t−1)g−⌊Gpt(t−1)​⌋×​g−⌊Gvt(t−1)​⌋×​g​=Φˇ11​Gt−1​R(t−2)g=Φˇ21​Gt−1​R(t−2)g−⌊Gpt−1(t−2)​⌋×​g−⌊Gvt−1(t−2)​Δt⌋×​g=Φˇ31​Gt−1​R(t−2)g−⌊Gvt−1(t−2)​⌋×​g​​(28)

公式（28）中第一行很容易求解：

Φ ˇ 11 = G t R ( t − 1 ) ( G t − 1 R ( t − 2 ) ) T (29) \check{\Phi}_{11}={}^{t}_{G}R^{(t-1)}({}^{t-1}_{G}R^{(t-2)})^{T} \tag{29} Φˇ11​=Gt​R(t−1)(Gt−1​R(t−2))T(29)

第二行和第三行在求解的時候，作者建構了一個最優化問題，該問題滿足如下公式：

m i n Φ ˇ ∥ Φ ˇ − Φ ^ ∥ F 2 s . t . Φ ˇ u = w (30) \begin{aligned} \mathop{min}_{\check{\Phi}} &\quad \left\| \check{\Phi}-\hat{\Phi} \right\|_{\mathcal{F}}^{2} \\ s.t. &\quad \check{\Phi}\mathrm{u}=\mathrm{w} \end{aligned} \tag{30} minΦˇ​s.t.​∥∥∥​Φˇ−Φ^∥∥∥​F2​Φˇu=w​(30)

其中：

1. 對于第二行和第三行， u = G t − 1 R ( t − 2 ) \mathrm{u}={}^{t-1}_{G}R^{(t-2)} u=Gt−1​R(t−2)；
2. 對于第二行， w = ⌊ G p t − 1 ( t − 2 ) + G v t − 1 ( t − 2 ) Δ t − G p t ( t − 1 ) ⌋ × \mathrm{w}=\lfloor {}^{G}p_{t-1}^{(t-2)}+{}^{G}v_{t-1}^{(t-2)}\Delta{t}-{}^{G}p_{t}^{(t-1)} \rfloor_{\times} w=⌊Gpt−1(t−2)​+Gvt−1(t−2)​Δt−Gpt(t−1)​⌋×​；
3. 對于第三行， w = ⌊ G v t − 1 ( t − 2 ) − G v t ( t − 1 ) ⌋ × \mathrm{w}=\lfloor {}^{G}v_{t-1}^{(t-2)}-{}^{G}v_{t}^{(t-1)} \rfloor_{\times} w=⌊Gvt−1(t−2)​−Gvt(t−1)​⌋×​；

對于公式（30）所示的優化問題而言，采用拉格朗日乘子法和KKT條件求解對偶問題，詳細步驟如下：

1. 建構拉格朗日函數 L ( Φ ˇ , α ) = ∥ Φ ˇ − Φ ^ ∥ F 2 + α ( Φ ˇ u − w ) L(\check{\Phi}, \alpha)=\left\| \check{\Phi}-\hat{\Phi} \right\|_{\mathcal{F}}^{2}+\alpha(\check{\Phi}\mathrm{u}-\mathrm{w}) L(Φˇ,α)=∥∥∥​Φˇ−Φ^∥∥∥​F2​+α(Φˇu−w)；則原始問題為：

   m i n Φ ˇ m a x α ∥ Φ ˇ − Φ ^ ∥ F 2 + α ( Φ ˇ u − w ) ⏟ L ( Φ ˇ , α ) (31) \mathop{min}_{\check{\Phi}} \mathop{max}_{\alpha} \underbrace{ \left\| \check{\Phi}-\hat{\Phi} \right\|_{\mathcal{F}}^{2}+\alpha(\check{\Phi}\mathrm{u}-\mathrm{w})}_{ L(\check{\Phi}, \alpha)} \tag{31} minΦˇ​maxα​L(Φˇ,α)

   ∥∥∥​Φˇ−Φ^∥∥∥​F2​+α(Φˇu−w)​​(31)

2. 在滿足KKT條件下（因為隻涉及到等式限制，是以KKT條件隻滿足等式限制和梯度限制就可以了）求解原始問題的對偶問題：

   m a x α m i n Φ ˇ ∥ Φ ˇ − Φ ^ ∥ F 2 ⏟ f ( Φ ˇ ) + α ( Φ ˇ u − w ) ⏟ g ( Φ ˇ ) s . t . ∂ f ( Φ ˇ ) ∂ Φ ˇ + α ∂ g ( Φ ˇ ) ∂ Φ ˇ = 2 ( Φ ˇ − Φ ^ ) + α u = 0 限制1 Φ ˇ u − w = 0 限制2 (32) \begin{aligned} \mathop{max}_{\alpha} \mathop{min}_{\check{\Phi}} &\quad \underbrace{ \left\| \check{\Phi}-\hat{\Phi} \right\|_{\mathcal{F}}^{2}}_{f(\check{\Phi})}+\underbrace{\alpha(\check{\Phi}\mathrm{u}-\mathrm{w})}_{g(\check{\Phi})} \\ s.t. &\quad \frac{\partial f(\check{\Phi})}{\partial \check{\Phi}}+\alpha \frac{\partial g(\check{\Phi})}{\partial \check{\Phi}}=2(\check{\Phi}-\hat{\Phi})+\alpha\mathrm{u}= 0 \quad \text{限制1} \\ &\quad \check{\Phi}\mathrm{u}-\mathrm{w} = 0 \quad \text{限制2} \end{aligned} \tag{32} maxα​minΦˇ​s.t.​f(Φˇ)

   ∥∥∥​Φˇ−Φ^∥∥∥​F2​​​+g(Φˇ)

   α(Φˇu−w)​​∂Φˇ∂f(Φˇ)​+α∂Φˇ∂g(Φˇ)​=2(Φˇ−Φ^)+αu=0限制1Φˇu−w=0限制2​(32)

3. 将限制 1 推得的 Φ ˇ = Φ ^ − 1 2 α u \check{\Phi}=\hat{\Phi}-\frac{1}{2}\alpha\mathrm{u} Φˇ=Φ^−21​αu帶入到對偶問題中得到：

   m a x α − 1 4 α 2 u T u + α ( Φ ^ u − w ) (33) \begin{aligned} \mathop{max}_{\alpha} &\quad -\frac{1}{4}\alpha^{2}\mathrm{u}^T\mathrm{u}+\alpha(\hat{\Phi}\mathrm{u}-\mathrm{w}) \end{aligned} \tag{33} maxα​​−41​α2uTu+α(Φ^u−w)​(33)

   對 α \alpha α求導為0可得 α = 2 ( Φ ^ u − w ) ( u T u ) − 1 \alpha=2(\hat{\Phi}\mathrm{u}-\mathrm{w})(\mathrm{u}^{T}\mathrm{u})^{-1} α=2(Φ^u−w)(uTu)−1，将結果帶入限制 1 的推論中可得：

   Φ ˇ = Φ ^ − ( Φ ^ u − w ) ( u T u ) − 1 u T (34) \check{\Phi}=\hat{\Phi}-(\hat{\Phi}\mathrm{u}-\mathrm{w})(\mathrm{u}^{T}\mathrm{u})^{-1}\mathrm{u}^{T} \tag{34} Φˇ=Φ^−(Φ^u−w)(uTu)−1uT(34)

   最後的 u 取轉置是為了次元的适配，該最優解帶入KKT條件的限制 2 可得：

   Φ ˇ u − w = ( Φ ^ − ( Φ ^ u − w ) ( u T u ) − 1 u T ) u − w = Φ ^ u − w − ( Φ ^ u − w ) = 0 (35) \begin{aligned} \check{\Phi}\mathrm{u}-\mathrm{w}&=(\hat{\Phi}-(\hat{\Phi}\mathrm{u}-\mathrm{w})(\mathrm{u}^{T}\mathrm{u})^{-1}\mathrm{u}^T)\mathrm{u}-\mathrm{w} \\ &=\hat{\Phi}\mathrm{u}-\mathrm{w}-(\hat{\Phi}\mathrm{u}-\mathrm{w}) \\ &=\mathbf{0} \end{aligned} \tag{35} Φˇu−w​=(Φ^−(Φ^u−w)(uTu)−1uT)u−w=Φ^u−w−(Φ^u−w)=0​(35)

   于是公式（34）滿足KKT條件，對偶問題的最優解就是原始問題的最優解；

将公式（29）和公式（34）所得到的部分帶入到公式（26）中就可以得到修改之後的狀态轉移矩陣，該矩陣為：

Φ ˇ ( t , t − 1 ) = [ G t R ( t − 1 ) ( G t − 1 R ( t − 2 ) ) T 0 0 Φ ˇ 21 I I Δ t Φ ˇ 31 0 I ] (36) \check{\Phi}(t, t-1)= \begin{bmatrix} {}^{t}_{G}R^{(t-1)}({}^{t-1}_{G}R^{(t-2)})^{T} & \mathbf{0} & \mathbf{0} \\ \check{\Phi}_{21} & \mathbf{I} & \mathbf{I}\Delta{t} \\ \check{\Phi}_{31} & \mathbf{0} & \mathbf{I} \end{bmatrix} \tag{36} Φˇ(t,t−1)=⎣⎡​Gt​R(t−1)(Gt−1​R(t−2))TΦˇ21​Φˇ31​​0I0​0IΔtI​⎦⎤​(36)

修改觀測矩陣 H ^ ( t ) \hat{\mathbf{H}}(t) H^(t)

修改過狀态轉移矩陣之後，t 時刻的零空間如公式（27）左邊部分所示，是以我們需要找到最優的觀測矩陣 H ˇ ( t ) \check{\mathbf{H}}(t) Hˇ(t)，使之滿足公式（25）：

H ˇ f j N t = [ H θ ~ H p ~ H v ~ H f j ] [ 0 G t R ( t − 1 ) g I − ⌊ G p t ( t − 1 ) ⌋ × g 0 − ⌊ G v t ( t − 1 ) ⌋ × g I − ⌊ G p f j ⌋ × g ] (37) \begin{aligned} \check{\mathbf{H}}_{f_j}\mathbf{N}_{t} = \begin{bmatrix}\begin{array}{ccc|c} \mathbf{H}_{\tilde{\theta}} & \mathbf{H}_{\tilde{p}} & \mathbf{H}_{\tilde{v}} & \mathbf{H}_{f_j} \end{array}\end{bmatrix} \begin{bmatrix} \mathbf{0} & {}^{t}_{G}R^{(t-1)}\mathbf{g} \\ \mathbf{I} & -\lfloor {}^{G}p_{t}^{(t-1)} \rfloor_{\times}\mathbf{g} \\ \mathbf{0} & -\lfloor {}^{G}v_{t}^{(t-1)} \rfloor_{\times}\mathbf{g} \\ \hline \mathbf{I} & -\lfloor {}^{G}p_{f_j} \rfloor_{\times}\mathbf{g} \end{bmatrix} \end{aligned} \tag{37} Hˇfj​​Nt​=[Hθ~​​Hp~​​​Hv~​​Hfj​​​​]⎣⎢⎢⎡​0I0I​Gt​R(t−1)g−⌊Gpt(t−1)​⌋×​g−⌊Gvt(t−1)​⌋×​g−⌊Gpfj​​⌋×​g​​⎦⎥⎥⎤​​(37)

由零空間的第一列可以得到 H f j = − H θ ~ \mathbf{H}_{f_j}=-\mathbf{H}_{\tilde{\theta}} Hfj​​=−Hθ~​，該結論很重要，它訓示了當算法修改了前面的觀測矩陣元素時，對應的點的元素也要修改！不過這裡也提供了一些友善，我們可以僅僅關注前三個元素，當求出位置的元素之後可以直接替換掉點的元素部分。

于是重點其實還是在第二行，作者和修改狀态轉移矩陣一樣，也是建構了一個最優化問題進行求解：

m i n H ˇ ∥ H ˇ − H ^ ∥ F 2 s . t . H ˇ u = 0 (38) \begin{aligned} \mathop{min}_{\check{\mathbf{H}}} &\quad \left\| \check{\mathbf{H}} - \hat{\mathbf{H}} \right\|^{2}_{\mathcal{F}} \\ s.t. &\quad \check{\mathbf{H}}\mathbf{u}=\mathbf{0} \end{aligned} \tag{38} minHˇ​s.t.​∥∥∥​Hˇ−H^∥∥∥​F2​Hˇu=0​(38)

其中：

• H ˇ \check{\mathbf{H}} Hˇ表示要求解的最優值；

• H ^ \hat{\mathbf{H}} H^表示實際的觀測矩陣，如下：

  H t = J f j ( t ) I C R G I t R ( t − 1 ) [ [ G p ^ f j − G p ^ I t ( t − 1 ) ] × ( G I t R ( t − 1 ) ) T ⏟ H θ − I 3 × 3 ⏟ H p 0 3 × 3 ⏟ H v ] (39) H_{t}=J_{f_j}^{(t)} \quad {}^{C}_{I}R \quad {}_{G}^{I_t}R^{(t-1)}\begin{bmatrix} \underbrace{\left[{}^{G}\hat{\mathrm{p}}_{f_j}-{}^{G}\hat{\mathrm{p}}_{I_t}^{(t-1)}\right]_{\times}({}_{G}^{I_t}R^{(t-1)})^{T}}_{\mathbf{H}_{\theta}} & \underbrace{ -\mathbf{I}_{3\times3}}_{\mathbf{H}_{p}} & \underbrace{ \mathbf{0}_{3\times3}}_{\mathbf{H}_{v}}\end{bmatrix} \tag{39} Ht​=Jfj​(t)​IC​RGIt​​R(t−1)⎣⎡​Hθ​

  [Gp^​fj​​−Gp^​It​(t−1)​]×​(GIt​​R(t−1))T​​​Hp​

  −I3×3​​​​Hv​

  03×3​​​​⎦⎤​(39)

• u \mathbf{u} u表示零空間中的元素，如下：

  u = [ ( G t R ( t − 1 ) g ) T ( − ⌊ G p t ( t − 1 ) ⌋ × g ) T ( − ⌊ G v t ( t − 1 ) ⌋ × g ) T ] T (40) \mathbf{u}=\begin{bmatrix} ({}^{t}_{G}R^{(t-1)}\mathbf{g})^{T} & (-\lfloor {}^{G}p_{t}^{(t-1)} \rfloor_{\times}\mathbf{g})^{T} & (-\lfloor {}^{G}v_{t}^{(t-1)} \rfloor_{\times}\mathbf{g})^{T} \end{bmatrix}^{T} \tag{40} u=[(Gt​R(t−1)g)T​(−⌊Gpt(t−1)​⌋×​g)T​(−⌊Gvt(t−1)​⌋×​g)T​]T(40)

依舊使用拉格朗日乘子法和KKT求得最優解為：

H ˇ ( t ) = H ^ ( t ) − H ^ ( t ) u ( u T u ) − 1 u T (41) \check{\mathbf{H}}(t)=\hat{\mathbf{H}}(t)-\hat{\mathbf{H}}(t)\mathrm{u}(\mathrm{u}^{T}\mathrm{u})^{-1}\mathrm{u}^{T} \tag{41} Hˇ(t)=H^(t)−H^(t)u(uTu)−1uT(41)

小結

從本節的分析可以得知：

1. OC-KF建立了一個比較理想的模型：算法認為 t 時刻之前的狀态接近理想狀态，是以需要維護的零空間可以按照理想情況從初始時刻傳播過來，滿足公式（23）；
2. 在 t 時刻的預測階段，OC-KF通過修改 t-1 到 t 的狀态傳遞矩陣強制使 t 時刻的零空間滿足理想情況下的形式，如公式（27）所示，這一步也使得後面再次進行步驟 1 時能滿足必要的條件；
3. 在 t 時刻的更新階段，OC-KF通過修改觀測矩陣強制試該優化方向與 t 時刻的零空間正交，一方面保證了能觀性，另一方面保證了優化出來的參數變量不會影響 t 時刻的零空間，即還是步驟 2 中的零空間，使得後面再次進行步驟 1 是能滿足必要的條件；

那麼随之而來一個問題：這麼強制的優化方向如果不好，使之偏離了理想值，那麼步驟 1 所依賴的理想條件是不是就有點兒自相沖突？

零空間：有兩個年輕人，一個 Φ ˇ ( t , t − 1 ) \check{\Phi}(t, t-1) Φˇ(t,t−1)，一個 H ˇ ( t ) \check{\mathbf{H}}(t) Hˇ(t)，上來就是一個拉格朗日乘子法，一個KKT條件，一個矩陣替換，很快啊！他們說他們是亂打的，可不是亂打的，高等數學，線性代數，最優化，訓練有素，有備而來。這兩個年輕人不講武德，來騙，來偷襲我這個t0時刻的老零空間，這好嗎？這不好！我勸這兩位年輕人耗子尾汁，系統内部要講武德，不要窩裡鬥。

OC-KF方法在MSCKF中的應用

參考【1】所述的開源的S-MSCKF采用了上述的OC-KF對于零空間的維護方法，不同的是MSCKF維護了一個相機姿态的視窗，視窗内的相機位姿都還會被繼續更新。

是以這裡跟FEJ的分析方法相似，也以 α i + 1 \alpha_{i+1} αi+1​ 時刻對于節點 l l l 的能觀性矩陣項目分析，假設節點 l l l 是在 t 時刻預測和更新，則對應的項目如下：

O ^ l ( α i + 1 ) = H ^ f j ∣ l ( α i + 1 ) Φ ˇ ( t , t − 1 ) Φ ˇ ( t − 1 , t − 2 ) … Φ ˇ ( t 1 , t 0 ) ⏟ Φ ( t , t 0 ) (42) \hat{\mathcal{O}}_{l}^{(\alpha_{i+1})} = \hat{\mathbf{H}}_{f_j|l}^{(\alpha_{i+1})}\underbrace{ \check{\Phi}(t,t-1)\check{\Phi}(t-1,t-2)\dots\check{\Phi}(t_1,t_0)}_{\Phi(t, t_0)} \tag{42} O^l(αi+1​)​=H^fj​∣l(αi+1​)​Φ(t,t0​)

Φˇ(t,t−1)Φˇ(t−1,t−2)…Φˇ(t1​,t0​)​​(42)

和參考【1】分析一樣，因為在 t 時刻之後，之前的狀态矩陣已經确定了，是以在 α i + 1 \alpha_{i+1} αi+1​ 時刻，系統狀态矩陣部分的連乘已經确定（注意是經過修改之後的），不再變化，是以此時的系統零空間依舊為 t 時刻的系統零空間：

N t = [ 0 G t R ( t − 1 ) g I − ⌊ G p t ( t − 1 ) ⌋ × g I − ⌊ G p f j ⌋ × g ] (43) \mathbf{N}_{t}=\begin{bmatrix} \mathbf{0} & {}^{t}_{G}R^{(t-1)}\mathbf{g} \\ \mathbf{I} & -\lfloor {}^{G}p_{t}^{(t-1)} \rfloor_{\times}\mathbf{g} \\ \hline \mathbf{I} & -\lfloor {}^{G}p_{f_j} \rfloor_{\times}\mathbf{g} \end{bmatrix} \tag{43} Nt​=⎣⎡​0II​Gt​R(t−1)g−⌊Gpt(t−1)​⌋×​g−⌊Gpfj​​⌋×​g​​⎦⎤​(43)

這裡因為視窗中記錄的變量僅為位姿，沒有速度，是以這裡不再考慮速度進來。是以問題又回到上一章節的修改觀測矩陣部分，隻不過當時的觀測矩陣使用 t 時刻的狀态變量，而此時使用 α i \alpha_{i} αi​ 時刻更新後的變量值，但是整個思路沒有變。

是以在MSCKF中，僅僅需要記錄在 t 時刻的零空間，之後在 α i + 1 \alpha_{i+1} αi+1​ 時刻使用以下公式（44）來迫使優化方向與當時的零空間正交（來騙！來偷襲…）：

H ˇ ( α i + 1 ) = H ^ ( α i + 1 ) − H ^ ( α i + 1 ) u ( u T u ) − 1 u T (44) \check{\mathbf{H}}(\alpha_{i+1})=\hat{\mathbf{H}}(\alpha_{i+1})-\hat{\mathbf{H}}(\alpha_{i+1})\mathrm{u}(\mathrm{u}^{T}\mathrm{u})^{-1}\mathrm{u}^{T} \tag{44} Hˇ(αi+1​)=H^(αi+1​)−H^(αi+1​)u(uTu)−1uT(44)

總結

本文比較詳細的說明了：

1. 理想情況下零空間是可以通過狀态傳遞矩陣進行傳遞的，該結論同樣适用于FEJ的推理方法中；
2. OC-KF是如何維護系統的零空間的；
3. OC-KF的方法如何使用在MSCKF中的；

可以看到，該方法對于零空間的維護相比于FEJ來說：

• 優點是對于初始狀态的依賴性不是很強，後續的優化方向雖然和FEJ方法的零空間一樣正交，但是結合了目前的狀态值；
• 缺點筆者個人覺得就是比較理想，FEJ從理論上可以不通過修改狀态傳遞矩陣 Φ ^ ( t , t − 1 ) \hat{\Phi}(t,t-1) Φ^(t,t−1)就可以達到OC-KF修改狀态矩陣的效果，同時如果使用MSCKF2.0中的方法，将旋轉從狀态轉移矩陣中去除，那麼誤差應該會更小才對；

下一篇會稍微總結一下筆者所知道的所有關于零空間的處理方法。