[VINS-Mono]IMU预积分残差

残差

由预积分
[ p w b j q w b j v j w b j a b j g ] = [ p w b i + v i w Δ t − 1 2 g w Δ t 2 + q w b i α b i b j q w b i q b i b j v i w − g w Δ t + q w b i β b i b j b i a b i g ] \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}} \boldsymbol{\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}} \boldsymbol{\beta}_{b_{i} b_{j}} \\ \mathbf{b}_{i}^{a} \\ \mathbf{b}_{i}^{g} \end{array}\right] ​pwbj​​qwbj​​vjw​bja​bjg​​ ​= ​pwbi​​+viw​Δt−21​gwΔt2+qwbi​​αbi​bj​​qwbi​​qbi​bj​​viw​−gwΔt+qwbi​​βbi​bj​​bia​big​​ ​

把上式左侧状态移到右侧，残差为:
[ r p r q r v r b a r b y ] = [ p w b j − p w b i − v i w Δ t + 1 2 g w Δ t 2 − q w b i α b i b j 2 [ q b i b j ∗ ⊗ ( q w b i ∗ ⊗ q w b j ) ] x y z v j w − v i w + g w Δ t − q w b i β b i b j b j a − b i a b j g − b i g ] \left[\begin{array}{c} \mathbf{r}_{p} \\ \mathbf{r}_{q} \\ \mathbf{r}_{v} \\ \mathbf{r}_{b a} \\ \mathbf{r}_{b y} \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}} \boldsymbol{\alpha}_{b_{i} b_{j}} \\ 2\left[\mathbf{q}_{b_{i} b_{j}}^{*} \otimes\left(\mathbf{q}_{w b_{i}}^{*} \otimes \mathbf{q}_{w b_{j}}\right)\right]_{x y z} \\ \mathbf{v}_{j}^{w}-\mathbf{v}_{i}^{w}+\mathbf{g}^{w} \Delta t-\mathbf{q}_{w b_{i}} \boldsymbol{\beta}_{b_{i} b_{j}} \\ \mathbf{b}_{j}^{a}-\mathbf{b}_{i}^{a} \\ \mathbf{b}_{j}^{g}-\mathbf{b}_{i}^{g} \end{array}\right] ​rp​rq​rv​rba​rby​​ ​= ​pwbj​​−pwbi​​−viw​Δt+21​gwΔt2−qwbi​​αbi​bj​​2[qbi​bj​∗​⊗(qwbi​∗​⊗qwbj​​)]xyz​vjw​−viw​+gwΔt−qwbi​​βbi​bj​​bja​−bia​bjg​−big​​ ​
其中, 关于姿态残差 ${\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$, 无法直接加减（啥意思）, 因此, 把预积分残差进行修正, 得到:
[ r p r q r v r b a r b g ] = [ q w b i ∗ ( p w b j − p w b i − v i w Δ t + 1 2 g w Δ t 2 ) − α b i b j 2 [ q b i b j ∗ ⊗ ( q w b i ∗ ⊗ q w b j ) ] x y z q w b i ∗ ( v j w − v i w + g w Δ t ) − β b i b j b j a − b i a b j g − b i g ] \left[\begin{array}{c} \mathbf{r}_{p} \\ \mathbf{r}_{q} \\ \mathbf{r}_{v} \\ \mathbf{r}_{b a} \\ \mathbf{r}_{b g} \end{array}\right]=\left[\begin{array}{c} \mathbf{q}_{w b_{i}}^{*}\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)-\boldsymbol{\alpha}_{b_{i} b_{j}} \\ 2\left[\mathbf{q}_{b_{i} b_{j}}^{*} \otimes\left(\mathbf{q}_{w b_{i}}^{*} \otimes \mathbf{q}_{w b_{j}}\right)\right]_{x y z} \\ \mathbf{q}_{w b_{i}}^{*}\left(\mathbf{v}_{j}^{w}-\mathbf{v}_{i}^{w}+\mathbf{g}^{w} \Delta t\right)-\boldsymbol{\beta}_{b_{i} b_{j}} \\ \mathbf{b}_{j}^{a}-\mathbf{b}_{i}^{a} \\ \mathbf{b}_{j}^{g}-\mathbf{b}_{i}^{g} \end{array}\right] ​rp​rq​rv​rba​rbg​​ ​= ​qwbi​∗​(pwbj​​−pwbi​​−viw​Δt+21​gwΔt2)−αbi​bj​​2[qbi​bj​∗​⊗(qwbi​∗​⊗qwbj​​)]xyz​qwbi​∗​(vjw​−viw​+gwΔt)−βbi​bj​​bja​−bia​bjg​−big​​ ​

${\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]$求偏导
  J [ 0 ] = [ ∂ r ∂ p b i w ∂ r ∂ θ b i w ] = [ − R b i w [ R b i w ( p w b j − p w b i − v i w Δ t + 1 2 g w Δ t 2 ) ] × 0 − 2 [ 0 I ] [ q w b j ∗ ⊗ q w b i ] L [ q b i b j ] R [ 0 1 2 I ] 0 [ R b i w ( v j w − v i w + g w Δ t ) ] × 0 0 0 0 ] ∈ R 15 × 7 \mathbf{J}[0]=\begin{bmatrix} \frac{\partial r}{\partial \mathrm{p}_{b_{i}}^{w}} & \frac{\partial r}{\partial \mathrm{\theta }_{b_{i}}^{w}} \end{bmatrix}=\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}} \\ \mathbf{0} & -2\left[\begin{array}{cc} \mathbf{0} & \mathbf{I} \end{array}\right]\left[\mathbf{q}_{w b_{j}}^{*} \otimes \mathbf{q}_{w b_{i}}\right]_{L}\left[\mathbf{q}_{b_{i} b_{j}}\right]_{R}\left[\begin{array}{c} \mathbf{0} \\ \frac{1}{2} \mathbf{I} \end{array}\right] \\ \mathbf{0} & {\left[\mathbf{R}_{b_{i} w}\left(\mathbf{v}_{j}^{w}-\mathbf{v}_{i}^{w}+\mathbf{g}^{w} \Delta t\right)\right]_{\times}} \\ \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} \end{array}\right] \in \mathbb{R}^{15 \times 7} J[0]=[∂pbi​w​∂r​​∂θbi​w​∂r​​]= ​−Rbi​w​0000​[Rbi​w​(pwbj​​−pwbi​​−viw​Δt+21​gwΔt2)]×​−2[0​I​][qwbj​∗​