[VINS-Mono]IMU预积分残差

残差

由预积分
$$\left[\begin{array}{c}{\mathbf{p}}_{w{b}_j}\\ {\mathbf{q}}_{w{b}_j}\\ {\mathbf{v}}_j^w\\ {\mathbf{b}}_j^a\\ {\mathbf{b}}_j^g\end{array}\right]=\left[\begin{array}{c}{\mathbf{p}}_{w{b}_i}+{\mathbf{v}}_i^w\Delta t-\frac{1}{2}{\mathbf{g}}^w\Delta t^2+{\mathbf{q}}_{w{b}_i}{\mathbf{\alpha }}_{b_i{b}_j}\\ {\mathbf{q}}_{w{b}_i}{\mathbf{q}}_{b_i{b}_j}\\ {\mathbf{v}}_i^w-{\mathbf{g}}^w\Delta t+{\mathbf{q}}_{w{b}_i}{\mathbf{\beta }}_{b_i{b}_j}\\ {\mathbf{b}}_i^a\\ {\mathbf{b}}_i^g\end{array}\right]$$

把上式左侧状态移到右侧，残差为:
$$\left[\begin{array}{c}{\mathbf{r}}_p\\ {\mathbf{r}}_q\\ {\mathbf{r}}_v\\ {\mathbf{r}}_{ba}\\ {\mathbf{r}}_{by}\end{array}\right]=\left[\begin{array}{c}{\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-{\mathbf{q}}_{w{b}_i}{\mathbf{\alpha }}_{b_i{b}_j}\\ 2{\left[{\mathbf{q}}_{b_i{b}_j}^{\ast }\otimes \left({\mathbf{q}}_{w{b}_i}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}\right)\right]}_{xyz}\\ {\mathbf{v}}_j^w-{\mathbf{v}}_i^w+{\mathbf{g}}^w\Delta t-{\mathbf{q}}_{w{b}_i}{\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]$$
其中, 关于姿态残差 ${\mathbf{r}}_q$部分，需要将四元数拆开来看, 根据四元数与等轴旋转矢量$\varphi$的关系:
$$\mathbf{q}=\cos \frac{\varphi }{2}+\left(u_{x}i+u_{y}j+u_{z}k\right)\sin \frac{\varphi }{2}=\left[\begin{array}{c}\cos (\varphi /2)\\ \mathbf{u}\sin (\varphi /2)\end{array}\right]$$
等效旋转矢量可以用向量 $\varphi$，并用单位向量$\mathbf{u}$表示它的朝向, $\varphi$表示它的大小, 因此有： $\varphi =\varphi \mathbf{u}$ 其中,
$${\mathbf{r}}_q=2{\left[{\mathbf{q}}_{b_i{b}_j}^{\ast }\otimes \left({\mathbf{q}}_{w{b}_i}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}\right)\right]}_{xyz}$$
$[{]}_{xyz}$就是取四元数的虚部 $\mathbf{u}\sin (\varphi /2)$, 特别的，当旋转角度$\varphi$是小量时，$\sin (\varphi /2)\approx \varphi /2$ , 对其乘个 2 , 就得到了上面的姿态残差${\mathbf{r}}_q$。
在上面的预积分误差中, 和预积分相关的量, 仍然与上一时刻的姿态有关, 如 ${\mathbf{r}}_p$, ${\mathbf{r}}_v$, 无法直接加减（啥意思）, 因此, 把预积分残差进行修正, 得到:
$$\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}}_{w{b}_i}^{\ast }\left({\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\right)-{\mathbf{\alpha }}_{b_i{b}_j}\\ 2{\left[{\mathbf{q}}_{b_i{b}_j}^{\ast }\otimes \left({\mathbf{q}}_{w{b}_i}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}\right)\right]}_{xyz}\\ {\mathbf{q}}_{w{b}_i}^{\ast }\left({\mathbf{v}}_j^w-{\mathbf{v}}_i^w+{\mathbf{g}}^w\Delta t\right)-{\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]$$

${\mathbf{r}}_p$对i时刻状态的雅克比：

• 对$\mathrm{i}$时刻 ${\mathrm{p}}_{{\mathrm{b}}_{\mathrm{i}}}^{\mathrm{w}}$的导数:
  $$\frac{\partial r_p}{\partial {\mathrm{p}}_{b_i}^w}=-{\mathrm{R}}_{\mathrm{w}}^{b_i}$$
• 对$\mathrm{i}$时刻${\mathrm{v}}_{{\mathrm{b}}_{\mathrm{i}}}^{\mathrm{w}}$的导数:
  $$\frac{\partial r_p}{\partial {\mathrm{v}}_{b_i}^w}=-R_w^{b_i}\Delta t$$
• 对$\mathrm{i}$时刻${\mathrm{q}}_{{\mathrm{b}}_{\mathrm{i}}}^{\mathrm{w}}$的导数：
  $$\begin{array}{l}\frac{\partial r_p}{\delta {\theta }_{b_i}^w}=\frac{\partial {\mathrm{R}}_{\mathrm{w}}^{{\mathrm{b}}_{\mathrm{i}}}\exp \left({\left[\delta {\theta }_{{\mathrm{b}}_{\mathrm{i}}}^{\mathrm{w}}\right]}_{\times }\right)\left({\mathrm{p}}_{{\mathrm{b}}_j}^{\mathrm{w}}-{\mathrm{p}}_{{\mathrm{b}}_i}^{\mathrm{w}}-{\mathrm{v}}_i^{\mathrm{w}}\Delta \mathrm{t}+\frac{1}{2}{\mathrm{g}}^{\mathrm{w}}\Delta {\mathrm{t}}^2\right)}{\partial \delta {\theta }_{{\mathrm{b}}_{\mathrm{i}}}^{\mathrm{w}}}\\ \approx \frac{\partial {\mathrm{R}}_{\mathrm{w}}^{{\mathrm{b}}_i}\left(\mathrm{I}+{\left[\delta {\theta }_{{\mathrm{b}}_i}^{\mathrm{w}}\right]}_{\times }\right)\left({\mathrm{p}}_{{\mathrm{b}}_j}^{\mathrm{w}}-{\mathrm{p}}_{{\mathrm{b}}_i}^{\mathrm{w}}-{\mathrm{v}}_i^{\mathrm{w}}\Delta \mathrm{t}+\frac{1}{2}{\mathrm{g}}^{\mathrm{w}}\Delta {\mathrm{t}}^2\right)}{\partial \delta {\theta }_{{\mathrm{b}}_i}^{\mathrm{w}}}\\ =\frac{\partial -{\left[\delta {\theta }_{{\mathrm{b}}_i}^{\mathrm{w}}\right]}_{\times }{\mathrm{R}}_{\mathrm{w}}^{{\mathrm{b}}_i}\left({\mathrm{p}}_{{\mathrm{b}}_j}^{\mathrm{w}}-{\mathrm{p}}_{{\mathrm{b}}_i}^{\mathrm{w}}-{\mathrm{v}}_i^{\mathrm{w}}\Delta \mathrm{t}+\frac{1}{2}{\mathrm{g}}^{\mathrm{w}}\Delta {\mathrm{t}}^2\right)}{\partial \delta {\theta }_{{\mathrm{b}}_i}^{\mathrm{w}}}\\ =\frac{\partial {\left[{\mathrm{R}}_{\mathrm{w}}^{{\mathrm{b}}_i}\left({\mathrm{p}}_{{\mathrm{b}}_j}^{\mathrm{w}}-{\mathrm{p}}_{{\mathrm{b}}_i}^{\mathrm{w}}-{\mathrm{v}}_i^{\mathrm{w}}\Delta \mathrm{t}+\frac{1}{2}{\mathrm{g}}^{\mathrm{w}}\Delta {\mathrm{t}}^2\right)\right]}_{\times }\delta {\theta }_{{\mathrm{b}}_i}^{\mathrm{w}}}{\partial \delta {\theta }_{{\mathrm{b}}_{\mathrm{k}}}^{\mathrm{w}}}\\ ={\left[{\mathrm{R}}_{\mathrm{w}}^{{\mathrm{b}}_i}\left({\mathrm{p}}_{{\mathrm{b}}_j}^{\mathrm{w}}-{\mathrm{p}}_{{\mathrm{b}}_i}^{\mathrm{w}}-{\mathrm{v}}_i^{\mathrm{w}}\Delta \mathrm{t}+\frac{1}{2}{\mathrm{g}}^{\mathrm{w}}\Delta {\mathrm{t}}^2\right)\right]}_{\mathrm{x}}\end{array}$$
• 对$\mathrm{i}$时刻 ${\mathrm{b}}_{\mathrm{a}}$和 ${\mathrm{b}}_{\mathrm{w}}$的导数:
  $$\begin{array}{l}\frac{\partial r_p}{\partial b_a}=\frac{\partial r_p}{\partial {\alpha }_{b_j}^{b_i}}\frac{\partial {\alpha }_{b_j}^{b_i}}{\partial b_a}=-J_{b_a}^{\alpha }\\ \frac{\partial r_p}{\partial b_w}=\frac{\partial r_p}{\partial {\alpha }_{b_j}^{b_i}}\frac{\partial {\alpha }_{b_j}^{b_i}}{\partial b_w}=-J_{b_w}^{\alpha }\end{array}$$

${\mathbf{r}}_p$对j时刻状态的雅克比：

$$\begin{array}{l}\frac{\partial r_p}{\partial {\mathrm{p}}_{b_j}^w}={\mathrm{R}}_{\mathrm{w}}^{b_i}\\ \frac{\partial r_p}{\partial {\mathrm{v}}_{b_j}^w}=0\\ \frac{\partial r_p}{\delta {\theta }_{b_j}^w}=0\\ \frac{\partial r_p}{\partial b_a}=0\\ \frac{\partial r_p}{\partial b_w}=0\end{array}$$

${\mathbf{r}}_q$对i时刻状态的雅克比：

$$\begin{array}{l}\frac{\partial r_q}{\partial {\mathrm{p}}_{b_i}^w}=0\\ \frac{\partial r_q}{\partial {\mathrm{v}}_{b_i}^w}=0\\ \frac{\partial r_q}{\partial {{\mathrm{b}}_{\mathrm{i}}}^a}=0\end{array}$$

• 对$\mathrm{i}$时刻 ${\theta }_{{\mathrm{b}}_{\mathrm{i}}}^{\mathrm{w}}$的导数:
  $$\begin{array}{rl}\frac{\partial r_q}{\partial {\theta }_{b_i}^w}& =\frac{\partial 2{\left[{\mathbf{q}}_{b_i{b}_j}^{\ast }\otimes \left({\mathbf{q}}_{w{b}_i}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}\right)\right]}_{xyz}}{\partial {\theta }_{b_i}^w}\\ & =\frac{\partial 2{\left[{\mathbf{q}}_{b_i{b}_j}^{\ast }\otimes {\left({\mathbf{q}}_{w{b}_i}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\theta }_{b_i}^w\end{array}\right]\right)}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}\right]}_{xyz}}{\partial {\theta }_{b_i}^w}\\ & =\frac{\partial -2{\left[{\left({\mathbf{q}}_{b_i{b}_j}^{\ast }\otimes {\left({\mathbf{q}}_{w{b}_i}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\theta }_{b_i}^w\end{array}\right]\right)}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}\right)}^{\ast }\right]}_{xyz}}{\partial {\theta }_{b_i}^w}\\ & =\frac{\partial -2{\left[{\mathbf{q}}_{w{b}_j}^{\ast }\otimes \left({\mathbf{q}}_{w{b}_i}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\theta }_{b_i}^w\end{array}\right]\right)\otimes {\mathbf{q}}_{b_i{b}_j}\right]}_{xyz}}{\partial {\theta }_{b_i}^w}\\ & =\frac{\partial -2{\left[{\mathbf{q}}_{w{b}_j}^{\ast }\otimes \left({\mathbf{q}}_{w{b}_i}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\theta }_{b_i}^w\end{array}\right]\right)\otimes {\mathbf{q}}_{b_i{b}_j}\right]}_{xyz}}{\partial {\theta }_{b_i}^w}\\ & =-2\left[\begin{array}{cc}0& I\end{array}\right]\frac{\partial {\mathbf{q}}_{w{b}_j}^{\ast }\otimes \left({\mathbf{q}}_{w{b}_i}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\theta }_{b_i}^w\end{array}\right]\right)\otimes {\mathbf{q}}_{b_i{b}_j}}{\partial {\theta }_{b_i}^w}\\ & =-2\left[\begin{array}{cc}0& 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 {\theta }_{b_i}^w\end{array}\right]\otimes {\mathbf{q}}_{b_i{b}_j}}{\partial {\theta }_{b_i}^w}\\ & =-2\left[\begin{array}{cc}0& I\end{array}\right]\frac{\partial \mathbf{L}\left({\mathbf{q}}_{w{b}_j}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}\right)\mathbf{R}\left({\mathbf{q}}_{b_i{b}_j}\right)\left[\begin{array}{c}1\\ \frac{1}{2}\delta {\theta }_{b_i}^w\end{array}\right]}{\partial {\theta }_{b_i}^w}\\ & =-2\left[\begin{array}{cc}0& I\end{array}\right]\mathbf{L}\left({\mathbf{q}}_{w{b}_j}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}\right)\mathbf{R}\left({\mathbf{q}}_{b_i{b}_j}\right)\left[\begin{array}{c}0\\ \frac{1}{2}I\end{array}\right]\\ & =-\mathbf{L}\left({\mathbf{q}}_{w{b}_j}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}\right)\mathbf{R}\left({\mathbf{q}}_{b_i{b}_j}\right)\end{array}$$

• 对$\mathrm{i}$时刻 ${\mathrm{b}}_{\mathrm{i}}^{\mathrm{g}}$的导数:
  由
  $$\begin{array}{l}{\alpha }_{b_{k+1}}^{b_k}\approx {\hat{\alpha }}_{b_{k+1}}^{b_k}+{\mathbf{J}}_{b_a}^{\alpha }\delta b_{a_k}+{\mathbf{J}}_{b_w}^{\alpha }\delta b_{w_k}\\ {\beta }_{b_{k+1}}^{b_k}\approx {\hat{\beta }}_{b_{k+1}}^{b_k}+{\mathbf{J}}_{b_a}^{\beta }\delta b_{a_k}+{\mathbf{J}}_{b_w}^{\beta }\delta b_{w_k}\\ {\gamma }_{b_{k+1}}^{b_k}\approx {\hat{\gamma }}_{b_{k+1}}^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\mathbf{J}}_{b_w}^{\gamma }\delta b_{w_k}\end{array}\right]\end{array}$$

可得
$$\begin{array}{rl}\frac{\partial r_q}{\partial b_i^g}& =\frac{\partial 2{\left[{\mathbf{q}}_{b_i{b}_j}^{\ast }\otimes \left({\mathbf{q}}_{w{b}_i}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}\right)\right]}_{xyz}}{\partial b_i^g}\\ & =\frac{\partial 2{\left[{\left({\mathbf{q}}_{b_i{b}_j}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{J}_{b_i^g}^q\delta b_i^g\end{array}\right]\right)}^{\ast }\otimes \left({\mathbf{q}}_{w{b}_i}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}\right)\right]}_{xyz}}{\partial b_i^g}\\ & =\frac{\partial -2{\left[{\left({\left({\mathbf{q}}_{b_i{b}_j}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{J}_{b_i^g}^q\delta b_i^g\end{array}\right]\right)}^{\ast }\otimes \left({\mathbf{q}}_{w{b}_i}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}\right)\right)}^{\ast }\right]}_{xyz}}{\partial b_i^g}\\ & =\frac{\partial -2{\left[{\mathbf{q}}_{w{b}_j}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}\otimes \left({\mathbf{q}}_{b_i{b}_j}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{J}_{b_i^g}^q\delta b_i^g\end{array}\right]\right)\right]}_{xyz}}{\partial b_i^g}\\ & =-2\left[\begin{array}{cc}0& I\end{array}\right]\frac{\partial {\mathbf{q}}_{w{b}_j}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}\otimes \left({\mathbf{q}}_{b_i{b}_j}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{J}_{b_i^g}^q\delta b_i^g\end{array}\right]\right)}{\partial b_i^g}\\ & =-2\left[\begin{array}{cc}0& I\end{array}\right]\frac{\partial \mathbf{L}\left({\mathbf{q}}_{w{b}_j}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}\otimes {\mathbf{q}}_{b_i{b}_j}\right)\left[\begin{array}{c}1\\ \frac{1}{2}{J}_{b_i^g}^q\delta b_i^g\end{array}\right]}{\partial b_i^g}\\ & =-2\left[\begin{array}{cc}0& I\end{array}\right]\mathbf{L}\left({\mathbf{q}}_{w{b}_j}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}\otimes {\mathbf{q}}_{b_i{b}_j}\right)\left[\begin{array}{c}0\\ \frac{1}{2}{J}_{b_i^g}^q\end{array}\right]\\ & =-\mathbf{L}\left({\mathbf{q}}_{w{b}_j}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}\otimes {\mathbf{q}}_{b_i{b}_j}\right)J_{b_i^g}^q\end{array}$$

${\mathbf{r}}_q$对j时刻状态的雅克比：

• 对$\mathrm{j}$时刻 ${\theta }_{{\mathrm{b}}_{\mathrm{j}}}^{\mathrm{w}}$的导数:
  $$\begin{array}{rl}\frac{\partial r_q}{\partial {\theta }_{b_j}^w}& =\frac{\partial 2{\left[{\mathbf{q}}_{b_i{b}_j}^{\ast }\otimes \left({\mathbf{q}}_{w{b}_i}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}\right)\right]}_{xyz}}{\partial {\theta }_{b_j}^w}\\ & =\frac{\partial 2{\left[{\mathbf{q}}_{b_i{b}_j}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}\otimes \left[\begin{array}{c}1\\ \frac{1}{2\delta {\theta }_{b_j}^w}\end{array}\right]\right]}_{xyz}}{\partial {\theta }_{b_j}^w}\\ & =2\left[\begin{array}{cc}0& I\end{array}\right]\frac{\partial {\mathbf{q}}_{b_i{b}_j}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}\otimes \left[\begin{array}{c}1\\ \frac{1}{2\delta {\theta }_{b_j}^w}\end{array}\right]}{\partial {\theta }_{b_j}^w}\\ & =2\left[\begin{array}{cc}0& I\end{array}\right]\frac{\partial \mathbf{L}\left({\mathbf{q}}_{b_i{b}_j}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}\right)\left[\begin{array}{c}1\\ \frac{1}{2\delta {\theta }_{b_j}^w}\end{array}\right]}{\partial {\theta }_{b_j}^w}\\ & =2\left[\begin{array}{cc}0& I\end{array}\right]\mathbf{L}\left({\mathbf{q}}_{b_i{b}_j}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}\right)\left[\begin{array}{c}0\\ \frac{1}{2}I\end{array}\right]\\ & =\mathbf{L}\left({\mathbf{q}}_{b_i{b}_j}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}\right)\end{array}$$
• 对$\mathrm{j}$时刻 ${\mathrm{p}}_{{\mathrm{b}}_{\mathrm{j}}}^{\mathrm{w}}$、${\mathrm{v}}_{{\mathrm{b}}_{\mathrm{j}}}^{\mathrm{w}}$、${{\mathrm{b}}_{\mathrm{j}}}^a$ 、${{\mathrm{b}}_{\mathrm{j}}}^g$的导数:
  $$\begin{array}{l}\frac{\partial r_q}{\partial {\mathrm{p}}_{b_j}^w}=0\\ \frac{\partial r_q}{\partial {\mathrm{v}}_{b_j}^w}=0\\ \frac{\partial r_q}{\partial {{\mathrm{b}}_{\mathrm{j}}}^a}=0\\ \frac{\partial r_q}{\partial {{\mathrm{b}}_{\mathrm{j}}}^g}=0\end{array}$$

${\mathbf{r}}_v$对i时刻状态的雅克比：

• 对$\mathrm{i}$时刻 ${\theta }_{{\mathrm{b}}_{\mathrm{i}}}^{\mathrm{w}}$的导数:
  $$\begin{array}{rl}\frac{\partial r_v}{\partial {\theta }_{b_i}^w}& =\frac{\partial {\mathbf{q}}_{w{b}_i}^{\ast }\left({\mathbf{v}}_j^w-{\mathbf{v}}_i^w+{\mathbf{g}}^w\Delta t\right)}{\partial {\theta }_{b_i}^w}\\ & =\frac{\partial {\left({\mathbf{q}}_{w{b}_i}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\theta }_{b_i}^w\end{array}\right]\right)}^{\ast }\left({\mathbf{v}}_j^w-{\mathbf{v}}_i^w+{\mathbf{g}}^w\Delta t\right)}{\partial {\theta }_{b_i}^w}\\ & =\frac{\partial \left(I-{\delta {\theta }_{b_i}^w}^{\wedge }\right)R_{w{b}_i}^T\left({\mathbf{v}}_j^w-{\mathbf{v}}_i^w+{\mathbf{g}}^w\Delta t\right)}{\partial {\theta }_{b_i}^w}\\ & =\frac{\partial -{\delta {\theta }_{b_i}^w}^{\wedge }{R}_{w{b}_i}^T\left({\mathbf{v}}_j^w-{\mathbf{v}}_i^w+{\mathbf{g}}^w\Delta t\right)}{\partial {\theta }_{b_i}^w}\\ & ={\left(R_{w{b}_i}^T\left({\mathbf{v}}_j^w-{\mathbf{v}}_i^w+{\mathbf{g}}^w\Delta t\right)\right)}^{\wedge }\end{array}$$

• 对i时刻${\mathrm{v}}_{{\mathrm{b}}_{\mathrm{i}}}^{\mathrm{w}}$的导数
  $$\frac{\partial r_v}{\partial v_{b_i}^w}=-R_{w{b}_i}^T$$

• 对i时刻${{\mathrm{b}}_{\mathrm{i}}}^a$、${{\mathrm{b}}_{\mathrm{i}}}^g$的导数
  $$\begin{array}{l}\frac{\partial r_v}{\partial b_a}=\frac{\partial r_v}{\partial {\beta }_{b_j}^{b_i}}\frac{\partial {\beta }_{b_j}^{b_i}}{\partial b_a}=-J_{b_a}^{\beta }\\ \frac{\partial r_v}{\partial b_g}=\frac{\partial r_v}{\partial {\beta }_{b_j}^{b_i}}\frac{\partial {\beta }_{b_j}^{b_i}}{\partial b_g}=-J_{b_g}^{\beta }\end{array}$$

• 对i时刻${\mathrm{p}}_{{\mathrm{b}}_{\mathrm{i}}}^{\mathrm{w}}$的导数
  $$\frac{\partial r_v}{\partial p_{b_i}^w}=0$$

${\mathbf{r}}_v$对j时刻状态的雅克比：

$$\begin{array}{l}\frac{\partial r_q}{\partial {\mathrm{p}}_{b_j}^w}=0\\ \frac{\partial r_q}{\partial {\mathrm{v}}_{b_j}^w}=R_{w{b}_i}^T\\ \frac{\partial r_q}{\partial {\theta }_{b_j}^w}=0\\ \frac{\partial r_q}{\partial {{\mathrm{b}}_{\mathrm{j}}}^a}=0\\ \frac{\partial r_q}{\partial {{\mathrm{b}}_{\mathrm{j}}}^g}=0\end{array}$$

总结

• 对$\mathrm{i}$ 时刻$\left[\delta p_{b_i}^w,\delta {\theta }_{b_i}^w\right]$求偏导
  $$\mathbf{J}[0]=\left[\begin{array}{cc}\frac{\partial r}{\partial {\mathrm{p}}_{b_i}^w}& \frac{\partial r}{\partial {\theta }_{b_i}^w}\end{array}\right]=\left[\begin{array}{cc}-{\mathbf{R}}_{b_{i}w}& {\left[{\mathbf{R}}_{b_{i}w}\left({\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\right)\right]}_{\times }\\ 0& -2\left[\begin{array}{cc}0& \mathbf{I}\end{array}\right]{\left[{\mathbf{q}}_{w{b}_j}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}\right]}_L{\left[{\mathbf{q}}_{b_i{b}_j}\right]}_R\left[\begin{array}{c}0\\ \frac{1}{2}\mathbf{I}\end{array}\right]\\ 0& {\left[{\mathbf{R}}_{b_{i}w}\left({\mathbf{v}}_j^w-{\mathbf{v}}_i^w+{\mathbf{g}}^w\Delta t\right)\right]}_{\times }\\ 0& 0\\ 0& 0\end{array}\right]\in {\mathbb{R}}^{15\times 7}$$
• 对$\mathrm{i}$时刻$\left[\delta v_i^w,\delta b_i^a,\delta b_i^g\right]$求偏导
  $$\mathbf{J}[1]=\left[\begin{array}{ccc}\frac{\partial r}{\partial {\mathrm{v}}_{b_i}^w}& \frac{\partial r}{\partial {{\mathrm{b}}_{\mathrm{i}}}^a}& \frac{\partial r}{\partial {{\mathrm{b}}_{\mathrm{i}}}^g}\end{array}\right]=\left[\begin{array}{ccc}-{\mathbf{R}}_{b_{i}w}\Delta t& -{\mathbf{J}}_{b_i^a}^{\alpha }& -{\mathbf{J}}_{b_i^g}^{\alpha }\\ 0& 0& -2\left[\begin{array}{ll}0& \mathbf{I}\end{array}\right]{\left[\begin{array}{c}{\mathbf{q}}_{w{b}_j}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}\otimes {\mathbf{q}}_{b_i{b}_j}\end{array}\right]}_L\left[\begin{array}{c}0\\ \frac{1}{2}{\mathbf{J}}_{b_i^g}^q\end{array}\right]\\ -{\mathbf{R}}_{b_{i}w}& -{\mathbf{J}}_{b_i^a}^{\beta }& -{\mathbf{J}}_{b_i^g}^{\beta }\\ 0& -\mathbf{I}& 0\\ 0& 0& -\mathbf{I}\end{array}\right]\in {\mathbb{R}}^{15\times 9}$$
• 对j时刻$[\delta p_{b_j}^w,\delta {\theta }_{b_j}^w]$求偏导
  $$\mathbf{J}[2]=\left[\begin{array}{cc}\frac{\partial r}{\partial {\mathrm{p}}_{b_j}^w}& \frac{\partial r}{\partial {\theta }_{b_j}^w}\end{array}\right]=\left[\begin{array}{cc}{\mathbf{R}}_{b_{i}w}& 0\\ 0& 2\left[\begin{array}{ll}0& \mathbf{I}\end{array}\right]{\left[{\mathbf{q}}_{b_i{b}_j}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}\right]}_L\left[\begin{array}{c}0\\ \frac{1}{2}\mathbf{I}\end{array}\right]\\ 0& 0\\ 0& 0\\ 0& 0\end{array}\right]\in {\mathbb{R}}^{15\times 7}$$
• 对j时刻$\left[\delta v_j^w,\delta b_j^a,\delta b_j^g\right]$求偏导
  $$\mathbf{J}[3]=\left[\begin{array}{ccc}\frac{\partial r}{\partial {\mathrm{v}}_{b_j}^w}& \frac{\partial r}{\partial {{\mathrm{b}}_{\mathrm{j}}}^a}& \frac{\partial r}{\partial {{\mathrm{b}}_{\mathrm{j}}}^g}\end{array}\right]=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ {\mathbf{R}}_{b_{i}w}& 0& 0\\ 0& \mathbf{I}& 0\\ 0& 0& \mathbf{I}\end{array}\right]\in {\mathbb{R}}^{15\times 9}$$