VIO第3讲：基于优化的IMU与视觉信息融合之预积分残差雅可比推导

？？？

4 IMU 预积分残差的雅克比

4.1 预积分残差

$${\mathbf{e}}_B(x_i,x_j)=\left[\begin{array}{c}{\mathbf{r}}_p\\ {\mathbf{r}}_q\\ {\mathbf{r}}_v\\ {\mathbf{r}}_{ba}\\ {\mathbf{r}}_{bg}\end{array}\right]={\left[\begin{array}{c}{\mathbf{q}}_{b_{i}w}({\mathbf{p}}_{w{b}_j}-{\mathbf{p}}_{w{b}_i}-{\mathbf{v}}_i^w\Delta t+\frac{1}{2}{\mathbf{g}}^w\Delta t^2)-{\alpha }_{b_i{b}_j}\\ 2[{\mathbf{q}}_{b_j{b}_i}\otimes ({\mathbf{q}}_{b_{i}w}\otimes {\mathbf{q}}_{w{b}_j}){]}_{xyz}\\ {\mathbf{q}}_{b_{i}w}({\mathbf{v}}_j^w-{\mathbf{v}}_i^w+{\mathbf{g}}^w\Delta t)-{\mathbf{\beta }}_{b_i{b}_j}\\ {\mathbf{b}}_j^a-{\mathbf{b}}_i^a\\ {\mathbf{b}}_j^g-{\mathbf{b}}_i^g\end{array}\right]}_{15\times 1}$$

4.2 残差对两个关键帧i、j状态量的雅可比

  对于状态量的雅可比，是比较好计算的，但对于偏差较为复杂

  因为 i 时刻的 bias 相关的预积分计算是通过迭代一步一步累计递推的，可以算但是太复杂。所以对于预积分量直接在 i 时刻的 bias 附近用一阶泰勒展开来近似，而不用真的去迭代计算。
$$\begin{array}{rl}& {\mathbf{\alpha }}_{b_i{b}_j}={\mathbf{\alpha }}_{b_i{b}_j}+{\mathbf{J}}_{b_i^a}^{\alpha }\delta {\mathbf{b}}_i^a+{\mathbf{J}}_{b_i^g}^{\alpha }\delta {\mathbf{b}}_i^g\\ & {\beta }_{b_i{b}_j}={\beta }_{b_i{b}_j}+{\mathbf{J}}_{b_i^a}^{\beta }\delta {\mathbf{b}}_i^a+{\mathbf{J}}_{b_i^g}^{\beta }\delta {\mathbf{b}}_i^g\\ & {\mathbf{q}}_{b_i}{b}_j={\mathbf{q}}_{b_i{b}_j}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\mathbf{J}}_{b_i^g}^q\delta {\mathbf{b}}_i^g\end{array}\right]\end{array}$$
  下面式子表示了预积分量对i时刻的bias的导数
$$\begin{array}{r}{\mathbf{J}}_{b_i^a}^{\alpha }=\frac{\partial {\alpha }_{b_i{b}_j}}{\partial \delta {\mathbf{b}}_i^a},{\mathbf{J}}_{b_i^g}^{\alpha }=\frac{\partial {\alpha }_{b_i{b}_j}}{\partial \delta {\mathbf{b}}_i^g},{\mathbf{J}}_{b_i^a}^{\beta }=\frac{\partial {\beta }_{b_i{b}_j}}{\partial \delta {\mathbf{b}}_i^a},{\mathbf{J}}_{b_i^g}^{\beta }=\frac{\partial {\beta }_{b_i{b}_j}}{\partial \delta {\mathbf{b}}_i^g},{\mathbf{J}}_{b_i^g}^q=\frac{{\mathbf{q}}_{b_i{b}_j}}{\partial {\mathbf{b}}_i^g}\end{array}$$
  这些雅可比可以通过预积分中协方差传递公式进行一步步递推
$${\mathbf{J}}_{k+1}=\mathbf{F}{\mathbf{J}}_k$$
  由于$r_p$和$r_v$的误差形式很相近，对各状态量求导的 Jacobian 形式也很相似，所以这里只对$r_v$的推导进行详细介绍。

① 速度误差$r_v$对i时刻状态量雅可比

<1> 位移p

$$\frac{\partial {\mathbf{r}}_v}{\partial \delta {\mathbf{p}}_{{b_i{b}_i}^{\prime }}}=0$$

<2> 旋转q

  注意这里是四元数，四元素的导数等价于乘以一个微小的四元数，这些推导都可以在那个专门讲四元数的那个pdf里面找到！
$$\begin{array}{rl}\frac{\partial {\mathbf{r}}_v}{\partial \delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}}& =\frac{\partial ({\mathbf{q}}_{w{b}_i}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}\end{array}\right]{)}^{-1}({\mathbf{v}}_j^w-{\mathbf{v}}_i^w+{\mathbf{g}}^w\Delta t)}{\partial \delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}}\\ & =\frac{\partial ({\mathbf{R}}_{w{b}_i}\exp ([\delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}{]}_{\times }){)}^{-1}({\mathbf{v}}_j^w-{\mathbf{v}}_i^w+{\mathbf{g}}^w\Delta t)}{\partial \delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}}\\ & =\frac{\partial \exp ([-\delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}{]}_{\times }){\mathbf{R}}_{b_{i}w}({\mathbf{v}}_j^w-{\mathbf{v}}_i^w+{\mathbf{g}}^w\Delta t)}{\partial \delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}}\end{array}$$
  这里实际上还是转为了相应的李代数，这样子好求导！
$$\begin{array}{rl}\frac{\partial {\mathbf{r}}_v}{\partial \delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}}& =\frac{\partial (\mathbf{I}-[\delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}{]}_{\times }){\mathbf{R}}_{b_{i}w}({\mathbf{v}}_j^w-{\mathbf{v}}_i^w+{\mathbf{g}}^w\Delta t)}{\partial \delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}}\\ & =\frac{\partial -[\delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}{]}_{\times }{\mathbf{R}}_{b_{i}w}({\mathbf{v}}_j^w-{\mathbf{v}}_i^w+{\mathbf{g}}^w\Delta t)}{\partial \delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}}\\ & =\frac{\partial [{\mathbf{R}}_{b_{i}w}({\mathbf{v}}_j^w-{\mathbf{v}}_i^w+{\mathbf{g}}^w\Delta t{]}_{\times })\delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}}{\partial \delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}}\\ & =[{\mathbf{R}}_{b_{i}w}({\mathbf{v}}_j^w-{\mathbf{v}}_i^w+{\mathbf{g}}^w\Delta t){]}_{\times }\end{array}$$

<3> 速度v

$$\frac{\partial {\mathbf{r}}_v}{\partial \delta {\mathbf{v}}_i^w}=-{\mathbf{R}}_{b_{i}w}$$

<4> 加速度偏置

  注意 bias 量只和预积分 β 有关
$$\frac{\partial {\mathbf{r}}_v}{\partial \delta {\mathbf{b}}_i^a}=-\frac{\partial {\beta }_{b_i{b}_j}}{\partial \delta {\mathbf{b}}_i^a}=-{\mathbf{J}}_{b_i^a}^{\beta }$$

<5> 角速度偏置

速度中没有角速度这种分量，导数是0

② 角度误差$r_q$对i时刻状态量雅可比

<1> 位移p

$$\frac{\partial {\mathbf{r}}_q}{\partial \delta {\mathbf{p}}_{b_i}{b}_i^{\prime }}=0$$

<2> 旋转q

  角标xyz表示四元数的虚部，*表示四元数的逆
$$\begin{array}{rl}\frac{\partial {\mathbf{r}}_q}{\partial \delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}}& =\frac{\partial 2[{\mathbf{q}}_{b_j{b}_i}\otimes ({\mathbf{q}}_{b_{i}w}\otimes {\mathbf{q}}_{w{b}_j}){]}_{xyz}}{\partial \delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}}\\ & =\frac{\partial 2[{\mathbf{q}}_{b_i{b}_j}^{\ast }\otimes ({\mathbf{q}}_{w{b}_i}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}\end{array}\right]{)}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}{]}_{xyz}}{\partial \delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}}\\ & =\frac{\partial -2[({\mathbf{q}}_{b_i{b}_j}^{\ast }\otimes ({\mathbf{q}}_{w{b}_i}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}\end{array}\right]{)}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}{)}^{\ast }{]}_{xyz}}{\partial \delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}}\\ & =\frac{\partial -2[{\mathbf{q}}_{w{b}_j}^{\ast }\otimes ({\mathbf{q}}_{w{b}_i}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}\end{array}\right])\otimes {\mathbf{q}}_{b_i{b}_j}{]}_{xyz}}{\partial \delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}}\end{array}$$
  这里实际上还是转为了相应的李代数，这样子好求导！
$$\begin{array}{rl}\frac{\partial {\mathbf{r}}_q}{\partial \delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}}& =-2\left[\begin{array}{cc}0& \mathbf{I}\end{array}\right]\frac{\partial {\mathbf{q}}_{w{b}_j}^{\ast }\otimes ({\mathbf{q}}_{w{b}_i}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}\end{array}\right])\otimes {\mathbf{q}}_{b_i{b}_j}}{\partial \delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}}\\ & =-2\left[\begin{array}{cc}0& \mathbf{I}\end{array}\right]\frac{\partial [{\mathbf{q}}_{w{b}_j}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}{]}_L[{\mathbf{q}}_{b_i{b}_j}{]}_R\left[\begin{array}{c}1\\ \frac{1}{2}\delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}\end{array}\right]}{\partial \delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}}\\ & =-2\left[\begin{array}{cc}0& \mathbf{I}\end{array}\right][{\mathbf{q}}_{w{b}_j}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}{]}_L[{\mathbf{q}}_{b_i{b}_j}{]}_R\left[\begin{array}{c}0\\ \frac{1}{2}\mathbf{I}\end{array}\right]\end{array}$$
​  $[\cdot {]}_L\text{ 和 }[\cdot {]}_R$是四元数转为左/右旋转矩阵的算子，本质上是两个四元数乘法的不同表示！有的时候用L[q]或R[q]表示！

<3> 速度v

$$\frac{\partial {\mathbf{r}}_v}{\partial \delta {\mathbf{v}}_i^w}=0$$

<4> 角速度偏置

  注意 bias 量只和预积分 γ 有关
$$\begin{array}{rl}\frac{{\mathbf{r}}_q}{\partial \delta {\mathbf{b}}_i^g}& =\frac{\partial 2[({\mathbf{q}}_{b_i{b}_j}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\mathbf{J}}_{b_i^g}^q\delta {\mathbf{b}}_i^g\end{array}\right]{)}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}{]}_{xyz}}{\partial \delta {\mathbf{b}}_i^g}\\ & =\frac{\partial -2[(({\mathbf{q}}_{b_i{b}_j}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\mathbf{J}}_{b_i^g}^q\delta {\mathbf{b}}_i^g\end{array}\right]{)}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}{)}^{\ast }{]}_{xyz}}{\partial \delta {\mathbf{b}}_i^g}\\ & =\frac{\partial -2[{\mathbf{q}}_{w{b}_j}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}\otimes ({\mathbf{q}}_{b_i{b}_j}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\mathbf{J}}_{b_i^g}^q\delta {\mathbf{b}}_i^g\end{array}\right]){]}_{xyz}}{\partial \delta {\mathbf{b}}_i^g}\\ & =-2\left[\begin{array}{cc}0& \mathbf{I}\end{array}\right][{\mathbf{q}}_{w{b}_j}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}\otimes {\mathbf{q}}_{b_i{b}_j}{]}_L\left[\begin{array}{c}0\\ \frac{1}{2}{\mathbf{J}}_{b_i^g}^q\end{array}\right]\end{array}$$

<5> 加速度偏置

  角度中没有加速度这种分量，导数是0

④ 总之还有很多，可以参考崔华坤推导，后续有时间再补充