Notes for “A micro Lie theory for state estimation in robotics“ - 公式详细推导 (I)

[Title]: Notes for “A micro Lie theory for state estimation in robotics” - 公式详细推导 (I)

“A micro Lie theory for state estimation in robotics” ¹ 这篇论文中比较难的是几个Jacobian矩阵的推导，也是这个笔记的主要内容.

在下面推导笔记中，形如 $\stackrel{(31)}{=}$ 或 $\stackrel{(31)}{\Rightarrow }$ 意味着该步推导依据的是论文¹ 中 eq(31).

I. About “Example 6”

1. 对于任意旋转矩阵 $R$及向量 $a$ 和 $b$，有

$$Ra\times Rb=R(a\times b)$$

2. 向量$u$，定义对应的线性变换是$u_{\times }$

$$u_{\times }:v\to u\times v$$

也可以写成$u_{\times }\triangleq [u{]}_{\times }$。以及有
$$[a{]}_{\times }b=a\times b=-b\times a=-[b{]}_{\times }a$$

对于任意向量 $\theta \in {\mathbb{R}}^3$ 及 $R\in SO(3)$，可以计算
$$\begin{array}{rl}{\left[R\theta \right]}_{\times }v& =(R\theta )\times v\\ & =(R\theta )\times (R{R}^{-1}v)\\ & =R(\theta \times R^{-1}v)\\ & =R([\theta {]}_{\times }{R}^{-1}v)\\ & =R[\theta {]}_{\times }{R}^{-1}v\end{array}$$
所以：
$${\left[R\theta \right]}_{\times }=R[\theta {]}_{\times }{R}^{-1}=R[\theta {]}_{\times }{R}^{\mathrm{T}}\text{\hspace{1em}(I-1)}$$
依据"Example 6"中推导，已知
$$M=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]$$
时，
$${\mathrm{Ad}}_{\mathrm{M}}=\left[\begin{array}{cc}R& [t{]}_{\times }R\\ 0& R\end{array}\right]\in {\mathbb{R}}^6$$
将$M^{-1}$对应各部分代入，得到
$${\mathrm{Ad}}_{{\mathrm{M}}^{-1}}=\left[\begin{array}{cc}{R}^{\mathrm{T}}& [-R^{\mathrm{T}}t{]}_{\times }{R}^{\mathrm{T}}\\ 0& R^{\mathrm{T}}\end{array}\right]$$
以及，计算$\mathrm{A}{\mathrm{d}}_{\mathrm{M}}$的逆
$${{\mathrm{Ad}}_{\mathrm{M}}}^{-1}=\left[\begin{array}{cc}{R}^{\mathrm{T}}& -R^{\mathrm{T}}[t{]}_{\times }\\ 0& R^{\mathrm{T}}\end{array}\right]$$
根据 ($\mathrm{I}\text{-}1$) 有
$$[-R^{\mathrm{T}}t{]}_{\times }{R}^{\mathrm{T}}=R^{\mathrm{T}}R[-R^{\mathrm{T}}t{]}_{\times }{R}^{\mathrm{T}}=R^{\mathrm{T}}[R(-R^{\mathrm{T}}t){]}_{\times }=-R^{\mathrm{T}}[t{]}_{\times }$$
所以有 $\mathrm{A}{\mathrm{d}}_{{\mathrm{M}}^{-1}}={\mathrm{A}{\mathrm{d}}_{\mathrm{M}}}^{-1}$， 即论文中 eq(33).

II. About eq(46)

论文中Figure 9中知，
$${}^{ϵ}\sigma =\frac{{}^{ϵ}Dy}{Dx}{}^{ϵ}\tau \text{\hspace{1em}(II-1)}$$
其中
$${}^{ϵ}\sigma ={\mathrm{Ad}}_y{}^y\sigma$$

$${}^{ϵ}\tau ={\mathrm{Ad}}_x{}^x\tau$$

将 ${}^{ϵ}\sigma$ 和 ${}^{ϵ}\tau$ 分别代入 $(\mathrm{II}\text{-}1)$ 可以得到
$${\mathrm{Ad}}_y{}^y\sigma =\frac{{}^{ϵ}Dy}{Dx}{\mathrm{Ad}}_x{}^x\tau \text{\hspace{1em}(II-2)}$$
又由论文中 Figure 9 中已知
$${}^y\sigma =\frac{{}^{x}Dy}{Dx}{}^x\tau$$
代入 $(\mathrm{II}\text{-}2)$ 得到
$${\mathrm{Ad}}_y\frac{{}^{x}Dy}{Dx}{}^x\tau =\frac{{}^{ϵ}Dy}{Dx}{\mathrm{Ad}}_x{}^x\tau$$
把 ${\mathrm{Ad}}_y$ 和 ${\mathrm{Ad}}_x$ 都看作矩阵运算，得到
$${\mathrm{Ad}}_y\frac{{}^{x}Dy}{Dx}=\frac{{}^{ϵ}Dy}{Dx}{\mathrm{Ad}}_x\Rightarrow \frac{{}^{ϵ}Df(x)}{Dx}{\mathrm{Ad}}_x={\mathrm{Ad}}_{f(x)}\frac{{}^{x}Df(x)}{Dx}$$
就是论文中 eq(46).

III. About eq(68)~eq(74)

论文中 Figure 8¹ 中是关于函数$f:\mathcal{M}\to \mathcal{N}$ 的 Jacobian. 即 Jacobian maps differential increments in the Lie Algebra:
$$\begin{array}{rl}J:T_{x}M& \to T_{f(x)}N\\ {}^x\tau & \to {}^{f(x)}\sigma \end{array}$$
流形上的Right Jacobian定义参见论文中 eq(68)，需要特别注意 该定义的是流形$\mathcal{M}$ 上的 Jacobian，把参数 $\mathbf{\tau }$ 的变化 (李代数，单位元处切空间的变化) 映射为 $\mathrm{Exp}(\mathbf{\tau })$ 局部切空间上的变化. 都映射到切空间上，所以 eq(41b) 有 $\mathrm{Log}$.

下面推导 eq(68)
$$\begin{array}{rl}{J}_r(\tau )& \stackrel{(67)}{=}\frac{{}^{\tau }D\mathrm{Exp}(\tau )}{D\tau }\\ & \stackrel{(41a)}{=}\underset{\delta \tau \to 0}{\lim }\frac{\mathrm{Exp}(\tau \oplus \delta \tau )\ominus \mathrm{Exp}(\tau )}{\delta \tau }\\ & \stackrel{(41c)}{=}\underset{\delta \tau \to 0}{\lim }\frac{\mathrm{Log}\left[{\mathrm{Exp}(\tau )}^{-1}\mathrm{Exp}(\tau +\delta \tau )\right]}{\delta \tau }\end{array}$$
当 $\delta \tau$ 很小时，
$$\begin{array}{rl}{J}_r(\tau )\delta \tau & =\mathrm{Log}\left[{\mathrm{Exp}(\tau )}^{-1}\mathrm{Exp}(\tau +\delta \tau )\right]\\ \Rightarrow \mathrm{Exp}\left(J_r(\tau )\delta \tau \right)& ={\mathrm{Exp}(\tau )}^{-1}\mathrm{Exp}(\tau +\delta \tau )\\ \Rightarrow \mathrm{Exp}(\tau )\mathrm{Exp}\left(J_r(\tau )\delta \tau \right)& =\mathrm{Exp}(\tau +\delta \tau )\end{array}$$
得到 eq(68). 注意上面 $\tau$ 和 $\delta \tau$ 都 $\in {\mathbb{R}}^m$，所以 $\oplus =+$；而 $\mathrm{Exp}(\tau )\in \mathcal{M}$，所示还是 “right-$\ominus$” defined in eq(26).

eq(72) 与 eq(68) 类似计算.

下面计算 eq(69)，由 eq(67) 已知 $J_r(\tau )\in {\mathbb{R}}^{m\times m}$，所以就当 ${J_r(\tau )}^{-1}\delta \tau \in {\mathbb{R}}^m$ 只是对 $\delta \tau \in {\mathbb{R}}^m$ 尺寸的变换而不改变 $\delta \tau$ 所处的空间.
$$\begin{array}{rl}{J}_r(\tau )& \stackrel{(67)}{=}\frac{{}^{\tau }D\mathrm{Exp}(\tau )}{D\tau }\\ & \stackrel{(41a)}{=}\underset{J_r^{-1}\delta \tau \to 0}{\lim }\frac{\mathrm{Exp}(\tau \oplus J_r^{-1}\delta \tau )\ominus \mathrm{Exp}(\tau )}{J_r^{-1}\delta \tau }\\ & \stackrel{(41c)}{=}\underset{\delta \tau \to 0}{\lim }\frac{\mathrm{Log}\left[{\mathrm{Exp}(\tau )}^{-1}\mathrm{Exp}(\tau +J_r^{-1}\delta \tau )\right]}{J_r^{-1}\delta \tau }\end{array}$$
当 $\delta \tau$ 很小时，
$$\begin{array}{rl}{J}_r(\tau )J_r^{-1}\delta \tau & =\mathrm{Log}\left[{\mathrm{Exp}(\tau )}^{-1}\mathrm{Exp}(\tau +J_r^{-1}\delta \tau )\right]\\ \Rightarrow \mathrm{Exp}(\delta \tau )& ={\mathrm{Exp}(\tau )}^{-1}\mathrm{Exp}(\tau +J_r^{-1}\delta \tau )\\ \Rightarrow \mathrm{Exp}(\tau )\mathrm{Exp}(\delta \tau )& =\mathrm{Exp}(\tau +{J_r}^{-1}\delta \tau )\\ \Rightarrow \mathrm{Log}\left(\mathrm{Exp}(\tau )\mathrm{Exp}(\delta \tau )\right)& =\tau +{J_r}^{-1}\delta \tau \end{array}$$
得到 eq(69) 和 eq(70). eq(73) 和 eq(74) 类似.

另外一种解释如下：

参考 “State Estimation for Robotics”，通过BCH公式 (Baker-Campbell-Hausdorff formula) 计算² ，可以获得关于 $\mathbf{\tau }$ 和 $\delta \mathbf{\tau }$ 的多项式，如 "State Estimation for Robotics"中的 (7.74). 因为 $\delta \mathbf{\tau }\ll \mathbf{\tau }$， 可以将多项式 (7.74) 中的 $\delta \mathbf{\tau }$ 二次项及以上项都去除掉，得到 (7.75)， 即可对应论文中的 eq(69)、eq(70)、eq(73)、eq(74). 实际上，Jacobina 及其逆的级数求和形式也都是从该BCH公式近似推导而来.

IV. About eq(75)

由 eq(68) 和 eq(72) 可知
$$\begin{array}{rl}\mathrm{Exp}(\tau )\mathrm{Exp}(J_r(\tau )\delta \tau )& =\mathrm{Exp}(J_l(\tau )\delta \tau )\mathrm{Exp}(\tau )\\ \Rightarrow \mathrm{Exp}(\tau )\mathrm{Exp}(J_r(\tau )\delta \tau ){\mathrm{Exp}(\tau )}^{-1}& =\mathrm{Exp}(J_l(\tau )\delta \tau )\end{array}$$
上式中

$$\mathrm{Exp}(J_r(\tau )\delta \tau )\stackrel{(23)}{=}\exp \left[(J_r(\tau )\delta \tau {)}^{\wedge }\right]\stackrel{(16)}{=}I+(J_r(\tau )\delta \tau {)}^{\wedge }+\frac{1}{2}{(J_r(\tau )\delta \tau {)}^{\wedge }}^2+...$$
则
$$\mathrm{Exp}(\tau )\mathrm{Exp}(J_r(\tau )\delta \tau ){\mathrm{Exp}(\tau )}^{-1}=I+\mathrm{Exp}(\tau )(J_r(\tau )\delta \tau {)}^{\wedge }{\mathrm{Exp}(\tau )}^{-1}+\mathrm{Exp}(\tau )\frac{1}{2}{(J_r(\tau )\delta \tau {)}^{\wedge }}^2{\mathrm{Exp}(\tau )}^{-1}\text{\hspace{1em}(IV-1)}$$
另有
$$\mathrm{Exp}(J_l(\tau )\delta \tau )\stackrel{(23)}{=}\exp \left[(J_l(\tau )\delta \tau {)}^{\wedge }\right]\stackrel{(16)}{=}I+(J_l(\tau )\delta \tau {)}^{\wedge }+\frac{1}{2}{(J_l(\tau )\delta \tau {)}^{\wedge }}^2+...\text{\hspace{1em}(IV-2)}$$
其中 $\delta \tau \in {\mathbb{R}}^m$ 是任意的向量（变量），把 ($\text{IV-1}$) 和 ($\text{IV-2}$) 看作是 $\delta \tau$ 的多项式，所以需要 ($\text{IV-1}$) 和 ($\text{IV-2}$) 的 $\delta \tau$ 对应各次项系数相同. 一次项系数相同可以得到
$$\begin{array}{rl}\mathrm{Exp}(\tau )(J_r(\tau )\delta \tau {)}^{\wedge }{\mathrm{Exp}(\tau )}^{-1}& =(J_l(\tau )\delta \tau {)}^{\wedge }\\ \Rightarrow {\left[\mathrm{Exp}(\tau )(J_r(\tau )\delta \tau {)}^{\wedge }{\mathrm{Exp}(\tau )}^{-1}\right]}^{\vee }& =J_l(\tau )\delta \tau \\ \stackrel{(31)}{\Rightarrow }{\mathrm{Ad}}_{\mathrm{Exp}(\tau )}(J_r(\tau )\delta \tau )& =J_l(\tau )\delta \tau \end{array}$$
根据论文 eq(31) 中，把 ${\mathrm{Ad}}_X$ 作为 “matrix operator”， 即称为 “adjoint matrix”. 这样把 ${\mathrm{Ad}}_{\mathrm{Exp}(\tau )}$ 看作矩阵表示的线性映射，根据矩阵乘法的结合律就有
$$\begin{array}{rl}{\mathrm{Ad}}_{\mathrm{Exp}(\tau )}{J}_r(\tau )& =J_l(\tau )\\ \Rightarrow {\mathrm{Ad}}_{\mathrm{Exp}(\tau )}& =J_l(\tau ){J_r(\tau )}^{-1}\end{array}$$
获得 eq(75).

V. About eq(79)

方法一：

由论文中 eq(57) 下面一行对 $J_x^{f(x)}$ 的定义， 以及题设中 $\tau =\mathrm{Log}(x)$，可知
$$\begin{array}{rl}{J}_x^{\mathrm{Log}(x)}& \stackrel{(定义)}{=}\frac{D\mathrm{Log}(x)}{Dx}\\ & \stackrel{(题设)}{=}\frac{D\tau }{D(\mathrm{Exp}(\tau ))}\\ & =\frac{1}{\frac{D(\mathrm{Exp}(\tau ))}{D\tau }}\\ & \stackrel{(67)}{=}{J_r(\tau )}^{-1}\end{array}$$
方法二：

方法一中已知
$$J_x^{\mathrm{Log}(x)}=\frac{D\mathrm{Log}(x)}{Dx}$$
对应于论文中 Figure 8 和 eq(41a) ，“Right Jacobian of function” 中的函数为 $f(x)=\mathrm{Log}(x)$. 虽然题设条件中提到 $\tau =\mathrm{Log}(\mathrm{x})$，但条件中讲的是切空间 (李代数) 与流形之间的指数映射，即 $\mathrm{Exp}(\tau )=x$ . 此处需要注意两个 $\mathrm{Log}$ 之间的角色区别. 由 eq(41a) 的定义
$$\begin{array}{rl}\frac{{}^{x}D\mathrm{Log}(x)}{Dx}& =\underset{\delta \tau \to 0}{\lim }\frac{\mathrm{Log}(x\oplus \delta \tau )\ominus \mathrm{Log}(x)}{\delta \tau }\\ (\mathbf{notes}:f(x)& =\mathrm{Log}(x),x\in \mathcal{M},\delta \tau \in {\mathbb{R}}^m)\\ & =\underset{\delta \tau \to 0}{\lim }\frac{\mathrm{Log}(x\mathrm{Exp}(\delta \tau ))\ominus \mathrm{Log}(x)}{\delta \tau }\\ (\mathbf{notes}:\tau & =\mathrm{Log}(x)\in {\mathbb{R}}^m,x=\mathrm{Exp}(\tau )\in \mathcal{M})\\ & =\underset{\delta \tau \to 0}{\lim }\frac{\mathrm{Log}(\mathrm{Exp}(\tau )\mathrm{Exp}(\delta \tau ))-\tau }{\delta \tau }\end{array}$$
当 $\delta \tau$ 很小的时候
$$\frac{\mathrm{Log}(\mathrm{Exp}(\tau )\mathrm{Exp}(\delta \tau ))-\tau }{\delta \tau }=J_x^{\mathrm{Log}(x)}$$
另外由 eq(70) 可知
$$\frac{\mathrm{Log}(\mathrm{Exp}(\tau )\mathrm{Exp}(\delta \tau ))-\tau }{\delta \tau }={J_r(\tau )}^{-1}$$
所以 eq(79) 成立，即 $J_x^{\mathrm{Log}(x)}={J_r(\tau )}^{-1}$.

VI. About “Rodrigues rotation formula”

可以计算发现，Rodrigues rotation formula 除了对 $SO(3)$ 旋转成立（Example 4），对 2D 空间中的旋转 $S^1$ 和 $SO(2)$ 一样成立.

1 对 $S^1$ 的验证

$$\begin{gathered} {\tau }^{\wedge }=[\theta {]}_{\times }=i\theta =[1{]}_{\times }\theta \\ [1{]}_{\times }=i \end{gathered}$$

计算 Rodrigues rotation formula
$$\begin{array}{rl}\exp ([1{]}_{\times }\theta )& =1+[1{]}_{\times }\sin (\theta )+[1{]}_{\times }^2(1-\cos (\theta ))\\ & =1+i\sin (\theta )-(1-\cos (\theta ))\\ & =\cos (\theta )+i\sin (\theta )\end{array}$$

2 对 $SO(2)$ 的验证

$$\begin{gathered} {\tau }^{\wedge }=[\theta {]}_{\times }=\left[\begin{array}{cc}0& -\theta \\ \theta & 0\end{array}\right]=[u{]}_{\times }\theta \\ [u{]}_{\times }=\left[\begin{array}{cc}0& -1\\ 1& 0\end{array}\right] \\ [u{]}_{\times }^2=\left[\begin{array}{cc}0& -1\\ 1& 0\end{array}\right]\left[\begin{array}{cc}0& -1\\ 1& 0\end{array}\right]=\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right] \end{gathered}$$

计算 Rodrigues rotation formula
$$\begin{array}{rl}\exp ([u{]}_{\times }\theta )& =I+[u{]}_{\times }\sin (\theta )+[u{]}_{\times }^2(1-\cos (\theta ))\\ & =\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]+\left[\begin{array}{cc}0& -1\\ 1& 0\end{array}\right]\sin (\theta )+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right](1-\cos (\theta ))\\ & =\left[\begin{array}{cc}0& -1\\ 1& 0\end{array}\right]\sin (\theta )+\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right](\cos (\theta ))\\ & =\left[\begin{array}{cc}\cos (\theta )& -\sin (\theta )\\ \sin (\theta )& \cos (\theta )\end{array}\right]\end{array}$$

3 对 $SO(3)$ 的计算

Rodrigues rotation formula 在 $SO(3)$ 中的计算由论文中 Example 4 给出，下面计算 eq(135)
$$\begin{gathered} R=\exp ([\mathbf{u}\theta {]}_{\times })=I+[\mathbf{u}{]}_{\times }\sin (\theta )+[\mathbf{u}{]}_{\times }^2(1-\cos (\theta )) \\ [\mathbf{u}{]}_{\times }^{\mathrm{T}}=-[\mathbf{u}{]}_{\times } \\ [\mathbf{u}{]}_{\times }^2=\mathbf{u}{\mathbf{u}}^{\mathrm{T}}-I={([\mathbf{u}{]}_{\times }^2)}^{\mathrm{T}} \\ \Rightarrow R^{\mathrm{T}}=I-[\mathbf{u}{]}_{\times }\sin (\theta )+[\mathbf{u}{]}_{\times }^2(1-\cos (\theta )) \\ \Rightarrow R-R^{\mathrm{T}}=2[\mathbf{u}{]}_{\times }\sin (\theta ) \\ \Rightarrow \frac{R-R^{\mathrm{T}}}{2\sin (\theta )}=[\mathbf{u}{]}_{\times } \\ \Rightarrow {\left(\frac{R-R^{\mathrm{T}}}{2\sin (\theta )}\right)}^{\vee }=\mathbf{u}\in {\mathbb{R}}^3 \\ \Rightarrow \mathbf{u}\theta =\frac{\theta (R-R^{\mathrm{T}}{)}^{\vee }}{2\sin (\theta )}\in {\mathbb{R}}^3 \end{gathered}$$
其中 $\mathbf{u}=[u_x,u_y,u_z{]}^{\mathrm{T}}$ 是单位向量.
$$\begin{gathered} [\mathbf{u}{]}_{\times }=\left[\begin{array}{ccc}0& -u_z& u_y\\ u_z& 0& -u_x\\ -u2& u_x& 0\end{array}\right] \\ [\mathbf{u}{]}_{\times }^2=\mathbf{u}{\mathbf{u}}^{\mathrm{t}}-I=\left[\begin{array}{ccc}{u}_x^2-1& u_x{u}_y& u_x{u}_z\\ u_x{u}_y& u_y^2-1& u_y{u}_z\\ u_x{u}_z& u_y{u}_z& u_z^2-1\end{array}\right] \\ R=\exp ([\mathbf{u}\theta {]}_{\times })=\left[\begin{array}{ccc}1& & \\ & 1& \\ & & 1\end{array}\right]+\left[\begin{array}{ccc}0& -u_z& u_y\\ u_z& 0& -u_x\\ -u2& u_x& 0\end{array}\right]\sin (\theta )+\left[\begin{array}{ccc}{u}_x^2-1& u_x{u}_y& u_x{u}_z\\ u_x{u}_y& u_y^2-1& u_y{u}_z\\ u_x{u}_z& u_y{u}_z& u_z^2-1\end{array}\right](1-\cos (\theta )) \\ \Rightarrow \mathrm{trace}(R)=3+(u_x^2+u_y^2+u_z^2-3)(1-cos(\theta ))=3-2+2cos(\theta ) \\ \Rightarrow cos(\theta )=\frac{\mathrm{trace}(R)-1}{2}\Rightarrow \theta ={\cos }^{-1}\left(\frac{\mathrm{trace}(R)-1}{2}\right) \end{gathered}$$
得到 eq(135).

— To be continued —

<div id="ref-1"></div>
[1] Joan Solà, Jeremie Deray, Dinesh Atchuthan, "A micro Lie theory for state estimation in robotics", arXiv:1812.01537 [cs.RO]

<div id="ref-2"></div>
[2] Timothy D. Barfoot, "State Estimation for Robotics", Cambridge University Press, 2017

<div id="ref-3"></div>
[3] Bernoulli Number, https://mathworld.wolfram.com/BernoulliNumber.html

<div id="ref-4"></div>
[4] 双曲三角函数与三角函数泰勒展开式, https://zhuanlan.zhihu.com/p/166135510

<div id="ref-5"></div>
[5] 三角函数半角公式, https://zhidao.baidu.com/question/1245914655285565579.html

<div id="ref-6"></div>
[6] Gregory S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups, Volume 2", Birkhäuser Boston, MA, 2011