参考
A micro Lie theory for state estimation in robotics
manif issues 116
常用求导公式
| Operation | 左雅克比 | 右雅克比 |
|---|---|---|
| X − 1 \mathcal{X}^{-1}X−1 | J X X − 1 = − I \mathbf{J}_{\mathcal{X}}^{\mathcal{X}^{-1}}=\mathbf{-I}JXX−1=−I | J X X − 1 = − A d X \mathbf{J}_{\mathcal{X}}^{\mathcal{X}^{-1}}=-\mathbf{Ad}_{\mathcal{X}}JXX−1=−AdX |
| X ∘ Y \mathcal{X}\circ\mathcal{Y}X∘Y | J X X ∘ Y = I ∣ J Y X ∘ Y = A d X \mathbf{J}_{\mathcal{X}}^{\mathcal{X} \circ \mathcal{Y}}=\mathbf{I}\mid\mathbf{J}_{\mathcal{Y}}^{\mathcal{X} \circ \mathcal{Y}}=\mathbf{Ad}_{\mathcal{X}}JXX∘Y=I∣JYX∘Y=AdX | J X X ∘ Y = A d Y − 1 ∣ J Y X ∘ Y = I \mathbf{J}_{\mathcal{X}}^{\mathcal{X} \circ \mathcal{Y}}=\mathbf{Ad}_{\mathcal{Y}}^{-1}\mid\mathbf{J}_{\mathcal{Y}}^{\mathcal{X} \circ \mathcal{Y}}=\mathbf{I}JXX∘Y=AdY−1∣JYX∘Y=I |
| E x p ( τ ) Exp(\boldsymbol{\tau})Exp(τ) | J τ E x p ( τ ) = J l ( τ ) \mathbf{J}_{\boldsymbol{\tau}}^{Exp(\boldsymbol{\tau})}=\mathbf{J}_{l}(\boldsymbol{\tau})JτExp(τ)=Jl(τ) | J τ E x p ( τ ) = J r ( τ ) \mathbf{J}_{\boldsymbol{\tau}}^{Exp(\boldsymbol{\tau})}=\mathbf{J}_{r}(\boldsymbol{\tau})JτExp(τ)=Jr(τ) |
| L o g ( X ) Log(\mathcal{X})Log(X) | J X L o g ( X ) = J l − 1 ( τ ) \mathbf{J}_{\mathcal{X}}^{Log(\mathcal{X})}=\mathbf{J}_{l}^{-1}(\boldsymbol{\tau})JXLog(X)=Jl−1(τ) | J X L o g ( X ) = J r − 1 ( τ ) \mathbf{J}_{\mathcal{X}}^{Log(\mathcal{X})}=\mathbf{J}_{r}^{-1}(\boldsymbol{\tau})JXLog(X)=Jr−1(τ) |
| Plus | J X τ ⊕ X = A d E x p ( τ ) ∣ J τ τ ⊕ X = J l ( τ ) \mathbf{J}_{\mathcal{X}}^{\boldsymbol{\tau}\oplus\mathcal{X}}=\mathbf{Ad}_{Exp(\boldsymbol{\tau})}\mid\mathbf{J}_{\boldsymbol{\tau}}^{\boldsymbol{\tau}\oplus\mathcal{X}}=\mathbf{J}_{l}(\boldsymbol{\tau})JXτ⊕X=AdExp(τ)∣Jττ⊕X=Jl(τ) | J X X ⊕ τ = A d E x p ( τ ) − 1 ∣ J τ X ⊕ τ = J r ( τ ) \mathbf{J}_{\mathcal{X}}^{\mathcal{X}\oplus\boldsymbol{\tau}}=\mathbf{Ad}_{Exp(\boldsymbol{\tau})}^{-1}\mid\mathbf{J}_{\boldsymbol{\tau}}^{\mathcal{X}\oplus\boldsymbol{\tau}}=\mathbf{J}_{r}(\boldsymbol{\tau})JXX⊕τ=AdExp(τ)−1∣JτX⊕τ=Jr(τ) |
| Minus | J X X ⊖ Y = − J r − 1 ( τ ) ∣ J Y X ⊖ Y = J l − 1 ( τ ) \mathbf{J}_{\mathcal{X}}^{\mathcal{X}\ominus\mathcal{Y}}=-\mathbf{J}_{r}^{-1}(\boldsymbol{\tau})\mid\mathbf{J}_{\mathcal{Y}}^{\mathcal{X}\ominus\mathcal{Y}}=\mathbf{J}_{l}^{-1}(\boldsymbol{\tau})JXX⊖Y=−Jr−1(τ)∣JYX⊖Y=Jl−1(τ) | J X X ⊖ Y = J r − 1 ( τ ) ∣ J Y X ⊖ Y = − J l − 1 ( τ ) \mathbf{J}_{\mathcal{X}}^{\mathcal{X}\ominus\mathcal{Y}}=\mathbf{J}_{r}^{-1}(\boldsymbol{\tau})\mid\mathbf{J}_{\mathcal{Y}}^{\mathcal{X}\ominus\mathcal{Y}}=-\mathbf{J}_{l}^{-1}(\boldsymbol{\tau})JXX⊖Y=Jr−1(τ)∣JYX⊖Y=−Jl−1(τ) |
公式中的伴随矩阵
对于SO3:
A d R = R \mathbf{Ad_{R}} = \mathbf{R}AdR=R
对于SE3:
M = [ R t 0 1 ] A d M = [ R ⌊ t ⌋ × R 0 R ] \begin{aligned} \mathbf{M} &= \begin{bmatrix} \mathbf{R} & \mathbf{t}\\ \mathbf{0} & 1 \end{bmatrix} \\ \mathbf{Ad_{M}} &= \begin{bmatrix} \mathbf{R} & \left \lfloor \mathbf{t} \right \rfloor_{\times}\mathbf{R} \\ \mathbf{0} & \mathbf{R} \end{bmatrix} \end{aligned}MAdM=[R0t1]=[R0⌊t⌋×RR]
公式中的左右雅克比 J r \mathbf{J}_rJr、J l \mathbf{J}_lJl
对于SO3:
对于SE3:
版权声明：本文为weixin_37835423原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接和本声明。