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) 为参数定义的线性算子。
转载自 CSDN-专业IT技术社区
原文链接：https://blog.csdn.net/qq_44691564/article/details/169928996
