BCH(Baker-Campbell-Hausdorff)公式
:
ln(exp(A)exp(B))=A+B+12[A,B]+112[A,[A,B]]−112[B,[A,B]]+⋯
当为SO(3)上的李代数
ln(exp(Φ∧1)exp(Φ∧2))∨
时,BCH变为:
ln(exp(Φ∧1)exp(Φ∧2))∨≈{Jl(Φ2)−1Φ1+Φ2Jr(Φ1)−1Φ2+Φ1if Φ1 is small,if Φ2 is small.
这其中,左乘BCH近似的Jacobi
Jl
为:
Jl=J=sinθθI+(1−sinθθ)a⃗ a⃗ ⊤+1−cosθθa⃗ ∧
Jl
的逆为
J−1l=θ2cotθ2I+(1−θ2cotθ2)a⃗ a⃗ ⊤−θ2a⃗ ∧
另外, 右乘Jacobi
Jr
可对自变量取负号即可得到:
Jr(Φ)=Jl(−Φ)
BCH近似的意义
. 旋转
R
对应的李代数为
Φ
, 左乘微小旋转
△R
所对应的李代数为
△Φ
.
则李群上的结果为
△R⋅R
,李代数的结果根据BCH变为
J−1l(Φ)△Φ+Φ
:
exp(△Φ∧)exp(Φ∧)=exp((Φ+J−1l(Φ)△Φ)∧)
若在李代数上做加法,
Φ→Φ+△Φ
:
exp((Φ+△Φ)∧)=exp((Jl△Φ)∧)exp(Φ∧)=exp(Φ∧)exp((Jr△Φ)∧)
SO(3)李代数的求导
。两种思路:
- [x] 用李代数表示姿态,根据李代数加法对李代数求导;
- [x] 对李群左乘或右乘微小的扰动,对该扰动求导.
3.1 问题
:对一个空间点
p⃗
进行旋转,得到
Rp⃗
,求旋转后的点相对于旋转的导数,即
∂Rp⃗ ∂R
.
设
R
对应的李代数为
Φ
, 则问题表述为:
∂(exp(Φ∧)p⃗ )∂Φ
∂(exp(Φ∧)p⃗ )∂Φ=limδΦ→0exp((Φ+δΦ)∧)p⃗ −exp(Φ∧)p⃗ δΦ=limδΦ→0exp((JlδΦ)∧)exp(Φ∧)p⃗ −exp(Φ∧)p⃗ δΦ (BCH linear approximation)≈limδΦ→0I+((JlδΦ)∧)exp(Φ∧)p⃗ −exp(Φ∧)p⃗ δΦ (Taylor's formula)=limδΦ→0(JlδΦ)∧exp(Φ∧)p⃗ δΦ=limδΦ→0−(exp(Φ∧)p⃗ )∧JlδΦδΦ (cross product)=−(Rp⃗ )∧Jl.
所以旋转后的点相对于李代数的导数为:
∂(Rp⃗ )∂Φ=(−Rp⃗ )∧Jl
3.2 扰动模型的求导公式推导
问题
:对
R
的一次扰动量为
△R
。设左扰动
△R
对应的李代数为
φ
,对
φ
求导即为问题解:
∂Rp⃗ ∂φ=limφ→0exp(φ∧)exp(Φ∧)p⃗ −exp(Φ∧)p⃗ φ.
∂Rp⃗ ∂φ=limφ→0exp(φ∧)exp(Φ∧)p⃗ −exp(Φ∧)p⃗ φ≈limφ→0(1+φ∧)exp(Φ∧)p⃗ −exp(Φ∧)p⃗ φ=limφ→0φ∧exp(Φ∧)p⃗ φ=limφ→0φ∧Rp⃗ φ=limφ→0−(Rp⃗ )∧φφ=−(Rp⃗ )∧.