一步一步学习S-MSCKF观测更新 left[egin{matrix} end{matrix} ight]
Posted liuzhenbo
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了一步一步学习S-MSCKF观测更新 left[egin{matrix} end{matrix} ight]相关的知识，希望对你有一定的参考价值。
测量模型
考虑一个特征点(f_j)被位置在((^{C_i}_Gq,{^{G}p}_{C_i}))的双目相机观测到。双目的左右相机位姿可以表示为((^{C_{i,1}}_Gq,^{G}p_{C_{i,1}}))和((^{C_{i,2}}_Gq,^{G}p_{C_{i,2}}))。虽然状态向量仅仅包含左相机的位姿，但是相机左目与右目的位姿可以通过双目相机的外参产生联系。
双目的观测值（归一化坐标）可以表达为：
[z^{j}_i=
left[egin{matrix}
u_{i,1}^{j} \ v_{i,1}^{j} \ u_{i,2}^{j} \ v_{i,2}^{j}
end{matrix}
ight]=
left[egin{matrix}
frac{1}{^{C_{i,1}}Z_j}I_{2 imes 2} & 0_{2 imes 2} \
0_{2 imes 2} & frac{1}{^{C_{i,2}}Z_j}I_{2 imes 2}
end{matrix}
ight]
left[egin{matrix}
^{C_{i,1}X_j} \ ^{C_{i,1}Y_j} \ ^{C_{i,2}X_j} \ ^{C_{i,2}Y_j}
end{matrix}
ight]+n_i^j
]
左右目的观测值与相机的位姿的关系为：
[^{C_{i,1}}p_j=left[egin{matrix}{^{C_{i,1}}X_j} \ {^{C_{i,1}}Y_j} \ {^{C_{i,1}}Z_j}end{matrix}
ight]=C({^{C_{i,1}}_Gq})({^Gp_j}-{^Gp_{C_{i,1}}})
]
[^{C_{i,2}}p_j=left[egin{matrix}{^{C_{i,2}}X_j} \ {^{C_{i,2}}Y_j} \ {^{C_{i,2}}Z_j}end{matrix}
ight]=C({^{C_{i,2}}_Gq})({^Gp_j}-{^Gp_{C_{i,2}}})=
C(^{C_{i,2}}_{C_{i,1}}q)(^{C_{i,1}}p_j-{^{C_{i,1}}p_{C_{i,2}}})
]
从上面可以发现右目观测量可以用左目位姿来表示。其中(^Gp_j)是前面已经计算得到的特征点三维坐标。
对(z_i^j)在估计值(hat x)处泰勒展开得到：
[z_i^j=
left[egin{matrix}
hat u_{i,1}^{j} \ hat v_{i,1}^{j} \ hat u_{i,2}^{j} \ hat v_{i,2}^{j}
end{matrix}
ight]+
frac{partial z^j_i}{partial x_{C_{i,1}}} ilde x_{C_i}+
frac{partial z^j_i}{partial {^Gp_j}} {^G ilde p_j}+n_i^j
]
以上是关于一步一步学习S-MSCKF观测更新 left[egin{matrix} end{matrix} ight]的主要内容，如果未能解决你的问题，请参考以下文章