残差
由预积分
[
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