VINS-Mono之初始化

1. IMU估计与相机外参

在VINS可在线估计IMU外参与相机 q b c q_{bc} qbc。我们知道 q c b ? q c k 1 c k = q b k 1 b k ? q c b q^{b}_{c}\otimes q^{c_k}_{c_{k 1}}=q^{b_k}_{b_{k 1}}\otimes q^b_{c} qcb?qck 1c k ​​=qbk+1​bk​​⊗qcb​ 写成矩阵形式： [ ( q c k + 1 c k ) R − ( q b k + 1 b k ) L ] q c b = 0 \left[ (q^{c_k}_{c_{k+1}})_R -(q^{b_k}_{b_{k+1}})_L \right] q_{c}^{b} =0 [(qck+1​ck​​)R​−(qbk+1​bk​​)L​]qcb​=0 对于上式， q c k + 1 c k q^{c_k}_{c_{k+1}} qck+1​ck​​以及 [ ( q c 1 c 0 ) R − ( q b 1 b 0 ) L ( q c 2 c 1 ) R − ( q b k 2 b 1 ) L ⋮ ( q c k + 1 c k ) R − ( q b k + 1 b k ) L ] q c b = 0 \begin{bmatrix} (q^{c_0}_{c_{1}})_R -(q^{b_0}_{b_{1}})_L\\ (q^{c_1}_{c_{2}})_R -(q^{b_1}_{b_{k2}})_L\\ \vdots \\ (q^{c_k}_{c_{k+1}})_R -(q^{b_k}_{b_{k+1}})_L \end{bmatrix} q_{c}^{b} = 0 ⎣⎢⎢⎢⎢⎡​(qc1​c0​​)R​−(qb1​b0​​)L​(qc2​c1​​)R​−(qbk2​b1​​)L​⋮(qck+1​ck​​)R​−(qbk+1​bk​​)L​​⎦⎥⎥⎥⎥⎤​qcb​=0 于是，对于 q c b q^b_c qcb​的求解问题转换为求解 A x = 0 Ax=0 Ax=0方程的问题， q c b q^b_c qcb​的最优解为上式矩阵 A A A奇异值分解最后一列右奇异向量。

2. 利用旋转估计陀螺仪的零偏bias

对于预积分我们知道有如下约束： q b k + 1 c 0 = ( q c b ) − 1 ⊗ γ b k + 1 b k q^{c_0}_{b_{k+1}} = (q^b_{c})^{-1}\otimes\gamma^{b_{k}}_{b_{k+1}} qbk+1​c0​​=(qcb​)−1⊗γbk+1​bk​​ 而又有： γ b k + 1 b k = γ ^ b k + 1 b k ⊗ [ 1 1 2 J b g γ δ b g ] \gamma^{b_k}_{b_{k+1}} = \hat{\gamma}^{b_k}_{b_{k+1}}\otimes \begin{bmatrix} 1\\ \frac{1}{2}J^{\gamma}_{b_g}\delta b_g \end{bmatrix} γbk+1​bk​​=γ^​bk+1​bk​​⊗[121​Jbg​γ​δbg​​] 合并上面两式子得到: q b k + 1 c 0 = ( q c b ) − 1 ⊗ γ ^ b k + 1 b k ⊗ [ 1 1 2 J b g γ δ b g ] J b g γ δ b g = 2 ( ( γ b k + 1 b k ) − 1 ⊗ q c b ⊗ q b k + 1 c 0 ) v e c \begin{aligned} q^{c_0}_{b_{k+1}} = (q^{b}_c)^{-1} \otimes \hat{\gamma}^{b_k}_{b_{k+1}} \otimes \begin{bmatrix} 1\\ \frac{1}{2}J^{\gamma}_{b_g}\delta b_g \end{bmatrix}\\ J^{\gamma}_{b_g}\delta b_g = 2\left( (\gamma^{b_k}_{b_{k+1}})^{-1}\otimes q_{c}^b \otimes q^{c_0}_{b_{k+1}} \right)_{vec} \end{aligned} qbk+1​c0​​ 标签： thx03微量程动态扭矩传感器