- MSCKF的做法是根据历史相机位姿和观测来三角化计算特征点的3D坐标。
- 这又带来了一个问题:如何确保三角化的精度呢?如果三角化误差太大,那么观测模型就会不准,最终会使得VIO精度太差。
- MSCKF做法是当特征点跟踪丢失后再进行三角化,特征点跟丢表示该特征的观测不会再继续增加了,这时利用所有的历史观测三角化。所以MSCKF中观测更新的时机是特征点跟丢。
一个特征feature记作 f j f_j fj,被 M j M_j Mj个camera pose ( G C i q ˉ , G p C i ) \left({ }_G^{C_i} \bar{q},{ }^G \mathbf{p}_{C_i}\right) (GCiqˉ,GpCi)观测到
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)}=\frac{1}{{ }^{C_i} Z_j}\left[\begin{array}{l} { }^{C_i} X_j \\ { }^{C_i} Y_j \end{array}\right]+\mathbf{n}_i^{(j)}, \quad i \in \mathcal{S}_j zi(j)=CiZj1[CiXjCiYj]+ni(j),i∈Sj , 这里 S j \mathcal{S}_j Sj表示 M j M_j Mj个camera pose构成的集合。
n i ( j ) n_i^{(j)} ni(j)表示观测噪声,单位是pixel, n i ( j ) ~ N ( 0 , σ i m 2 I 2 ) n_i^{(j)} \text{\textasciitilde} N(0, \sigma_{im}^2I_2) ni(j)~N(0,σim2I2)
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}=\left[\begin{array}{c} {}^{C_i} X_j \\ { }^{C_i} Y_j \\ { }^{C_i} Z_j \end{array}\right]=\mathbf{C}\left({ }_G^{C_i} \bar{q}\right)\left({ }^G \mathbf{p}_{f_j}-{ }^G \mathbf{p}_{C_i}\right) Cipfj= CiXjCiYjCiZj =C(GCiqˉ)(Gpfj−GpCi)
定义残差
将残差线性化 r i ( j ) ≃ H X i ( j ) X ~ + H f i ( j ) G p ~ f j + n i ( j ) \mathbf{r}_i^{(j)} \simeq \mathbf{H}_{\mathbf{X}_i}^{(j)} \widetilde{\mathbf{X}}+\mathbf{H}_{f_i}^{(j) G} \widetilde{\mathbf{p}}_{f_j}+\mathbf{n}_i^{(j)} ri(j)≃HXi(j)X +Hfi(j)Gp fj+ni(j)
G p ~ f j { }^G \widetilde{\mathbf{p}}_{f_j} Gp fj是feature的3d位置估计误差,
- MSCKF做法是当特征点跟踪丢失后再进行三角化,特征点跟丢表示该特征的观测不会再继续增加了,这时利用所有的历史观测三角化。所以MSCKF中观测更新的时机是特征点跟丢。
最后计算K的时候需要求解
(
H
P
H
T
+
R
)
−
1
(HPH^T + R)^{-1}
(HPHT+R)−1这个地方使用QR分解来降低算法复杂度,参见QR分解的应用。