【slam公式】 - 技术栈

F：状态转移矩阵

H：观测矩阵

R: 观测噪声协方差

泰勒展开

ex=1+x+x2/2...e^x = 1+x+x^2 /2...ex=1+x+x2/2...

cos⁡θ=∑n=0∞(−1)n(2n)!θ2n=1−θ22!+θ44!−θ66!+⋯ ,θ∈R \cos\theta=\sum_{n=0}^{\infty}\frac{(-1)^n}{(2n)!}\theta^{2n} =1-\frac{\theta^2}{2!}+\frac{\theta^4}{4!}-\frac{\theta^6}{6!}+\cdots,\quad \theta\in R cosθ=n=0∑∞(2n)!(−1)nθ2n=1−2!θ2+4!θ4−6!θ6+⋯,θ∈R

sin⁡θ=∑n=0∞(−1)n(2n+1)!θ2n+1=θ−θ33!+θ55!−⋯ ,θ∈R \sin\theta=\sum_{n=0}^{\infty}\frac{(-1)^n}{(2n+1)!}\theta^{2n+1} =\theta-\frac{\theta^3}{3!}+\frac{\theta^5}{5!}-\cdots,\quad \theta\in \mathbf R sinθ=n=0∑∞(2n+1)!(−1)nθ2n+1=θ−3!θ3+5!θ5−⋯,θ∈R

罗德里格斯公式

R=exp⁡(ϕ∧)=I+sin⁡θ⋅a∧+(1−cos⁡θ)⋅(a∧)2 \boldsymbol{R} = \exp(\boldsymbol{\phi}^\wedge) = \boldsymbol{I} + \sin\theta \cdot \boldsymbol{a}^\wedge + (1-\cos\theta)\cdot (\boldsymbol{a}^\wedge)^2 R=exp(ϕ∧)=I+sinθ⋅a∧+(1−cosθ)⋅(a∧)2

反对称记号：a∧\boldsymbol{a}^\wedgea∧ 是单位轴 a\boldsymbol{a}a 的反对称矩阵

泰勒展开形式（李群指数映射展开）

exp⁡(ϕ∧)=I+ϕ∧+(ϕ∧)22!+(ϕ∧)33!+(ϕ∧)44!+⋯\exp(\boldsymbol{\phi}^\wedge) =\boldsymbol{I}+\boldsymbol{\phi}^\wedge+\frac{(\boldsymbol{\phi}^\wedge)^2}{2!}+\frac{(\boldsymbol{\phi}^\wedge)^3}{3!}+\frac{(\boldsymbol{\phi}^\wedge)^4}{4!}+\cdotsexp(ϕ∧)=I+ϕ∧+2!(ϕ∧)2+3!(ϕ∧)3+4!(ϕ∧)4+⋯

利用恒等式：

(a∧)3=−a∧,(a∧)4=−(a∧)2(\boldsymbol{a}^\wedge)^3 = -\boldsymbol{a}^\wedge,\quad (\boldsymbol{a}^\wedge)^4=-(\boldsymbol{a}^\wedge)^2(a∧)3=−a∧,(a∧)4=−(a∧)2

代入 ϕ∧=θa∧\boldsymbol{\phi}^\wedge=\theta \boldsymbol{a}^\wedgeϕ∧=θa∧ 合并级数，就得到上面紧凑的罗德里格斯。

小角度近似sin⁡θ≈θ, cos⁡θ≈1−θ22 \sin\theta \approx \theta,\, \cos\theta \approx 1-\frac{\theta^2}{2} sinθ≈θ,cosθ≈1−2θ2

R≈I+ϕ∧,θ≪1 \boldsymbol{R}\approx \boldsymbol{I}+\boldsymbol{\phi}^\wedge,\quad \theta\ll1 R≈I+ϕ∧,θ≪1

旋转矩阵

具备正交性：RTR=I\boldsymbol R^T \boldsymbol R = \boldsymbol IRTR=I，所以 R−1=RT\boldsymbol R^{-1}=\boldsymbol R^TR−1=RT；

反对称矩阵

(ω∧)T=−ω∧ (\mathbf{\omega}^\wedge)^T = -\mathbf{\omega}^\wedge (ω∧)T=−ω∧

ω∧v=ω×v\mathbf{\omega}^\wedge \mathbf{v} = \mathbf{\omega}\times \mathbf{v}ω∧v=ω×v

叉乘恒等式

a∧b=−b∧a \mathbf{a}^\wedge \mathbf{b} = -\mathbf{b}^\wedge \mathbf{a} a∧b=−b∧a

旋转保叉乘性质

R(a×b)=Ra×RbR(a\times b) = Ra \times RbR(a×b)=Ra×Rb

矩阵指数共轭性质

exp⁡(RART)=Rexp⁡(A)RT\exp(R A R^T) = R\exp(A)R^Texp(RART)=Rexp(A)RT（因为 (RART)n=RAnRT(RAR^T)^n = RA^nR^T(RART)n=RAnRT

伴随性质

(Ra)∧=Ra∧RT (\mathbf{R}\mathbf{a})^\wedge = \mathbf{R}\mathbf{a}^\wedge \mathbf{R}^T (Ra)∧=Ra∧RT

BCH公式

Z=X+Y+12 $X,Y$ +112 $X,\[X,Y$ ]−112 $Y,\[X,Y$ ]+⋯ \mathbf{Z}=\mathbf{X}+\mathbf{Y}+\frac12 $\\mathbf{X},\\mathbf{Y}$ +\frac{1}{12} $\\mathbf{X},\[\\mathbf{X},\\mathbf{Y}$ ] -\frac{1}{12} $\\mathbf{Y},\[\\mathbf{X},\\mathbf{Y}$ ]+\cdots Z=X+Y+21 $X,Y$ +121 $X,\[X,Y$ ]−121 $Y,\[X,Y$ ]+⋯

$X,Y$ =XY−YX $X,Y$ = XY-YX $X,Y$ =XY−YX：李括号

BCH 一阶近似（小扰动，核心）右乘扰动

exp⁡(ϕ∧)exp⁡(δϕ∧)=exp⁡((ϕ+Jr(ϕ)−1δϕ)∧)exp⁡(δϕ∧)exp⁡(ϕ∧)=exp⁡((ϕ+Jl(ϕ)−1δϕ)∧)\begin{aligned} \exp(\phi^\wedge)\exp(\delta\phi^\wedge) &= \exp\Big(\big(\phi + J_r(\phi)^{-1}\delta\phi\big)^\wedge\Big) \\ \exp(\delta\phi^\wedge)\exp(\phi^\wedge) &= \exp\Big(\big(\phi + J_l(\phi)^{-1}\delta\phi\big)^\wedge\Big) \end{aligned}exp(ϕ∧)exp(δϕ∧)exp(δϕ∧)exp(ϕ∧)=exp((ϕ+Jr(ϕ)−1δϕ)∧)=exp((ϕ+Jl(ϕ)−1δϕ)∧)

左乘扰动

共轭恒等式

exp⁡((Ra)∧)=Rexp⁡(a∧)RT\exp\big((R a)^\wedge\big) = R \exp(a^\wedge) R^Texp((Ra)∧)=Rexp(a∧)RT

推导过程：

(Ra)∧=Ra∧RT (\mathbf{R}\mathbf{a})^\wedge = \mathbf{R}\mathbf{a}^\wedge \mathbf{R}^T (Ra)∧=Ra∧RT

exp⁡(RAR−1)=∑n=0∞(RAR−1)nn!=∑n=0∞RAnR−1n!=R(∑n=0∞Ann!)R−1=Rexp⁡(A)R−1\begin{aligned} \exp(RAR^{-1}) &= \sum_{n=0}^{\infty} \frac{(RAR^{-1})^n}{n!} \\ &= \sum_{n=0}^{\infty} \frac{R A^n R^{-1}}{n!} \\ &= R\left( \sum_{n=0}^{\infty} \frac{A^n}{n!} \right) R^{-1} \\ &= R\exp(A) R^{-1} \end{aligned}exp(RAR−1)=n=0∑∞n!(RAR−1)n=n=0∑∞n!RAnR−1=R(n=0∑∞n!An)R−1=Rexp(A)R−1

李代数伴随算子

ad 是怎么定义的？
对于一个李代数 (\mathfrak g)，其中任意两个元素 (X,Y)，可以定义一个李括号：
$X,Y$ ∈g $X,Y$ \in\mathfrak g $X,Y$ ∈g

那么，固定其中一个元素 (X)，把另一个元素 (Y) 看作输入，就可以定义一个映射：

adX(Y)= $X,Y$ \boxed{ \mathrm{ad}_X(Y)= $X,Y$ } adX(Y)= $X,Y$

注意区分：

(X)：固定的李代数元素。
(Y)：输入的李代数元素。
( $X,Y$ )：两个李代数元素之间的李括号。
(\mathrm{ad}_X)：以 (X) 为参数定义的线性算子。