一步一步学习S-MSCKF（五）观测更新

1 单个特征对单个双目的观测模型

考虑一个特征点$f_j$被位置在${(}_G^{C_i}q,{{}^{G}p}_{C_i})$的双目相机观测到。双目的左右相机位姿可以表示为${(}_G^{C_{i,1}}q,{}^G{p}_{C_{i,1}})$和${(}_G^{C_{i,2}}q,{}^G{p}_{C_{i,2}})$。虽然状态向量仅仅包含左相机的位姿，但是相机左目与右目的位姿可以通过双目相机的外参产生联系。

双目的观测值（归一化坐标）可以表达为：
$$z_i^j=\left[\begin{array}{c}{u}_{i,1}^j\\ v_{i,1}^j\\ u_{i,2}^j\\ v_{i,2}^j\end{array}\right]=\left[\begin{array}{cc}\frac{1}{{}^{C_{i,1}}{Z}_j}{I}_{2\times 2}& 0_{2\times 2}\\ 0_{2\times 2}& \frac{1}{{}^{C_{i,2}}{Z}_j}{I}_{2\times 2}\end{array}\right]\left[\begin{array}{c}{}^{C_{i,1}}{X}_j\\ {}^{C_{i,1}}{Y}_j\\ {}^{C_{i,2}}{X}_j\\ {}^{C_{i,2}}{Y}_j\end{array}\right]+n_i^j$$
左右目的观测值与相机的位姿的关系为：
$${}^{C_{i,1}}{p}_j=\left[\begin{array}{c}{}^{C_{i,1}}{X}_j\\ {}^{C_{i,1}}{Y}_j\\ {}^{C_{i,1}}{Z}_j\end{array}\right]=C({}_G^{C_{i,1}}q)({}^G{p}_j-{}^G{p}_{C_{i,1}})$$
$${}^{C_{i,2}}{p}_j=\left[\begin{array}{c}{}^{C_{i,2}}{X}_j\\ {}^{C_{i,2}}{Y}_j\\ {}^{C_{i,2}}{Z}_j\end{array}\right]=C({}_G^{C_{i,2}}q)({}^G{p}_j-{}^G{p}_{C_{i,2}})=C{(}_{C_{i,1}}^{C_{i,2}}q){(}^{C_{i,1}}{p}_j-{}^{C_{i,1}}{p}_{C_{i,2}})$$
从上面可以发现右目观测量可以用左目位姿来表示。

对$z_i^j$在估计值$({\hat{x}}_{C_i},{}^G{\hat{p}}_j)$处泰勒展开得到：
$$z_i^j=\left[\begin{array}{c}{\hat{u}}_{i,1}^j\\ {\hat{v}}_{i,1}^j\\ {\hat{u}}_{i,2}^j\\ {\hat{v}}_{i,2}^j\end{array}\right]+\frac{\partial z_i^j}{\partial x_{C_{i,1}}}{\tilde{x}}_{C_i}+\frac{\partial z_i^j}{\partial {}^G{p}_j}{}^G{\tilde{p}}_j+n_i^j$$
所以测量残差约为：
$$r_i^j=z_i^j-{\hat{z}}_i^j=\frac{\partial z_i^j}{\partial x_{C_{i,1}}}{\tilde{x}}_{C_i}+\frac{\partial z_i^j}{\partial {}^G{p}_j}{}^G{\tilde{p}}_j+n_i^j=H_{C_i}^j{\tilde{x}}_{}$$