VINS-Mono-VIO初始化 （三：陀螺仪零偏初始化）

之前估计的旋转等我外参，加速度计的零偏不在这里进行估计，会在后面滑窗优化的时候进行估计
$q_{b_k}^{c_0}$ 表示枢纽帧 $c_0$ 到第 $k$ 帧imu的变换
$q_{c_k}^{c_0}$ 表示枢纽帧 $c_0$ 到第k帧相机的变换
$q_c^b$ 表示旋转外参的估计量，上一节推导的

通过公式 $q_{b_k}^{c_0}=q_{c_k}^{c_0}\oplus (q_c^b{)}^{-1}$就能获得枢纽帧到第k帧imu的变换，这个第k是按相机的顺序来算的

然后平移公式如下： $s{P}_{b_k}^{c_0}=s{P}_{c_k}^{c_0}-R_{b_k}^{c_0}{P}_c^b$，$s$ 是尺度
通过公式 $T_{c_0{c}_k}=T_{c_0{b}_k}\cdot T_{bc}$ 展开推一下就出来了

上来两个公式等于是用已知量算出来了 $T_{c_0{b}_k}$，这个是用视觉算出来旋转乘上外参算出来的，这样就和imu坐标系有关联了，后面才乘上个 $T_{b_{k+1}{c}_0}$ ，就能获得两个imu之间的旋转$T_{b_{k+1}{b}_k}$

整体思想就是，通过相机计算出来的旋转（得通过外参转换到imu坐标系下）理论上是等于预积分出来的旋转的
求陀螺仪零偏的公式如下
$min\sum ||(q_{b_{k+1}}^{c_0}{)}^{-1}\oplus q_{b_k}^{c_0}\oplus {\gamma }_{b_{k+1}}^{b_k}|{|}^2$
里面的 $(q_{b_{k+1}}^{c_0}{)}^{-1}\oplus q_{b_k}^{c_0}=q_{b_k}^{b_{k+1}}$ ，这个本质上是和 ${\gamma }_{b_{k+1}}^{b_k}$ 的逆是一样的，他们两个相乘结果理论上是单位矩阵， ${\gamma }_{b_{k+1}}^{b_k}$是预积分量。
由于零偏的存在，所以他们的相乘不会是单位阵
更新的时候采用以下公式
${\gamma }_{b_{k+1}}^{b_k}\approx \widehat{{\gamma }_{b_{k+1}}^{b_k}}\oplus \left[\begin{array}{c}1\\ \frac{1}{2}{J}_{b_w}^{\gamma }∆b_w\end{array}\right]$
上述的意思是当零偏经过优化发生变化是，通过获取零偏变化的微量 $∆b_w$ 乘上旋转量对零偏的雅克比矩阵（这个在前面零偏建模的时候有过推导是怎么来的），然后通过这样的方式更新旋转量

然后计算公式变为如下
[ 1 0 0 0 ] = ( q b k + 1 c 0 ) − 1 ⊕ q b k c 0 ⊕ γ b k + 1 b k ^ ⊕ [ 1 1 2 J b w γ ∆ b w ] \begin{bmatrix} 1\\0\\0\\0 \end{bmatrix}=(q^{c_{0}}_{b_{k+1}})^{-1}⊕q^{c_{0}}_{b_{k}}⊕\hat{γ^{b_{k}}_{b_{k+1}}}⊕\begin{bmatrix}1\\\frac{1}{2}J^{γ}_{b_{w}}∆b_{w}\end{bmatrix} ​1000​ ​=(qbk+1​c0​​)−1⊕qbk​c0​​⊕γbk+1​bk​​^​⊕[121​Jbw​γ​∆bw​​]
这样就可以更新零偏然后知道满足结果为0为止，前面计算的对零偏的雅克比也是在这里这样用的

由于 $(q_{b_{k+1}}^{c_0}{)}^{-1}\oplus q_{b_k}^{c_0}\oplus \widehat{{\gamma }_{b_{k+1}}^{b_k}}$ 是已知的，都拿到左边去
则有
[$(\widehat{{\gamma }_{b_{k+1}}^{b_k}}{)}^{-1}\oplus (q_{b_k}^{c_0}{)}^{-1}\oplus q_{b_{k+1}}^{c_0}{]}_{vec}=\frac{1}{2}J∆b$
上面的 $vec$ 指的是取虚部，因为只有需要发生变化，实部一直都是1，整个公式的未知量只有 $∆b$ ，整个问题可以认为是 Ax=b 的模型， Ax=0的话就可以用 SVD来进行分解。
这里会有很多组数据，会把 $A,b$ 矩阵累积扩大
代码中进行小矩阵累加，就是为了避免大矩阵的运算，代码中用+=来对矩阵进行累加
只要最小化 $||Ax-b|{|}_{min}$ 即可
可以这样去进行记忆
$Ax=b$
$=>A^{T}Ax=A^{T}b$
$=>x==(A^{T}A{)}^{-1}\cdot A^T\cdot b$
这里的 $x$ 结果就是我们的 $∆b$ ，这个结果就是我们要的零偏估计

求解出零偏后会对滑窗中的预积分根据新的零偏重新进行传播，因为第一次估计的 $∆b$ 会比较大，所以才会重新传播，后面都是用雅克比矩阵进行一阶近似来传播的，算出来的是 $∆b$ ，所以代码中更新零偏也是采用+=的方式