这篇06年的文章提出了一种新的建立几何约束的方式，将3D feature的位置信息从卡尔曼滤波中的观测模型中去除，从而减少计算复杂度。
the main limitation of SLAM is its high computational complexity; properly treating these correlations is computationally costly, and thus performing vision-based SLAM in environments with thousands of features remains a challenging problem.
该文章也采用了图像之间对极约束的方法，不同的是 MSCKF 可以在多个相机 pose 之间计算约束。
one fundamental difference is that our algorithm can express constraints between multiple camera poses, and can thus attain higher estimation accuracy, in cases where the same feature is visible in more than two images.
同样的多个相机 pose 同时优化的方法，一个很大的缺点是当单个 feature 被多个图像观测时，其中多 pose 之间的约束被忽略了导致信息缺失。
Moreover, in contrast to SLAM-type approaches, it does not require the inclusion of the 3D feature positions in the filter state vector, but still attains optimal pose estimation. As a result of these properties, the described algorithm is very efficient.
X I M U = [ G I q ˉ T b g T G v I T b a T G p I T ] T \mathbf{X}_\mathrm{IMU}=\begin{bmatrix}^I_G\bar{q}^T&\mathbf{b}_g^T&^G\mathbf{v}_I^T&\mathbf{b}_a^T&^G\mathbf{p}_I^T\end{bmatrix}^T
X I M U = [ G I q ˉ T b g T G v I T b a T G p I T ] T
该文章使用G I q ˉ ^I_G\bar{q} G I q ˉ p I \mathbf{p}_I p I G v I ^G\mathbf{v}_I G v I b g T \mathbf{b}_g^T b g T b a \mathbf{b}_a b a
根据上式写出IMU误差状态：
X ~ I M U = [ δ θ I T b ~ g T G v ~ I T b ~ a T G p ~ I T ] T \widetilde{\mathbf{X}}_{\mathrm{IMU}}=\begin{bmatrix}\delta\boldsymbol{\theta}_{I}^T&\widetilde{\mathbf{b}}_{g}^T&^G\widetilde{\mathbf{v}}_{I}^T&\widetilde{\mathbf{b}}_{a}^T&^G\widetilde{\mathbf{p}}_{I}^T\end{bmatrix}^T
X I M U = [ δ θ I T b g T G v I T b a T G p I T ] T
其中
δ q ˉ ≃ [ 1 2 δ θ T 1 ] T \delta\bar{q}~~\simeq~~\left[\frac{1}{2}\delta\theta^{T}~~~1\right]^{T}
δ q ˉ ≃ [ 2 1 δ θ T 1 ] T
根据以上可以写出EKF的状态向量，将有N N N
X ^ k = [ X ^ I M U k T G C 1 q ^ ^ T G p ^ C 1 T ⋯ G C N q ^ T G p ^ C N T ] T \hat{\mathbf{X}}_k=\begin{bmatrix}\hat{\mathbf{X}}_{\mathrm{IMU}_k}^T&^{C_1}_G\hat{\hat{q}}^T&^G\hat{\mathbf{p}}_{C_1}^T&\cdots&^{C_N}_G\hat{\mathbf{q}}^T&^G\hat{\mathbf{p}}_{C_N}^T\end{bmatrix}^T
X ^ k = [ X ^ I M U k T G C 1 q ^ ^ T G p ^ C 1 T ⋯ G C N q ^ T G p ^ C N T ] T
C i C_i C i i i i
IMU的系统可以用以下的微分方程描述：
G I q ˉ ˙ ( t ) = 1 2 Ω ( ω ( t ) ) G I q ˉ ( t ) _G^I\dot{\bar{q}}(t)=\frac{1}{2}\boldsymbol{\Omega}(\boldsymbol{\omega}(t))_G^I\bar{q}(t)
G I q ˉ ˙ ( t ) = 2 1 Ω ( ω ( t ) ) G I q ˉ ( t )
b ˙ g ( t ) = n w g ( t ) \dot{\mathbf{b}}_g(t)=\mathbf{n}_{wg}(t)
b ˙ g ( t ) = n w g ( t )
G v ˙ I ( t ) = G a ( t ) , b ˙ a ( t ) = n w a ( t ) , G p ˙ I ( t ) = G v I ( t ) ^G\dot{\mathbf{v}}_I(t)=^G\mathbf{a}(t),\dot{\mathbf{b}}_a(t)=\mathbf{n}_{wa}(t),^G\dot{\mathbf{p}}_I(t)=^G\mathbf{v}_I(t)
G v ˙ I ( t ) = G a ( t ) , b ˙ a ( t ) = n w a ( t ) , G p ˙ I ( t ) = G v I ( t )
其中Ω ( ω ) \Omega(\omega) Ω ( ω )
Ω ( ω ) = [ − ⌊ ω × ⌋ ω − ω T 0 ] , ⌊ ω × ⌋ = [ 0 − ω z ω y ω z 0 − ω x − ω y ω x 0 ] \Omega(\omega)=\begin{bmatrix}-\lfloor\omega\times\rfloor&\omega\\-\omega^T&0\end{bmatrix},\quad\lfloor\omega\times\rfloor=\begin{bmatrix}0&-\omega_z&\omega_y\\\omega_z&0&-\omega_x\\-\omega_y&\omega_x&0\end{bmatrix}
Ω ( ω ) = [ − ⌊ ω × ⌋ − ω T ω 0 ] , ⌊ ω × ⌋ = ⎣ ⎡ 0 ω z − ω y − ω z 0 ω x ω y − ω x 0 ⎦ ⎤
IMU的测量和状态的关系如下：
ω m = ω + C ( G I q ˉ ) ω G + b g + n g a m = C ( G I q ˉ ) ( G a − G g + 2 ⌊ ω G × ⌋ G v I + ⌊ ω G × ⌋ 2 G p I ) + b a + n a \begin{aligned}&\mathbf{\omega}_m=\mathbf{\omega}+\mathbf{C}(_G^I\bar{q})\mathbf{\omega}_G+\mathbf{b}_g+\mathbf{n}_g\\&\mathbf{a}_m=\mathbf{C}(_G^I\bar{q})(^G\mathbf{a}-{}^G\mathbf{g}+2\lfloor\mathbf{\omega}_G\times\rfloor^G\mathbf{v}_I+\lfloor\mathbf{\omega}_G\times\rfloor^2{}^G\mathbf{p}_I)+\mathbf{b}_a+\mathbf{n}_a\end{aligned}
ω m = ω + C ( G I q ˉ ) ω G + b g + n g a m = C ( G I q ˉ ) ( G a − G g + 2 ⌊ ω G × ⌋ G v I + ⌊ ω G × ⌋ 2 G p I ) + b a + n a
C ( ⋅ ) C(\cdot) C ( ⋅ )
将每个状态概率分布的均值代入系统方程：
G I q ˉ ^ ˙ = 1 2 Ω ( ω ^ ) G I q ^ ^ , b ^ ˙ g = 0 3 × 1 ^I_G\dot{\hat{\bar{q}}}=\frac{1}{2}\boldsymbol{\Omega}(\hat{\boldsymbol{\omega}})_G^I\hat{\hat{q}},\quad\dot{\hat{\boldsymbol{b}}}_g=\boldsymbol{0}_{3\times1}
G I q ˉ ^ ˙ = 2 1 Ω ( ω ^ ) G I q ^ ^ , b ^ ˙ g = 0 3 × 1
G v ^ ˙ I = C q ^ T a ^ − 2 ⌊ ω G × ⌋ G v ^ I − ⌊ ω G × ⌋ 2 G p ^ I + G g ^G\dot{\hat{\mathbf{v}}}_I=\mathbf{C}_{\hat{\boldsymbol{q}}}^T\hat{\mathbf{a}}-2\lfloor\omega_G\times\rfloor^G\hat{\mathbf{v}}_I-\lfloor\omega_G\times\rfloor^2{}^G\hat{\mathbf{p}}_I+{}^G\mathbf{g}
G v ^ ˙ I = C q ^ T a ^ − 2 ⌊ ω G × ⌋ G v ^ I − ⌊ ω G × ⌋ 2 G p ^ I + G g
b ^ ˙ a = 0 3 × 1 , G p ^ ˙ I = G v ^ I \dot{\hat{\mathbf{b}}}_a=\mathbf{0}_{3\times1},^G\dot{\hat{\mathbf{p}}}_I=^G\hat{\mathbf{v}}_I
b ^ ˙ a = 0 3 × 1 , G p ^ ˙ I = G v ^ I
其中 C q ^ = C ( G I q ˉ ^ ) , a ^ = a m − b ^ a , ω ^ = ω m − b ^ g − C q ^ ω G \mathbf{C}_{\hat{q}}=\mathbf{C}({}_{G}^{I}\hat{\bar{q}}),\hat{\mathbf{a}}=\mathbf{a}_{m}-\hat{\mathbf{b}}_{a},\hat{\boldsymbol{\omega}}=\omega_{m}-\hat{\mathbf{b}}_{g}-\mathbf{C}_{\hat{q}}\omega_{G} C q ^ = C ( G I q ˉ ^ ) , a ^ = a m − b ^ a , ω ^ = ω m − b ^ g − C q ^ ω G G v ^ ˙ I ^G\dot{\hat{\mathbf{v}}}_I G v ^ ˙ I − 2 ⌊ ω G × ⌋ G v ^ I -2\lfloor\omega_G\times\rfloor^G\hat{\mathbf{v}}_I − 2 ⌊ ω G × ⌋ G v ^ I − ⌊ ω G × ⌋ 2 G p ^ I -\lfloor\omega_G\times\rfloor^2{}^G\hat{\mathbf{p}}_I − ⌊ ω G × ⌋ 2 G p ^ I
之后我们可以得到IMU的误差状态方程：
x ~ ˙ I M U = F X ~ I M U + G n I M U \begin{array}{rcl}\dot{\widetilde{\mathbf{x}}}_\mathrm{IMU}&=&\mathbf{F}\widetilde{\mathbf{X}}_\mathrm{IMU}+\mathbf{G}\mathbf{n}_\mathrm{IMU}\end{array}
x ˙ I M U = F X I M U + G n I M U
n I M U = [ n g T n w g T n a T n w a T ] T \mathbf{n}_\mathrm{IMU}=\begin{bmatrix}\mathbf{n}_g^T&\mathbf{n}_{wg}^T&\mathbf{n}_a^T&\mathbf{n}_{wa}^T\end{bmatrix}^T
n I M U = [ n g T n w g T n a T n w a T ] T
F = [ − ⌊ ω ^ × ⌋ − I 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 − C q ^ T ⌊ a ^ × ⌋ 0 3 × 3 − 2 ⌊ ω G × ⌋ − C q ^ T − ⌊ ω G × ⌋ 2 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 I 3 0 3 × 3 0 3 × 3 ] \mathbf{F}=\begin{bmatrix}-\lfloor\hat{\boldsymbol{\omega}}\times\rfloor&-\mathbf{I}_3&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}\\-\mathbf{C}_{\hat{\boldsymbol{q}}}^T\lfloor\hat{\boldsymbol{a}}\times\rfloor&\mathbf{0}_{3\times3}&-2\lfloor\omega_G\times\rfloor&-\mathbf{C}_{\hat{\boldsymbol{q}}}^T&-\lfloor\omega_G\times\rfloor^2\\\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{I}_3&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}\end{bmatrix}
F = ⎣ ⎢ ⎢ ⎢ ⎢ ⎡ − ⌊ ω ^ × ⌋ 0 3 × 3 − C q ^ T ⌊ a ^ × ⌋ 0 3 × 3 0 3 × 3 − I 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 − 2 ⌊ ω G × ⌋ 0 3 × 3 I 3 0 3 × 3 0 3 × 3 − C q ^ T 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 − ⌊ ω G × ⌋ 2 0 3 × 3 0 3 × 3 ⎦ ⎥ ⎥ ⎥ ⎥ ⎤
G = [ − I 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 I 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 − C q ^ T 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 I 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 ] \mathbf{G}=\begin{bmatrix}-\mathbf{I}_3&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}&\mathbf{I}_3&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&-\mathbf{C}_{\hat{q}}^T&\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{I}_3\\\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}\end{bmatrix}
G = ⎣ ⎢ ⎢ ⎢ ⎢ ⎡ − I 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 I 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 − C q ^ T 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 I 3 0 3 × 3 ⎦ ⎥ ⎥ ⎥ ⎥ ⎤
令状态的协方差矩阵为：
P k ∣ k = [ P I I k ∣ k P I C k ∣ k P I C k ∣ k T P C C k ∣ k ] \mathbf{P}_{k|k}=\begin{bmatrix}
\mathbf{P}_{II_{k|k}} & \mathbf{P}_{IC_{k|k}} \\
\mathbf{P}_{IC_{k|k}}^T & \mathbf{P}_{CC_{k|k}}
\end{bmatrix} P k ∣ k = [ P I I k ∣ k P I C k ∣ k T P I C k ∣ k P C C k ∣ k ]
P I I k ∣ k \mathbf{P}_{II_{k|k}} P I I k ∣ k 15 × 15 15\times 15 1 5 × 1 5 P C C k ∣ k \mathbf{P}_{CC_{k|k}} P C C k ∣ k 6 N × 6 N 6N\times 6N 6 N × 6 N P I C k ∣ k \mathbf{P}_{IC_{k|k}} P I C k ∣ k
P k + 1 ∣ k = [ P I I k + 1 ∣ k Φ ( t k + T , t k ) P I C k ∣ k P I C k ∣ k T Φ ( ι k + T , ι k ) T P C C k ∣ k ] \mathbf{P}_{k+1|k}=\begin{bmatrix}\mathbf{P}_{II_{k+1|k}}&\mathbf{\Phi}(t_k+T,t_k)\mathbf{P}_{IC_{k|k}}\\\mathbf{P}_{IC_{k|k}}^T\mathbf{\Phi}(\iota_k+T,\iota_k)^T&\mathbf{P}_{CC_{k|k}}\end{bmatrix}
P k + 1 ∣ k = [ P I I k + 1 ∣ k P I C k ∣ k T Φ ( ι k + T , ι k ) T Φ ( t k + T , t k ) P I C k ∣ k P C C k ∣ k ]
论文中描述到P I I k + 1 ∣ k \mathbf{P}_{II_{k+1|k}} P I I k + 1 ∣ k ( t k , t k + T ) (t_k, t_k + T) ( t k , t k + T ) P ˙ I I = F P I I + P I I F T + G Q I M U G T \dot{\mathbf{P}}_{II}=\mathbf{F}\mathbf{P}_{II}+\mathbf{P}_{II}\mathbf{F}^T+\mathbf{G}\mathbf{Q}_{\mathrm{IMU}}\mathbf{G}^T P ˙ I I = F P I I + P I I F T + G Q I M U G T Φ ( t k + T , t k ) \Phi(t_k + T, t_k) Φ ( t k + T , t k )
Φ ˙ ( t k + τ , t k ) = F Φ ( t k + τ , t k ) , τ ∈ [ 0 , T ] \dot{\Phi}(t_k+\tau,t_k)=\mathbf{F}\mathbf{\Phi}(t_k+\tau,t_k),\quad\tau\in[0,T]
Φ ˙ ( t k + τ , t k ) = F Φ ( t k + τ , t k ) , τ ∈ [ 0 , T ]
G q ˉ ^ = I C q ˉ ⊗ G I q ˉ ^ _G\hat{\bar{q}}=_I^C\bar{q}\otimes_G^I\hat{\bar{q}}
G q ˉ ^ = I C q ˉ ⊗ G I q ˉ ^
G p ^ C = G p ^ I + C q ^ T I p C ^G\hat{\mathbf{p}}_C=^G\hat{\mathbf{p}}_I+\mathbf{C}_{\hat{q}}^{TI}\mathbf{p}_C
G p ^ C = G p ^ I + C q ^ T I p C
这是将相机坐标系中的点转换到Global系中，按照以上的方程EKF中协方差矩阵的转移如下：
P k ∣ k ← [ I 6 N + 15 J ] P k ∣ k [ I 6 N + 15 J ] T \mathbf{P}_{k|k}\leftarrow\begin{bmatrix}\mathbf{I}_{6N+15}\\\mathbf{J}\end{bmatrix}\mathbf{P}_{k|k}\begin{bmatrix}\mathbf{I}_{6N+15}\\\mathbf{J}\end{bmatrix}^T
P k ∣ k ← [ I 6 N + 1 5 J ] P k ∣ k [ I 6 N + 1 5 J ] T
J = [ C ( I C q ˉ ) 0 3 × 9 0 3 × 3 0 3 × 6 N ⌊ C q ^ T I p C × ⌋ 0 3 × 9 I 3 0 3 × 6 N ] \mathbf{J}=\begin{bmatrix}
\mathbf{C}\left(^C_I\bar{q}\right)&\mathbf{0}_{3\times9}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times6N}\\\lfloor\mathbf{C}_{\hat{q}}^{TI}\mathbf{p}_C\times\rfloor&\mathbf{0}_{3\times9}&\mathbf{I}_3&\mathbf{0}_{3\times6N}
\end{bmatrix}
J = [ C ( I C q ˉ ) ⌊ C q ^ T I p C × ⌋ 0 3 × 9 0 3 × 9 0 3 × 3 I 3 0 3 × 6 N 0 3 × 6 N ]
对于误差状态 X ~ \tilde{\mathbf{X}} X ~
r = H X ~ + n o i s e \mathbf{r=H\widetilde{X}+noise}
r = H X + n o i s e
H H H
整个观测模型的建模方式在目前来看比较直观，按照每个 tracked feature 对 camera pose 进行分组约束以此来使用单个 feature 同时约束多个相机 pose 的相对位姿，同时后续的推导中可以使约束方程中不含 feature 的 3D position 信息。
观测的大致方式是将 feature 的 3D position 投影到相机成像平面坐标并与图像中跟踪的 feature 图像坐标计算残差。
z i ( j ) = 1 C i Z j [ C i X j C i Y j ] + n i ( j ) , i ∈ S j \mathbf{z}_i^{(j)}=\dfrac{1}{^{C_i}Z_j}\begin{bmatrix}^{C_i}X_j\\^{C_i}Y_j\end{bmatrix}+\mathbf{n}_i^{(j)},\quad i\in\mathcal{S}_j
z i ( j ) = C i Z j 1 [ C i X j C i Y j ] + n i ( j ) , i ∈ S j
C i p f j = [ C i X j C i Y j C i Z j ] = C ( G C i q ˉ ) ( G p f j − G p C i ) ^{C_i}\mathbf{p}_{f_j}=\begin{bmatrix}^{C_i}X_j\\^{C_i}Y_j\\^{C_i}Z_j\end{bmatrix}=\mathbf{C}(_G^{C_i}\bar{q})(^G\mathbf{p}_{f_j}-^G\mathbf{p}_{C_i})
C i p f j = ⎣ ⎡ C i X j C i Y j C i Z j ⎦ ⎤ = C ( G C i q ˉ ) ( G p f j − G p C i )
上式代表将在全局坐标系中的 3D feature 位置 G p f j ^G\mathbf{p}_{f_j} G p f j f j f_j f j G p ^ f j ^G\hat{\mathbf{p}}_{f_j} G p ^ f j 该方法在论文的附录中 )。之后可以构建出观测的残差：
r i ( j ) = z i ( j ) − z ^ i ( j ) \mathrm{r}_i^{(j)}=\mathrm{z}_i^{(j)}-\hat{\mathrm{z}}_i^{(j)}
r i ( j ) = z i ( j ) − z ^ i ( j )
z ^ i ( j ) = 1 C i Z ^ j [ C i X ^ j C i Y ^ j ] , [ C i X ^ j C i Y ^ j C i Z ^ j ] = C ( C i q ⃗ ^ q ) ( G p ^ f j − G p ^ C i ) \hat{\mathbf{z}}_i^{(j)}=\dfrac{1}{C_i\hat{Z}_j}\begin{bmatrix}C_i\hat{X}_j\\C_i\hat{Y}_j\end{bmatrix},\begin{bmatrix}C_i\hat{X}_j\\C_i\hat{Y}_j\\C_i\hat{Z}_j\end{bmatrix}=\mathbf{C}(\begin{matrix}C_i\hat{\vec{q}}\\q\end{matrix})(^G\mathbf{\hat{p}}_{f_j}-{}^G\mathbf{\hat{p}}_{C_i})
z ^ i ( j ) = C i Z ^ j 1 [ C i X ^ j C i Y ^ j ] , ⎣ ⎡ C i X ^ j C i Y ^ j C i Z ^ j ⎦ ⎤ = C ( C i q ^ q ) ( G p ^ f j − G p ^ C i )
上式通过泰勒展开之后的一阶近似可以写成：
r i ( j ) ≃ H X i ( j ) X ~ + H f i ( j ) G p ~ f j + n i ( j ) \mathrm{r}_i^{(j)}\simeq\mathrm{H}_{\mathbf{X}_i}^{(j)}\widetilde{\mathbf{X}}+\mathrm{H}_{f_i}^{(j)G}\widetilde{\mathbf{p}}_{f_j}+\mathbf{n}_i^{(j)}
r i ( j ) ≃ H X i ( j ) X + H f i ( j ) G p f j + n i ( j )
p ~ f j \widetilde{\mathbf{p}}_{f_j} p f j f j f_j f j
H X i ( j ) = [ 0 2 × 15 0 2 × 6 … J i ( j ) ⌊ C i X ^ f j × ⌋ − J i ( j ) C ( C i G ) ⏟ Jacobian wrt pose i … ] \mathbf{H}_{\mathbf{X}_i}^{(j)}=\begin{bmatrix}\mathbf{0}_{2\times15}&\mathbf{0}_{2\times6}&\ldots&\underbrace{\mathbf{J}_i^{(j)}\lfloor^{C_i}\hat{\mathbf{X}}_{f_j}\times\rfloor -\mathbf{J}_i^{(j)}\mathbf{C}\binom{C_i}G}_{\text{Jacobian wrt pose i}}&\ldots\end{bmatrix}
H X i ( j ) = ⎣ ⎡ 0 2 × 1 5 0 2 × 6 … Jacobian wrt pose i J i ( j ) ⌊ C i X ^ f j × ⌋ − J i ( j ) C ( G C i ) … ⎦ ⎤
H f i ( j ) = J i ( j ) C ( G C i q ˉ ^ ) \mathbf{H}_{f_i}^{(j)}=\mathbf{J}_i^{(j)}\mathbf{C}(_G^{C_i}\hat{\bar{q}})
H f i ( j ) = J i ( j ) C ( G C i q ˉ ^ )
J i ( j ) = ∇ c i p ^ f j z i ( j ) = 1 C i Z ^ j [ 1 0 − C i X ^ j C i Z ^ j 0 1 − C i Y ^ j C i Z ^ j ] \mathbf{J}_i^{(j)}=\nabla_{c_{i\hat{\mathbf{p}}f_j}}\mathbf{z}_i^{(j)}=\frac{1}{C_i\hat{Z}_j}\begin{bmatrix}1&0&-\frac{C_i\hat{X}_j}{C_i\hat{Z}_j}\\0&1&-\frac{C_i\hat{Y}_j}{C_i\hat{Z}_j}\end{bmatrix}
J i ( j ) = ∇ c i p ^ f j z i ( j ) = C i Z ^ j 1 ⎣ ⎡ 1 0 0 1 − C i Z ^ j C i X ^ j − C i Z ^ j C i Y ^ j ⎦ ⎤
将所有观测 M j M_j M j
r ( j ) ≃ H X ( j ) X ~ + H f ( j ) G p ~ f j + n ( j ) \mathrm{r}^{(j)}\simeq\mathrm{H}_{\mathbf{X}}^{(j)}\widetilde{\mathbf{X}}+\mathrm{H}_{f}^{(j)G}\widetilde{\mathbf{p}}_{f_{j}}+\mathbf{n}^{(j)}
r ( j ) ≃ H X ( j ) X + H f ( j ) G p f j + n ( j )
其中 r ( j ) , H X ( j ) , H f ( j ) G , n ( j ) \mathbf{r}^{(j)}, \mathbf{H}_{\mathbf{X}}^{(j)}, \mathbf{H}_{f}^{(j)G}, \mathbf{n}^{(j)} r ( j ) , H X ( j ) , H f ( j ) G , n ( j ) r i ( j ) , H X i ( j ) , H f i ( j ) \mathbf{r}_i^{(j)}, \mathbf{H}_{\mathbf{X}_i}^{(j)}, \mathbf{H}_{f_i}^{(j)} r i ( j ) , H X i ( j ) , H f i ( j ) 不同图像中观测到的feature是独立的 ，因此噪声 n ( j ) \mathbf{n}^{(j)} n ( j ) R ( j ) = σ 2 I 2 M j \mathbf{R}^{(j)}=\sigma^2 \mathbf{I}_{2M_j} R ( j ) = σ 2 I 2 M j
一个很大的问题是计算误差 p ~ f j \widetilde{\mathbf{p}}_{f_j} p f j X \mathbf{X} X p ~ f j \widetilde{\mathbf{p}}_{f_j} p f j X ~ \tilde{\mathbf{X}} X ~ 相关的 。所以该式实际上与r = H X ~ + n o i s e \mathbf{r=H\widetilde{X}+noise} r = H X + n o i s e
为了解决这个问题 MSCKF 中将误差 r ( j ) \mathbf{r^{(j)}} r ( j ) H f ( j ) \mathbf{H}_f^{(j)} H f ( j ) N l e f t ( H f ( j ) ) = { x ∈ R n ∣ x T H f ( j ) = 0 } \mathcal{N}_{left}(\mathbf{H}_f^{(j)}) = \left\{ x \in \mathbb{R}^n | x^T \mathbf{H}_f^{(j)} = \mathbf{0} \right\} N l e f t ( H f ( j ) ) = { x ∈ R n ∣ x T H f ( j ) = 0 } n n n A \mathbf{A} A N l e f t ( H f ( j ) ) \mathcal{N}_{left}(\mathbf{H}_f^{(j)}) N l e f t ( H f ( j ) )
r o ( j ) = A T ( z ( j ) − z ^ ( j ) ) ≃ A T H X ( j ) X ~ + A T H f ( j ) G p ~ f j ⏟ 0 + A T n ( j ) = H o ( j ) X ~ ( j ) + n o ( j ) \begin{aligned}
\mathbf{r}_{o}^{(j)}& =\mathbf{A}^{T}(\mathbf{z}^{(j)}-\hat{\mathbf{z}}^{(j)})\simeq\mathbf{A}^{T}\mathbf{H}_{\mathbf{X}}^{(j)}\widetilde{\mathbf{X}} + \underbrace{\mathbf{A}^T\mathbf{H}_f^{(j)G}\widetilde{\mathbf{p}}_{f_{j}}}_{\mathbf{0}} +\mathbf{A}^{T}\mathbf{n}^{(j)} \\
&=\mathbf{H}_o^{(j)}\widetilde{\mathbf{X}}^{(j)}+\mathbf{n}_o^{(j)}
\end{aligned} r o ( j ) = A T ( z ( j ) − z ^ ( j ) ) ≃ A T H X ( j ) X + 0 A T H f ( j ) G p f j + A T n ( j ) = H o ( j ) X ( j ) + n o ( j )
文章中分析到：H f ( j ) \mathbf{H}_f^{(j)} H f ( j ) 2 M j × 3 2M_j\times3 2 M j × 3 2 M j − 3 2M_j - 3 2 M j − 3 r o ( j ) \mathbf{r}_{o}^{(j)} r o ( j ) 2 M j − 3 2M_j - 3 2 M j − 3 r o ( j ) \mathbf{r}_{o}^{(j)} r o ( j )
根据，矩阵A \mathbf{A} A O ( M j 2 ) O(M_j^2) O ( M j 2 )
同时，因为 A \mathbf{A} A n o ( j ) \mathbf{n}_o^{(j)} n o ( j )
E { n o ( j ) n o ( j ) T } = σ i m 2 A T A = σ i m 2 I 2 M j − 3 E\{\mathbf{n}_o^{(j)}\mathbf{n}_o^{(j)T}\}=\sigma^2_{im}\mathbf{A}^T\mathbf{A} = \sigma^2_{im} \mathbf{I}_{2M_j-3}
E { n o ( j ) n o ( j ) T } = σ i m 2 A T A = σ i m 2 I 2 M j − 3
文章中也提到，这并不唯一的一种残差表示形式。也可以使用对极约束来建模几何约束，实验表明使用对极约束会导致计算复杂度更高但是并没有得到更好的结果。
文章首先说明EKF的update会在以下几种情况下被触发：
当在多幅图像中被跟踪的特征不再被跟踪到时，该特征的所有测量值都使用上文中提出的方法进行处理。
当达到最大允许相机 pose N m a x N_{max} N m a x N m a x / 3 N_{max}/3 N m a x / 3 注意 : 总是保留最久的 pose 来保证几何约束中的基线足够长。
接下来详细看看 update 的数学形式，先将之前的残差项整合成矩阵形式：
r o = H X X ~ + n o \mathrm{r}_o=\mathrm{H}_\mathbf{X}\widetilde{\mathrm{X}}+\mathrm{n}_o
r o = H X X + n o
r o , n o \mathrm{r}_o, \mathrm{n}_o r o , n o L L L r o ( j ) , n o ( j ) , j = 1 … L \mathrm{r}_o^{(j)}, \mathrm{n}_o^{(j)}, j=1\ldots L r o ( j ) , n o ( j ) , j = 1 … L H X \mathrm{H}_\mathbf{X} H X H o ( j ) , j = 1 … L \mathbf{H}_o^{(j)}, j=1\ldots L H o ( j ) , j = 1 … L n o \mathbf{n}_o n o R o = σ i m 2 I d , d = ∑ j = 1 L ( 2 M j − 3 ) \mathbf{R}_{o}=\sigma_{\mathrm{im}}^{2}\mathbf{I}_{d}, d=\sum_{j=1}^{L}(2M_j - 3) R o = σ i m 2 I d , d = ∑ j = 1 L ( 2 M j − 3 )
下一步对 H X \mathrm{H}_\mathbf{X} H X
H X = [ Q 1 Q 2 ] [ T H 0 ] \mathbf{H_X}=\begin{bmatrix}\mathbf{Q}_1&\mathbf{Q}_2\end{bmatrix}\begin{bmatrix}\mathbf{T}_H\\0\end{bmatrix}
H X = [ Q 1 Q 2 ] [ T H 0 ]
根据QR分解 的说明，Q 1 , Q 2 \mathbf{Q}_1, \mathbf{Q}_2 Q 1 , Q 2 H X \mathbf{H_X} H X T H \mathbf{T}_H T H [ Q 1 , Q 2 ] T [\mathbf{Q}_1, \mathbf{Q}_2]^T [ Q 1 , Q 2 ] T
r o = [ Q 1 Q 2 ] [ T H 0 ] X ~ + n o ⇒ [ Q 1 T r o Q 2 T r o ] = [ T H 0 ] X ~ + [ Q 1 T n o Q 2 T n o ] \begin{aligned}
\mathbf{r}_{o}& =\begin{bmatrix}\mathbf{Q}_1&\mathbf{Q}_2\end{bmatrix}\begin{bmatrix}\mathbf{T}_H\\\mathbf{0}\end{bmatrix}\widetilde{\mathbf{X}}+\mathbf{n}_o\Rightarrow \\
\begin{bmatrix}\mathbf{Q}_1^T\mathbf{r}_o\\\mathbf{Q}_2^T\mathbf{r}_o\end{bmatrix}& =\begin{bmatrix}\mathbf{T}_H\\0\end{bmatrix}\widetilde{\mathbf{X}}+\begin{bmatrix}\mathbf{Q}_1^T\mathbf{n}_o\\\mathbf{Q}_2^T\mathbf{n}_o\end{bmatrix}
\end{aligned} r o [ Q 1 T r o Q 2 T r o ] = [ Q 1 Q 2 ] [ T H 0 ] X + n o ⇒ = [ T H 0 ] X + [ Q 1 T n o Q 2 T n o ]
其中的 Q 2 T r o \mathbf{Q}_2^T\mathbf{r}_o Q 2 T r o
r n = Q 1 T r o = T H X ~ + n n \mathrm{r}_n=\mathbf{Q}_1^T\mathbf{r}_o=\mathrm{T}_H\widetilde{\mathbf{X}}+\mathrm{n}_n
r n = Q 1 T r o = T H X + n n
噪声项的协方差为：R n = Q 1 T R o Q 1 = σ i m 2 I r \mathbf{R}_n=\mathbf{Q}_1^T\mathbf{R}_o\mathbf{Q}_1=\sigma^2_{im}\mathbf{I}_r R n = Q 1 T R o Q 1 = σ i m 2 I r r r r Q 1 \mathbf{Q}_1 Q 1
K = P T H T ( T H P T H T + R n ) − 1 \mathbf{K}=\mathbf{P}\mathbf{T}_H^T\left(\mathbf{T}_H\mathbf{P}\mathbf{T}_H^T+\mathbf{R}_n\right)^{-1}
K = P T H T ( T H P T H T + R n ) − 1
状态的修正增量为：
Δ X = K r n \Delta \mathbf{X} = \mathbf{K} \mathbf{r}_n
Δ X = K r n
X ˇ = X ~ + Δ X \check{\mathbf{X}} = \tilde{\mathbf{X}} + \Delta \mathbf{X}
X ˇ = X ~ + Δ X
协方差的传递写成：
P k + 1 ∣ k + 1 = ( I ξ − K T H ) P k + 1 ∣ k ( I ξ − K T H ) T + K R n K T \mathbf{P}_{k+1|k+1}=\left(\mathbf{I}_\xi-\mathbf{K}\mathbf{T}_H\right)\mathbf{P}_{k+1|k}\left(\mathbf{I}_\xi-\mathbf{K}\mathbf{T}_H\right)^T+\mathbf{K}\mathbf{R}_n\mathbf{K}^T
P k + 1 ∣ k + 1 = ( I ξ − K T H ) P k + 1 ∣ k ( I ξ − K T H ) T + K R n K T
ξ = 6 N + 15 \xi = 6N + 15 ξ = 6 N + 1 5 O ( r 2 d ) O(r^2d) O ( r 2 d ) m a x { O ( r 2 d ) , O ( ξ 3 ) } max\{O(r^2d), O(\xi^3)\} m a x { O ( r 2 d ) , O ( ξ 3 ) }
可以看到，整个MSCKF中最为核心的部分就是将残差方程中使用投影到零空间的方式消除其中各项的线性相关性，来满足EKF使用的条件，同时这种投影方式还直接简化了滤波中的状态空间，在当时看来状态空间中不包含feature的3D位置信息能极大的简化计算复杂度。
