以上是结论
接下来将推导上面的公式是如何来的
估计值的求导也符合真实值求导的规则,下面会用这个规则对一些变量进行消除
位置求导
P P P 为估计值, P ′ = V P^{'}=V P′=V,真值为 P t = P + Δ P P_{t}=P+ΔP Pt=P+ΔP ,对其进行求导 ( P + Δ P ) ′ = V + Δ V = > P ′ + Δ P ′ = V + Δ V = > Δ P ′ = Δ V (P+ΔP)^{'}=V+ΔV =>P^{'}+ΔP^{'}=V+ΔV=>ΔP^{'}=ΔV (P+ΔP)′=V+ΔV=>P′+ΔP′=V+ΔV=>ΔP′=ΔV
零偏求导
b
t
′
=
0
=
>
(
b
+
Δ
b
′
)
=
0
=
>
b
′
+
Δ
b
′
=
0
b^{'}_{t}=0=>(b+Δb^{'})=0=>b^{'}+Δb^{'}=0
bt′=0=>(b+Δb′)=0=>b′+Δb′=0,
b
′
b^{'}
b′实际是一个高斯分布
n
b
n_{b}
nb,代入
则
Δ
b
′
=
−
n
b
则Δb^{'}=-n_{b}
则Δb′=−nb,正负都一样。
b
t
=
b
b
+
Δ
b
b
b_{t}=b_{b}+Δb_{b}
bt=bb+Δbb
速度求导
速度是相对于
b
k
b_{k}
bk 时刻的速度 ,要变换到bk时刻下,真实值
V
t
˙
=
R
b
k
,
t
⋅
(
a
−
b
a
)
\dot{V_{t}}=R_{b_{k},t}·(a-ba)
Vt˙=Rbk,t⋅(a−ba),按照真实值=估计值+误差值的推导。
对于速度和旋转变换是估计值+误差值的形式,而加速度则是
a
^
=
a
t
+
b
a
+
n
a
\hat{a}=a_{t}+b_{a}+n_{a}
a^=at+ba+na 意思是测量值等于真值+零偏+噪声
(
V
+
Δ
V
)
′
=
R
⋅
Δ
R
(
a
−
n
a
−
b
a
−
Δ
b
a
)
(V+ΔV)^{'}=R·ΔR(a-n_{a}-b_{a}-Δb_{a})
(V+ΔV)′=R⋅ΔR(a−na−ba−Δba)。
根据十四讲的内容,
Δ
R
=
e
x
p
(
Φ
^
)
ΔR=exp(Φ^{\hat{}})
ΔR=exp(Φ^),Φ是旋转向量(李代数),对其进行一阶泰勒展开=
(
I
+
Δ
Φ
^
)
(I+ΔΦ\hat{})
(I+ΔΦ^),将其代入展开
V
′
+
Δ
V
′
=
R
(
a
−
n
a
−
b
a
−
Δ
b
a
)
+
R
⋅
Δ
Φ
^
⋅
(
a
−
n
a
−
b
a
−
Δ
b
a
)
V^{'}+ΔV^{'}=R(a-n_{a}-b_{a}-Δb_{a})+R·ΔΦ\hat{}·(a-n_{a}-b_{a}-Δb_{a})
V′+ΔV′=R(a−na−ba−Δba)+R⋅ΔΦ^⋅(a−na−ba−Δba) ②
由于真实值
V
t
˙
=
R
b
k
,
t
⋅
(
a
−
b
a
)
\dot{V_{t}}=R_{b_{k},t}·(a-ba)
Vt˙=Rbk,t⋅(a−ba),实际上估计值也是可以写成相同的形式,知识
a
,
b
a
a,b_{a}
a,ba是测量值而已,则根据上面的式子,估计值速度的导数
V
˙
=
R
⋅
(
a
−
b
a
)
\dot{V}=R·(a-ba)
V˙=R⋅(a−ba) ,代入上式 ②,则可以把
V
˙
\dot{V}
V˙ 给抵消掉,变成
Δ
V
′
=
R
(
−
n
a
−
Δ
b
a
)
+
R
⋅
Δ
Φ
^
(
a
−
b
a
)
ΔV^{'}=R(-n_{a}-Δb_{a})+R·ΔΦ\hat{}(a-b_{a})
ΔV′=R(−na−Δba)+R⋅ΔΦ^(a−ba)
说明:
Δ
Φ
^
ΔΦ\hat{}
ΔΦ^实际上是一个一阶小量,而
n
a
,
Δ
b
a
n_{a},Δb_{a}
na,Δba 也是一阶小量,它们分别是相对于加速度和零偏的误差值,一阶小量乘一阶小量等于二阶,二阶是相对于一阶的无穷小,由于在这里只维护一阶小量,所以括号内忽略掉
n
a
,
Δ
b
a
n_{a},Δb_{a}
na,Δba
利用
a
^
⋅
b
=
a
×
b
=
−
b
×
a
=
−
b
^
⋅
a
a^{\hat{}}·b=a×b=-b×a=-b^{\hat{}} ·a
a^⋅b=a×b=−b×a=−b^⋅a 的性质
Δ
V
′
=
R
(
−
n
a
−
Δ
b
a
)
+
R
⋅
Δ
Φ
^
(
a
−
b
a
)
ΔV^{'}=R(-n_{a}-Δb_{a})+R·ΔΦ^{\hat{}}(a-b_{a})
ΔV′=R(−na−Δba)+R⋅ΔΦ^(a−ba) =>
Δ
V
′
=
R
(
−
n
a
−
Δ
b
a
)
−
R
⋅
(
a
−
b
a
)
^
⋅
Δ
Φ
ΔV^{'}=R(-n_{a}-Δb_{a})-R·(a-b_{a})^{\hat{}}·ΔΦ
ΔV′=R(−na−Δba)−R⋅(a−ba)^⋅ΔΦ ΔΦ表示李代数(旋转向量)
变换一下
Δ
V
′
=
−
R
⋅
(
a
−
b
a
)
^
⋅
Δ
Φ
−
R
n
a
−
R
Δ
b
a
ΔV^{'}=-R·(a-b_{a})^{\hat{}}·ΔΦ-Rn_{a}-RΔb_{a}
ΔV′=−R⋅(a−ba)^⋅ΔΦ−Rna−RΔba
这样就和最上面论文中的公式对应上了
位姿求导
根据第一节的求导可得 q ˙ = 1 2 Ω ( w ) ⋅ q \dot{q}=\frac{1}{2}Ω(w)·q q˙=21Ω(w)⋅q ,不过这里是矩阵的乘法, q ˙ = 1 2 [ q ( w ) ] R ⋅ q \dot{q}=\frac{1}{2}[q(w)]_{R}·q q˙=21[q(w)]R⋅q ,这个 q ( w ) q(w) q(w) 是一个是实部为 0,虚部为向量的一个四元数 ,变回四元数乘法则为 = > q t ˙ = 1 2 q ⊕ w = 1 2 q t ⊕ ( w t − b w t ) => \dot{q_{t}}=\frac{1}{2}q⊕w= \frac{1}{2}q_{t}⊕(w_{t}-b_{wt}) =>qt˙=21q⊕w=21qt⊕(wt−bwt) , w w w 为陀螺仪的角速度测量值,上面的 q ˙ \dot{q} q˙ 是当作真实值,现在写成真实值=估计值+误差值的形式
(
q
⊕
Δ
q
)
′
=
1
2
q
⊕
Δ
q
⊕
(
w
−
n
w
−
b
w
−
Δ
b
w
)
(q⊕Δq)^{'}=\frac{1}{2}q⊕Δq⊕(w-n_{w}-b_{w}-Δb_{w})
(q⊕Δq)′=21q⊕Δq⊕(w−nw−bw−Δbw),根据四元数乘法求导的规则,只对左边进行变形
=
>
q
˙
⊕
Δ
q
+
q
⊕
Δ
q
˙
=
.
.
.
=>\dot{q}⊕Δq+q⊕\dot{Δq}=...
=>q˙⊕Δq+q⊕Δq˙=...
由于估计值的导数和真实值是一样的,只是下标不同
=
>
1
2
q
⊕
(
w
−
b
w
)
⊕
Δ
q
+
q
⊕
Δ
q
˙
=
1
2
q
⊕
Δ
q
⊕
(
w
−
n
w
−
b
w
−
Δ
b
w
)
=> \frac{1}{2}q⊕(w-b_{w})⊕Δq+q⊕\dot{Δq}=\frac{1}{2}q⊕Δq⊕(w-n_{w}-b_{w}-Δb_{w})
=>21q⊕(w−bw)⊕Δq+q⊕Δq˙=21q⊕Δq⊕(w−nw−bw−Δbw) ,根据乘法结合律,约掉
q
q
q ,四元数乘法不符合乘法交换律但是符合乘法结合律
=
>
1
2
(
w
−
b
w
)
⊕
Δ
q
+
Δ
q
˙
=
1
2
Δ
q
⊕
(
w
−
n
w
−
b
w
−
Δ
b
w
)
=> \frac{1}{2}(w-b_{w})⊕Δq+\dot{Δq}=\frac{1}{2}Δq⊕(w-n_{w}-b_{w}-Δb_{w})
=>21(w−bw)⊕Δq+Δq˙=21Δq⊕(w−nw−bw−Δbw)
同时乘以2
=
>
(
w
−
b
w
)
⊕
Δ
q
+
2
⋅
Δ
q
˙
=
Δ
q
⊕
(
w
−
n
w
−
b
w
−
Δ
b
w
)
=> (w-b_{w})⊕Δq+2·\dot{Δq}=Δq⊕(w-n_{w}-b_{w}-Δb_{w})
=>(w−bw)⊕Δq+2⋅Δq˙=Δq⊕(w−nw−bw−Δbw)
把
(
w
−
b
w
)
−
>
y
,
(
w
−
n
w
−
b
w
−
Δ
b
w
)
−
>
x
(w-b_{w})->y,(w-n_{w}-b_{w}-Δb_{w})->x
(w−bw)−>y,(w−nw−bw−Δbw)−>x 通过这样进行替换把式子变得更简洁
把导数放到一边
2
⋅
Δ
q
˙
=
Δ
q
⊕
x
−
y
⊕
Δ
q
2·\dot{Δq}=Δq⊕x-y⊕Δq
2⋅Δq˙=Δq⊕x−y⊕Δq,为了把
Δ
q
Δq
Δq 提到一边,把四元数乘法变成矩阵乘法
=
>
2
⋅
Δ
q
˙
=
(
[
x
]
R
−
[
y
]
L
)
⋅
Δ
q
=>2·\dot{Δq}=([x]_{R}-[y]_{L})·Δq
=>2⋅Δq˙=([x]R−[y]L)⋅Δq
注意
x
x
x 和
y
y
y 是一个实部为0的四元数
根据这个公式(第二节有讲),只需要考虑内部的相减,不用考虑实部的相减即可
Δ
q
=
[
c
o
s
θ
2
n
⃗
⋅
s
i
n
θ
2
]
=
[
1
Δ
θ
2
]
{Δq}=\begin{bmatrix} cos\frac{θ}{2} \\ \vec{n}·sin\frac{θ}{2} \\ \end{bmatrix}=\begin{bmatrix} 1 \\ \frac{Δθ}{2} \\ \end{bmatrix}
Δq=[cos2θn⋅sin2θ]=[12Δθ] 其中
Δ
θ
是矢量
=
w
⋅
Δ
t
Δθ是矢量 =w·Δt
Δθ是矢量=w⋅Δt
2
Δ
q
˙
=
[
0
Δ
θ
′
]
2\dot{Δq}=\begin{bmatrix} 0 \\ Δθ^{'} \\ \end{bmatrix}
2Δq˙=[0Δθ′] 这里面把 2 乘进去了
右边的
(
[
x
]
R
−
[
y
]
L
)
⋅
Δ
q
([x]_{R}-[y]_{L})·Δq
([x]R−[y]L)⋅Δq 写成矩阵有
=
(
[
0
−
x
T
x
−
x
^
]
−
[
0
−
y
T
y
−
y
^
]
)
⋅
[
1
Δ
θ
2
]
=(\begin{bmatrix} 0 & -x^{T} \\ x & -x^{\hat{}} \\ \end{bmatrix}-\begin{bmatrix} 0 & -y^{T} \\ y & -y^{\hat{}} \\ \end{bmatrix})·\begin{bmatrix} 1 \\ \frac{Δθ}{2} \\ \end{bmatrix}
=([0x−xT−x^]−[0y−yT−y^])⋅[12Δθ]
合并则有
[
0
−
x
T
+
y
T
x
−
y
−
x
^
−
y
^
]
⋅
[
1
Δ
θ
2
]
\begin{bmatrix} 0 & -x^{T}+y^{T} \\ x-y & -x^{\hat{}}-y^{\hat{}} \\ \end{bmatrix}·\begin{bmatrix} 1 \\ \frac{Δθ}{2} \\ \end{bmatrix}
[0x−y−xT+yT−x^−y^]⋅[12Δθ],
[
0
−
x
T
+
y
T
x
−
y
−
x
^
−
y
^
]
\begin{bmatrix} 0 & -x^{T}+y^{T} \\ x-y & -x^{\hat{}}-y^{\hat{}} \\ \end{bmatrix}
[0x−y−xT+yT−x^−y^]∈4×4,第一行的是1×4,第2行是3×4,由于
2
Δ
q
˙
=
[
0
Δ
θ
′
]
∈
4
×
1
2\dot{Δq}=\begin{bmatrix} 0 \\ Δθ^{'} \\ \end{bmatrix}∈4×1
2Δq˙=[0Δθ′]∈4×1其中的实部为 0 ,所以只考虑虚部相乘的部分
=
>
Δ
θ
′
=
x
−
y
−
(
x
+
y
)
^
⋅
θ
2
=>Δθ^{'}=x-y-(x+y)^{\hat{}}·\frac{θ}{2}
=>Δθ′=x−y−(x+y)^⋅2θ 现在要把
(
w
−
b
w
)
−
>
y
,
(
w
−
n
w
−
b
w
−
Δ
b
w
)
−
>
x
(w-b_{w})->y,(w-n_{w}-b_{w}-Δb_{w})->x
(w−bw)−>y,(w−nw−bw−Δbw)−>x 替换回去
=
>
Δ
θ
′
=
−
n
w
−
Δ
b
w
−
(
2
w
−
2
b
w
)
^
⋅
Δ
θ
2
=>Δθ^{'}=-n_{w}-Δb_{w}-(2w-2b_{w})^{\hat{}}·\frac{Δθ}{2}
=>Δθ′=−nw−Δbw−(2w−2bw)^⋅2Δθ由于
Δ
θ
Δθ
Δθ 是一阶无穷小,所以
(
x
−
y
)
(x-y)
(x−y) 里面的
n
w
,
Δ
b
w
n_{w},Δb_{w}
nw,Δbw 是可以忽略的
约掉2
=
>
Δ
θ
′
=
−
n
w
−
Δ
b
w
−
(
w
−
b
w
)
^
⋅
Δ
θ
=>Δθ^{'}=-n_{w}-Δb_{w}-(w-b_{w})^{\hat{}}·Δθ
=>Δθ′=−nw−Δbw−(w−bw)^⋅Δθ 这样就和论文中的公式完全一致了
说明: 我们最后拿到IMU的值是离散的,我们要从连续推导离散的一个形式,这个只是一个中间形式,离散的形式才是最终的形式,将在下一节讲解