[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] pwbjqwbjvjwbjabjg = pwbi+viwΔt−21gwΔt2+qwbiαbibjqwbiqbibjviw−gwΔt+qwbiβbibjbiabig

把上式左侧状态移到右侧，残差为:
[ 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] rprqrvrbarby = pwbj−pwbi−viwΔt+21gwΔt2−qwbiαbibj2[qbibj∗⊗(qwbi∗⊗qwbj)]xyzvjw−viw+gwΔt−qwbiβbibjbja−biabjg−big
其中, 关于姿态残差 r q \mathbf{r}_{q} rq部分，需要将四元数拆开来看, 根据四元数与等轴旋转矢量 ϕ \phi ϕ的关系:
q = cos ⁡ ϕ 2 + ( u x i + u y j + u z k ) sin ⁡ ϕ 2 = [ cos ⁡ ( ϕ / 2 ) u sin ⁡ ( ϕ / 2 ) ] \mathbf{q}=\cos \frac{\phi}{2}+\left(u_{x} i+u_{y} j+u_{z} k\right) \sin \frac{\phi}{2}=\left[\begin{array}{c} \cos (\phi / 2) \\ \mathbf{u} \sin (\phi / 2) \end{array}\right] q=cos2ϕ+(uxi+uyj+uzk)sin2ϕ=[cos(ϕ/2)usin(ϕ/2)]
等效旋转矢量可以用向量 ϕ \phi ϕ，并用单位向量 u \mathbf{u} u表示它的朝向, ϕ \phi ϕ表示它的大小, 因此有： ϕ = ϕ u \phi=\phi \boldsymbol{u} ϕ=ϕu 其中,
r q = 2 [ q b i b j ∗ ⊗ ( q w b i ∗ ⊗ q w b j ) ] x y z \mathbf{r}_{q}=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} rq=2[qbibj∗⊗(qwbi∗⊗qwbj)]xyz
[ ] x y z []_{x y z} []xyz就是取四元数的虚部 u sin ⁡ ( ϕ / 2 ) \mathbf{u} \sin (\phi / 2) usin(ϕ/2), 特别的，当旋转角度 ϕ \phi ϕ是小量时， sin ⁡ ( ϕ / 2 ) ≈ ϕ / 2 \sin (\phi / 2) \approx \phi / 2 sin(ϕ/2)≈ϕ/2 , 对其乘个 2 , 就得到了上面的姿态残差 r q \mathbf{r}_{q} rq。
在上面的预积分误差中, 和预积分相关的量, 仍然与上一时刻的姿态有关, 如 r p \mathbf{r}_{p} rp, r v \mathbf{r}_{v} rv, 无法直接加减（啥意思）, 因此, 把预积分残差进行修正, 得到:
[ 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] rprqrvrbarbg = qwbi∗(pwbj−pwbi−viwΔt+21gwΔt2)−αbibj2[qbibj∗⊗(qwbi∗⊗qwbj)]xyzqwbi∗(vjw−viw+gwΔt)−βbibjbja−biabjg−big

r p \mathbf{r}_{p} rp对i时刻状态的雅克比：

对 i \mathrm{i} i时刻 p b i w \mathrm{p}_{\mathrm{b}_{\mathrm{i}}}^{\mathrm{w}} pbiw的导数:
∂ r p ∂ p b i w = − R w b i \frac{\partial r_p}{\partial \mathrm{p}_{b_{i}}^{w}}=-\mathrm{R}_{\mathrm{w}}^{b_{i}} ∂pbiw∂rp=−Rwbi
对 i \mathrm{i} i时刻 v b i w \mathrm{v}_{\mathrm{b_i}}^{\mathrm{w}} vbiw的导数:
∂ r p ∂ v b i w = − R w b i Δ t \frac{\partial r_p}{\partial \mathrm{v}_{b_{i}}^{w}}=-R_{w}^{b_{i}} \Delta t ∂vbiw∂rp=−RwbiΔt
对 i \mathrm{i} i时刻 q b i w \mathrm{q}_{\mathrm{b}_{\mathrm{i}}}^{\mathrm{w}} qbiw的导数：
∂ r p δ θ b i w = ∂ R w b i exp ⁡ ( [ δ θ b i w ] × ) ( p b j w − p b i w − v i w Δ t + 1 2 g w Δ t 2 ) ∂ δ θ b i w ≈ ∂ R w b i ( I + [ δ θ b i w ] × ) ( p b j w − p b i w − v i w Δ t + 1 2 g w Δ t 2 ) ∂ δ θ b i w = ∂ − [ δ θ b i w ] × R w b i ( p b j w − p b i w − v i w Δ t + 1 2 g w Δ t 2 ) ∂ δ θ b i w = ∂ [ R w b i ( p b j w − p b i w − v i w Δ t + 1 2 g w Δ t 2 ) ] × δ θ b i w ∂ δ θ b k w = [ R w b i ( p b j w − p b i w − v i w Δ t + 1 2 g w Δ t 2 ) ] x \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} δθbiw∂rp=∂δθbiw∂Rwbiexp([δθbiw]×)(pbjw−pbiw−viwΔt+21 gwΔt2)≈∂δθbiw∂Rwbi(I+[δθbiw]×)(pbjw−pbiw−viwΔt+21 gwΔt2)=∂δθbiw∂−[δθbiw]×Rwbi(pbjw−pbiw−viwΔt+21 gwΔt2)=∂δθbkw∂[Rwbi(pbjw−pbiw−viwΔt+21 gwΔt2)]×δθbiw=[Rwbi(pbjw−pbiw−viwΔt+21 gwΔt2)]x
对 i \mathrm{i} i时刻 b a \mathrm{b}_{\mathrm{a}} ba和 b w \mathrm{b}_{\mathrm{w}} bw的导数:
∂ r p ∂ b a = ∂ r p ∂ α b j b i ∂ α b j b i ∂ b a = − J b a α ∂ r p ∂ b w = ∂ r p ∂ α b j b i ∂ α b j b i ∂ b w = − J b 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} ∂ba∂rp=∂αbjbi∂rp∂ba∂αbjbi=−Jbaα∂bw∂rp=∂αbjbi∂rp∂bw∂αbjbi=−Jbwα

r p \mathbf{r}_{p} rp对j时刻状态的雅克比：

∂ r p ∂ p b j w = R w b i ∂ r p ∂ v b j w = 0 ∂ r p δ θ b j w = 0 ∂ r p ∂ b a = 0 ∂ r p ∂ b w = 0 \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} ∂pbjw∂rp=Rwbi∂vbjw∂rp=0δθbjw∂rp=0∂ba∂rp=0∂bw∂rp=0

r q \mathbf{r}_{q} rq对i时刻状态的雅克比：

∂ r q ∂ p b i w = 0 ∂ r q ∂ v b i w = 0 ∂ r q ∂ b i a = 0 \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_{i}}^{a}} = 0 \end{array} ∂pbiw∂rq=0∂vbiw∂rq=0∂bia∂rq=0

对 i \mathrm{i} i时刻 θ b i w \mathrm{\theta}_{\mathrm{b}_{\mathrm{i}}}^{\mathrm{w}} θbiw的导数:
∂ r q ∂ θ b i w = ∂ 2 [ q b i b j ∗ ⊗ ( q w b i ∗ ⊗ q w b j ) ] x y z ∂ θ b i w = ∂ 2 [ q b i b j ∗ ⊗ ( q w b i ⊗ [ 1 1 2 δ θ b i w ] ) ∗ ⊗ q w b j ] x y z ∂ θ b i w = ∂ − 2 [ ( q b i b j ∗ ⊗ ( q w b i ⊗ [ 1 1 2 δ θ b i w ] ) ∗ ⊗ q w b j ) ∗ ] x y z ∂ θ b i w = ∂ − 2 [ q w b j ∗ ⊗ ( q w b i ⊗ [ 1 1 2 δ θ b i w ] ) ⊗ q b i b j ] x y z ∂ θ b i w = ∂ − 2 [ q w b j ∗ ⊗ ( q w b i ⊗ [ 1 1 2 δ θ b i w ] ) ⊗ q b i b j ] x y z ∂ θ b i w = − 2 [ 0 I ] ∂ q w b j ∗ ⊗ ( q w b i ⊗ [ 1 1 2 δ θ b i w ] ) ⊗ q b i b j ∂ θ b i w = − 2 [ 0 I ] ∂ q w b j ∗ ⊗ q w b i ⊗ [ 1 1 2 δ θ b i w ] ⊗ q b i b j ∂ θ b i w = − 2 [ 0 I ] ∂ L ( q w b j ∗ ⊗ q w b i ) R ( q b i b j ) [ 1 1 2 δ θ b i w ] ∂ θ b i w = − 2 [ 0 I ] L ( q w b j ∗ ⊗ q w b i ) R ( q b i b j ) [ 0 1 2 I ] = − L ( q w b j ∗ ⊗ q w b i ) R ( q b i b j ) \begin{align} \frac{\partial r_q}{\partial \theta ^{w}_{b_i}} & = \frac{\partial 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} }{\partial \theta ^{w}_{b_i}} \\ & = \frac{\partial 2\left[\mathbf{q}_{b_{i} b_{j}}^{*} \otimes\left(\mathbf{q}_{w b_{i}} \otimes \begin{bmatrix} 1 \\ \frac{1}{2} \delta \theta^{w}_{b_i} \end{bmatrix}\right)^{*} \otimes \mathbf{q}_{w b_{j}}\right]_{x y z} }{\partial \theta ^{w}_{b_i}} \\ & = \frac{\partial -2\left[\left(\mathbf{q}_{b_{i} b_{j}}^{*} \otimes\left(\mathbf{q}_{w b_{i}} \otimes \begin{bmatrix} 1 \\ \frac{1}{2} \delta \theta^{w}_{b_i} \end{bmatrix}\right)^{*} \otimes \mathbf{q}_{w b_{j}}\right)^{{\color{Red} *} }\right]_{x y z} }{\partial \theta ^{w}_{b_i}} \\ & =\frac{\partial - 2\left[\mathbf{q}_{w b_{j}}^{*} \otimes \left(\mathbf{q}_{w b_{i}} \otimes \begin{bmatrix} 1 \\ \frac{1}{2} \delta \theta^{w}_{b_i} \end{bmatrix}\right) \otimes \mathbf{q}_{b_{i} b_{j}} \right]_{x y z} }{\partial \theta ^{w}_{b_i}} \\ & =\frac{\partial - 2\left[\mathbf{q}_{w b_{j}}^{*} \otimes \left(\mathbf{q}_{w b_{i}} \otimes \begin{bmatrix} 1 \\ \frac{1}{2} \delta \theta^{w}_{b_i} \end{bmatrix}\right) \otimes \mathbf{q}_{b_{i} b_{j}} \right]_{x y z} }{\partial \theta ^{w}_{b_i}} \\ & = - 2\begin{bmatrix} 0 & I \end{bmatrix}\frac{\partial \mathbf{q}_{w b_{j}}^{*} \otimes \left(\mathbf{q}_{w b_{i}} \otimes \begin{bmatrix} 1 \\ \frac{1}{2} \delta \theta^{w}_{b_i} \end{bmatrix}\right) \otimes \mathbf{q}_{b_{i} b_{j}} }{\partial \theta ^{w}_{b_i}} \\ &= - 2\begin{bmatrix} 0 & I \end{bmatrix}\frac{\partial \mathbf{q}_{w b_{j}}^{*} \otimes\mathbf{q}_{w b_{i}} \otimes \begin{bmatrix} 1 \\ \frac{1}{2} \delta \theta^{w}_{b_i} \end{bmatrix} \otimes \mathbf{q}_{b_{i} b_{j}} }{\partial \theta ^{w}_{b_i}} \\ & = - 2\begin{bmatrix} 0 & I \end{bmatrix}\frac{\partial \mathbf{L}\left( \mathbf{q}_{w b_{j}}^{*} \otimes\mathbf{q}_{w b_{i}} \right )\mathbf{R}\left( \mathbf{q}_{b_{i} b_{j}}\right) \begin{bmatrix} 1 \\ \frac{1}{2} \delta \theta^{w}_{b_i} \end{bmatrix} }{\partial \theta ^{w}_{b_i}} \\ & = - 2\begin{bmatrix} 0 & I \end{bmatrix}\mathbf{L}\left( \mathbf{q}_{w b_{j}}^{*} \otimes\mathbf{q}_{w b_{i}} \right )\mathbf{R}\left( \mathbf{q}_{b_{i} b_{j}}\right) \begin{bmatrix} 0 \\ \frac{1}{2}I \end{bmatrix} \\ &= -\mathbf{L}\left( \mathbf{q}_{w b_{j}}^{*} \otimes\mathbf{q}_{w b_{i}} \right )\mathbf{R}\left( \mathbf{q}_{b_{i} b_{j}}\right) \end{align} ∂θbiw∂rq=∂θbiw∂2[qbibj∗⊗(qwbi∗⊗qwbj)]xyz=∂θbiw∂2[qbibj∗⊗(qwbi⊗[121δθbiw])∗⊗qwbj]xyz=∂θbiw∂−2[(qbibj∗⊗(qwbi⊗[121δθbiw])∗⊗qwbj)∗]xyz=∂θbiw∂−2[qwbj∗⊗(qwbi⊗[121δθbiw])⊗qbibj]xyz=∂θbiw∂−2[qwbj∗⊗(qwbi⊗[121δθbiw])⊗qbibj]xyz=−2[0I]∂θbiw∂qwbj∗⊗(qwbi⊗[121δθbiw])⊗qbibj=−2[0I]∂θbiw∂qwbj∗⊗qwbi⊗[121δθbiw]⊗qbibj=−2[0I]∂θbiw∂L(qwbj∗⊗qwbi)R(qbibj)[121δθbiw]=−2[0I]L(qwbj∗⊗qwbi)R(qbibj)[021I]=−L(qwbj∗⊗qwbi)R(qbibj)

对 i \mathrm{i} i时刻 b i g \mathrm{b}_\mathrm{i}^\mathrm{g} big的导数:
由
α b k + 1 b k ≈ α ^ b k + 1 b k + J b a α δ b a k + J b w α δ b w k β b k + 1 b k ≈ β ^ b k + 1 b k + J b a β δ b a k + J b w β δ b w k γ b k + 1 b k ≈ γ ^ b k + 1 b k ⊗ [ 1 1 2 J b w γ δ b w k ] \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} αbk+1bk≈α^bk+1bk+Jbaαδbak+Jbwαδbwkβbk+1bk≈β^bk+1bk+Jbaβδbak+Jbwβδbwkγbk+1bk≈γ^bk+1bk⊗[121Jbwγδbwk]

可得
∂ r q ∂ b i g = ∂ 2 [ q b i b j ∗ ⊗ ( q w b i ∗ ⊗ q w b j ) ] x y z ∂ b i g = ∂ 2 [ ( q b i b j ⊗ [ 1 1 2 J b i g q δ b i g ] ) ∗ ⊗ ( q w b i ∗ ⊗ q w b j ) ] x y z ∂ b i g = ∂ − 2 [ ( ( q b i b j ⊗ [ 1 1 2 J b i g q δ b i g ] ) ∗ ⊗ ( q w b i ∗ ⊗ q w b j ) ) ∗ ] x y z ∂ b i g = ∂ − 2 [ q w b j ∗ ⊗ q w b i ⊗ ( q b i b j ⊗ [ 1 1 2 J b i g q δ b i g ] ) ] x y z ∂ b i g = − 2 [ 0 I ] ∂ q w b j ∗ ⊗ q w b i ⊗ ( q b i b j ⊗ [ 1 1 2 J b i g q δ b i g ] ) ∂ b i g = − 2 [ 0 I ] ∂ L ( q w b j ∗ ⊗ q w b i ⊗ q b i b j ) [ 1 1 2 J b i g q δ b i g ] ∂ b i g = − 2 [ 0 I ] L ( q w b j ∗ ⊗ q w b i ⊗ q b i b j ) [ 0 1 2 J b i g q ] = − L ( q w b j ∗ ⊗ q w b i ⊗ q b i b j ) J b i g q \begin{align} \frac{\partial r_q}{\partial b ^{g}_{i}} & = \frac{\partial 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} }{\partial b ^{g}_{i}} \\ & = \frac{\partial 2\left[\left (\mathbf{q}_{b_{i} b_{j}} \otimes {\color{Red} \begin{bmatrix} 1 \\ \frac{1}{2}J^q_ {b_i^g}\delta b_{i}^g \end{bmatrix}}\right)^{*} \otimes\left(\mathbf{q}_{w b_{i}}^{*} \otimes \mathbf{q}_{w b_{j}}\right)\right]_{x y z} }{\partial b ^{g}_{i}} \\ & = \frac{\partial -2\left[\left (\left (\mathbf{q}_{b_{i} b_{j}} \otimes \begin{bmatrix} 1 \\ \frac{1}{2}J^q_ {b_i^g}\delta b_{i}^g \end{bmatrix}\right)^{*} \otimes\left(\mathbf{q}_{w b_{i}}^{*} \otimes \mathbf{q}_{w b_{j}}\right)\right)^{\color{Red} *} \right]_{x y z} }{\partial b ^{g}_{i}} \\ & = \frac{\partial -2\left[\mathbf{q}_{w b_{j}}^{*} \otimes \mathbf{q}_{w b_{i}}\otimes \left (\mathbf{q}_{b_{i} b_{j}} \otimes \begin{bmatrix} 1 \\ \frac{1}{2}J^q_ {b_i^g}\delta b_{i}^g \end{bmatrix}\right)\right]_{x y z} }{\partial b ^{g}_{i}} \\ & = -2\begin{bmatrix} 0 & I \end{bmatrix}\frac{\partial \mathbf{q}_{w b_{j}}^{*} \otimes \mathbf{q}_{w b_{i}}\otimes \left (\mathbf{q}_{b_{i} b_{j}} \otimes \begin{bmatrix} 1 \\ \frac{1}{2}J^q_ {b_i^g}\delta b_{i}^g \end{bmatrix}\right)}{\partial b ^{g}_{i}} \\ & = -2\begin{bmatrix} 0 & I \end{bmatrix}\frac{\partial \mathbf{L}\left ( \mathbf{q}_{w b_{j}}^{*} \otimes \mathbf{q}_{w b_{i}}\otimes \mathbf{q}_{b_{i} b_{j}} \right ) \begin{bmatrix} 1 \\ \frac{1}{2}J^q_ {b_i^g}\delta b_{i}^g \end{bmatrix}}{\partial b ^{g}_{i}} \\ & = -2\begin{bmatrix} 0 & I \end{bmatrix}\mathbf{L}\left ( \mathbf{q}_{w b_{j}}^{*} \otimes \mathbf{q}_{w b_{i}}\otimes \mathbf{q}_{b_{i} b_{j}} \right ) \begin{bmatrix} 0\\ \frac{1}{2}J^q_ {b_i^g} \end{bmatrix} \\ & = -\mathbf{L}\left ( \mathbf{q}_{w b_{j}}^{*} \otimes \mathbf{q}_{w b_{i}}\otimes \mathbf{q}_{b_{i} b_{j}} \right ) J^q_ {b_i^g} \end{align} ∂big∂rq=∂big∂2[qbibj∗⊗(qwbi∗⊗qwbj)]xyz=∂big∂2[(qbibj⊗[121Jbigqδbig])∗⊗(qwbi∗⊗qwbj)]xyz=∂big∂−2[((qbibj⊗[121Jbigqδbig])∗⊗(qwbi∗⊗qwbj))∗]xyz=∂big∂−2[qwbj∗⊗qwbi⊗(qbibj⊗[121Jbigqδbig])]xyz=−2[0I]∂big∂qwbj∗⊗qwbi⊗(qbibj⊗[121Jbigqδbig])=−2[0I]∂big∂L(qwbj∗⊗qwbi⊗qbibj)[121Jbigqδbig]=−2[0I]L(qwbj∗⊗qwbi⊗qbibj)[021Jbigq]=−L(qwbj∗⊗qwbi⊗qbibj)Jbigq

r q \mathbf{r}_{q} rq对j时刻状态的雅克比：

对 j \mathrm{j} j时刻 θ b j w \mathrm{\theta}_{\mathrm{b}_{\mathrm{j}}}^{\mathrm{w}} θbjw的导数:
∂ r q ∂ θ b j w = ∂ 2 [ q b i b j ∗ ⊗ ( q w b i ∗ ⊗ q w b j ) ] x y z ∂ θ b j w = ∂ 2 [ q b i b j ∗ ⊗ q w b i ∗ ⊗ q w b j ⊗ [ 1 1 2 δ θ b j w ] ] x y z ∂ θ b j w = 2 [ 0 I ] ∂ q b i b j ∗ ⊗ q w b i ∗ ⊗ q w b j ⊗ [ 1 1 2 δ θ b j w ] ∂ θ b j w = 2 [ 0 I ] ∂ L ( q b i b j ∗ ⊗ q w b i ∗ ⊗ q w b j ) [ 1 1 2 δ θ b j w ] ∂ θ b j w = 2 [ 0 I ] L ( q b i b j ∗ ⊗ q w b i ∗ ⊗ q w b j ) [ 0 1 2 I ] = L ( q b i b j ∗ ⊗ q w b i ∗ ⊗ q w b j ) \begin{align} \frac{\partial r_q}{\partial \theta ^{w}_{b_j}} & = \frac{\partial 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} }{\partial \theta ^{w}_{b_j}} \\ & = \frac{\partial 2\left[\mathbf{q}_{b_{i} b_{j}}^{*} \otimes\mathbf{q}_{w b_{i}} ^{*} \otimes \mathbf{q}_{w b_{j}}\otimes \begin{bmatrix} 1 \\ \frac{1}{2\delta \theta^{w}_{b_j}} \end{bmatrix}\right]_{x y z} }{\partial \theta ^{w}_{b_j}} \\ &= 2\begin{bmatrix} 0& I \end{bmatrix}\frac{\partial\mathbf{q}_{b_{i} b_{j}}^{*} \otimes\mathbf{q}_{w b_{i}} ^{*} \otimes \mathbf{q}_{w b_{j}}\otimes \begin{bmatrix} 1 \\ \frac{1}{2\delta \theta^{w}_{b_j}} \end{bmatrix} }{\partial \theta ^{w}_{b_j}} \\ &= 2\begin{bmatrix} 0& I \end{bmatrix}\frac{\partial\mathbf{L}\left ( \mathbf{q}_{b_{i} b_{j}}^{*} \otimes\mathbf{q}_{w b_{i}} ^{*} \otimes \mathbf{q}_{w b_{j}}\right) \begin{bmatrix} 1 \\ \frac{1}{2\delta \theta^{w}_{b_j}} \end{bmatrix} }{\partial \theta ^{w}_{b_j}} \\ &= 2\begin{bmatrix} 0& I \end{bmatrix}\mathbf{L}\left ( \mathbf{q}_{b_{i} b_{j}}^{*} \otimes\mathbf{q}_{w b_{i}} ^{*} \otimes \mathbf{q}_{w b_{j}}\right) \begin{bmatrix} 0 \\ \frac{1}{2} I \end{bmatrix} \\ &=\mathbf{L}\left ( \mathbf{q}_{b_{i} b_{j}}^{*} \otimes\mathbf{q}_{w b_{i}} ^{*} \otimes \mathbf{q}_{w b_{j}}\right) \\ \end{align} ∂θbjw∂rq=∂θbjw∂2[qbibj∗⊗(qwbi∗⊗qwbj)]xyz=∂θbjw∂2[qbibj∗⊗qwbi∗⊗qwbj⊗[12δθbjw1]]xyz=2[0I]∂θbjw∂qbibj∗⊗qwbi∗⊗qwbj⊗[12δθbjw1]=2[0I]∂θbjw∂L(qbibj∗⊗qwbi∗⊗qwbj)[12δθbjw1]=2[0I]L(qbibj∗⊗qwbi∗⊗qwbj)[021I]=L(qbibj∗⊗qwbi∗⊗qwbj)
对 j \mathrm{j} j时刻 p b j w \mathrm{p}_{\mathrm{b}_{\mathrm{j}}}^{\mathrm{w}} pbjw、 v b j w \mathrm{v}_{\mathrm{b}_{\mathrm{j}}}^{\mathrm{w}} vbjw、 b j a \mathrm{b_{j}}^{a} bja 、 b j g \mathrm{b_{j}}^{g} bjg的导数:
∂ r q ∂ p b j w = 0 ∂ r q ∂ v b j w = 0 ∂ r q ∂ b j a = 0 ∂ r q ∂ b j g = 0 \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_{j}}^{a}} = 0 \\ \frac{\partial r_q}{\partial \mathrm{b_{j}}^{g}} = 0 \end{array} ∂pbjw∂rq=0∂vbjw∂rq=0∂bja∂rq=0∂bjg∂rq=0

r v \mathbf{r}_{v} rv对i时刻状态的雅克比：

对 i \mathrm{i} i时刻 θ b i w \mathrm{\theta}_{\mathrm{b}_{\mathrm{i}}}^{\mathrm{w}} θbiw的导数:
∂ r v ∂ θ b i w = ∂ q w b i ∗ ( v j w − v i w + g w Δ t ) ∂ θ b i w = ∂ ( q w b i ⊗ [ 1 1 2 δ θ b i w ] ) ∗ ( v j w − v i w + g w Δ t ) ∂ θ b i w = ∂ ( I − δ θ b i w ∧ ) R w b i T ( v j w − v i w + g w Δ t ) ∂ θ b i w = ∂ − δ θ b i w ∧ R w b i T ( v j w − v i w + g w Δ t ) ∂ θ b i w = ( R w b i T ( v j w − v i w + g w Δ t ) ) ∧ \begin{align} \frac{\partial r_v}{\partial \theta _{b_i}^w} & = \frac{\partial \mathbf{q}_{w b_{i}}^{*}\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 \begin{bmatrix} 1\\ \frac{1}{2}\delta \theta _{b_i}^w \end{bmatrix}\right)^{*}\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^w_{b_{i}}}^\wedge \right )R_{wb_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^w_{b_{i}}}^\wedge R_{wb_i}^T \left(\mathbf{v}_{j}^{w}-\mathbf{v}_{i}^{w}+\mathbf{g}^{w} \Delta t\right)}{\partial \theta _{b_i}^w} \\ &= \left (R_{wb_i}^T \left(\mathbf{v}_{j}^{w}-\mathbf{v}_{i}^{w}+\mathbf{g}^{w} \Delta t\right)\right )^{\wedge } \end{align} ∂θbiw∂rv=∂θbiw∂qwbi∗(vjw−viw+gwΔt)=∂θbiw∂(qwbi⊗[121δθbiw])∗(vjw−viw+gwΔt)=∂θbiw∂(I−δθbiw∧)RwbiT(vjw−viw+gwΔt)=∂θbiw∂−δθbiw∧RwbiT(vjw−viw+gwΔt)=(RwbiT(vjw−viw+gwΔt))∧
对i时刻 v b i w \mathrm{v}_{\mathrm{b}_{\mathrm{i}}}^{\mathrm{w}} vbiw的导数
∂ r v ∂ v b i w = − R w b i T \frac{\partial r_v}{\partial v _{b_i}^w}=-R^{T}_{wb_i} ∂vbiw∂rv=−RwbiT
对i时刻 b i a \mathrm{b_{i}}^{a} bia、 b i g \mathrm{b_{i}}^{g} big的导数
∂ r v ∂ b a = ∂ r v ∂ β b j b i ∂ β b j b i ∂ b a = − J b a β ∂ r v ∂ b g = ∂ r v ∂ β b j b i ∂ β b j b i ∂ b g = − J b 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} ∂ba∂rv=∂βbjbi∂rv∂ba∂βbjbi=−Jbaβ∂bg∂rv=∂βbjbi∂rv∂bg∂βbjbi=−Jbgβ
对i时刻 p b i w \mathrm{p}_{\mathrm{b}_{\mathrm{i}}}^{\mathrm{w}} pbiw的导数
∂ r v ∂ p b i w = 0 \frac{\partial r_v}{\partial p_{b_i}^w}=0 ∂pbiw∂rv=0

r v \mathbf{r}_{v} rv对j时刻状态的雅克比：

∂ r q ∂ p b j w = 0 ∂ r q ∂ v b j w = R w b i T ∂ r q ∂ θ b j w = 0 ∂ r q ∂ b j a = 0 ∂ r q ∂ b j g = 0 \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_{wb_i}^T\\ \frac{\partial r_q}{\partial \mathrm{\theta}_{b_{j}}^{w}} = 0\\ \frac{\partial r_q}{\partial \mathrm{b_{j}}^{a}} = 0 \\ \frac{\partial r_q}{\partial \mathrm{b_{j}}^{g}} = 0 \end{array} ∂pbjw∂rq=0∂vbjw∂rq=RwbiT∂θbjw∂rq=0∂bja∂rq=0∂bjg∂rq=0

总结

对 i \mathrm{i} i 时刻 [ δ p b i w , δ θ b i w ] \left[\delta p_{b_{i}}^{w}, \delta \theta_{b_{i}}^{w}\right] [δpbiw,δθbiw]求偏导
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]=[∂pbiw∂r∂θbiw∂r]= −Rbiw0000[Rbiw(pwbj−pwbi−viwΔt+21gwΔt2)]×−2[0I][qwbj∗⊗qwbi]L[qbibj]R[021I][Rbiw(vjw−viw+gwΔt)]×00 ∈R15×7
对 i \mathrm{i} i时刻 [ δ v i w , δ b i a , δ b i g ] \left[\delta v_{i}^{w}, \delta b_{i}^{a}, \delta b_{i}^{g}\right] [δviw,δbia,δbig]求偏导
J [ 1 ] = [ ∂ r ∂ v b i w ∂ r ∂ b i a ∂ r ∂ b i g ] = [ − R b i w Δ t − J b i a α − J b i g α 0 0 − 2 [ 0 I ] [ q w b j ∗ ⊗ q w b i ⊗ q b i b j ] L [ 0 1 2 J b i g q ] − R b i w − J b i a β − J b i g β 0 − I 0 0 0 − I ] ∈ R 15 × 9 \mathbf{J}[1]=\begin{bmatrix} \frac{\partial r}{\partial \mathrm{v}_{b_{i}}^{w}} & \frac{\partial r}{\partial \mathrm{b_{i}}^{a}}&\frac{\partial r}{\partial \mathrm{b_{i}}^{g}} \end{bmatrix}=\left[\begin{array}{ccc} -\mathbf{R}_{b_{i} w} \Delta t & -\mathbf{J}_{b_{i}^{a}}^{\alpha} & -\mathbf{J}_{b_{i}^{g}}^{\alpha} \\ \mathbf{0} & \mathbf{0} & -2\left[\begin{array}{ll} \mathbf{0} & \mathbf{I} \end{array}\right]\left[\begin{array}{c} \mathbf{q}_{w b_{j}}^{*} \otimes \mathbf{q}_{w b_{i}} \otimes \mathbf{q}_{b_{i} b_{j}} \end{array}\right]_{L}\left[\begin{array}{c} \mathbf{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} \\ \mathbf{0} & -\mathbf{I} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & -\mathbf{I} \end{array}\right] \in \mathbb{R}^{15 \times 9} J[1]=[∂vbiw∂r∂bia∂r∂big∂r]= −RbiwΔt0−Rbiw00−Jbiaα0−Jbiaβ−I0−Jbigα−2[0I][qwbj∗⊗qwbi⊗qbibj]L[021Jbigq]−Jbigβ0−I ∈R15×9
对j时刻 [ δ p b j w , δ θ b j w ] \left[\delta p_{b_{j}}^{w}, \delta \theta_{b_{j}}^{w}\right. ] [δpbjw,δθbjw]求偏导
J [ 2 ] = [ ∂ r ∂ p b j w ∂ r ∂ θ b j w ] = [ R b i w 0 0 2 [ 0 I ] [ q b i b j ∗ ⊗ q w b i ∗ ⊗ q w b j ] L [ 0 1 2 I ] 0 0 0 0 0 0 ] ∈ R 15 × 7 \mathbf{J}[2]=\begin{bmatrix} \frac{\partial r}{\partial \mathrm{p}_{b_{j}}^{w}} & \frac{\partial r}{\partial \mathrm{\theta }_{b_{j}}^{w}} \end{bmatrix}=\left[\begin{array}{cc} \mathbf{R}_{b_{i} w} & \mathbf{0} \\ \mathbf{0} & 2\left[\begin{array}{ll} \mathbf{0} & \mathbf{I} \end{array}\right]\left[\mathbf{q}_{b_{i} b_{j}}^{*} \otimes \mathbf{q}_{w b_{i}}^{*} \otimes \mathbf{q}_{w b_{j}}\right]_{L}\left[\begin{array}{c} \mathbf{0} \\ \frac{1}{2} \mathbf{I} \end{array}\right] \\ \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} \end{array}\right] \in \mathbb{R}^{15 \times 7} J[2]=[∂pbjw∂r∂θbjw∂r]= Rbiw000002[0I][qbibj∗⊗qwbi∗⊗qwbj]L[021I]000 ∈R15×7
对j时刻 [ δ v j w , δ b j a , δ b j g ] \left[\delta v_{j}^{w}, \delta b_{j}^{a}, \delta b_{j}^{g}\right] [δvjw,δbja,δbjg]求偏导
J [ 3 ] = [ ∂ r ∂ v b j w ∂ r ∂ b j a ∂ r ∂ b j g ] = [ 0 0 0 0 0 0 R b i w 0 0 0 I 0 0 0 I ] ∈ R 15 × 9 \mathbf{J}[3]=\begin{bmatrix} \frac{\partial r}{\partial \mathrm{v}_{b_{j}}^{w}} & \frac{\partial r}{\partial \mathrm{b_{j}}^{a}}&\frac{\partial r}{\partial \mathrm{b_{j}}^{g}} \end{bmatrix}=\left[\begin{array}{ccc} \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{R}_{b_{i} w} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{I} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{I} \end{array}\right] \in \mathbb{R}^{15 \times 9} J[3]=[∂vbjw∂r∂bja∂r∂bjg∂r]= 00Rbiw00000I00000I ∈R15×9