MSCKF中的观测模型

• MSCKF的做法是根据历史相机位姿和观测来三角化计算特征点的3D坐标。
• 这又带来了一个问题：如何确保三角化的精度呢？如果三角化误差太大，那么观测模型就会不准，最终会使得VIO精度太差。
  • MSCKF做法是当特征点跟踪丢失后再进行三角化，特征点跟丢表示该特征的观测不会再继续增加了，这时利用所有的历史观测三角化。所以MSCKF中观测更新的时机是特征点跟丢。
    一个特征feature记作 $$f_j$$，被 $$M_j$$个camera pose $$\left({}_G^{C_i}\bar{q},{}^G{\mathbf{p}}_{C_i}\right)$$观测到
    $${\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)},i\in {\mathcal{S}}_j$$ ， 这里$${\mathcal{S}}_j$$表示 $$M_j$$个camera pose构成的集合。
    $$n_i^{(j)}$$表示观测噪声，单位是pixel， $$n_i^{(j)}\text{~}N(0,{\sigma }_{im}^2{I}_2)$$
    $${}^{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)$$
    定义残差
    
    将残差线性化 $${\mathbf{r}}_i^{(j)}\simeq {\mathbf{H}}_{{\mathbf{X}}_i}^{(j)}\tilde{\mathbf{X}}+{\mathbf{H}}_{f_i}^{(j)G}{\tilde{\mathbf{p}}}_{f_j}+{\mathbf{n}}_i^{(j)}$$
    $${}^G{\tilde{\mathbf{p}}}_{f_j}$$是feature的3d位置估计误差，

最后计算K的时候需要求解 $$(HP{H}^T+R{)}^{-1}$$这个地方使用QR分解来降低算法复杂度，参见QR分解的应用。