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

[Title]: Notes for ``A micro Lie theory for state estimation in robotics’’ - 公式详细推导 (IV)

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

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

— Let’s continue —

XI About eq(180) in “Apendix D - the 3D rigid motion group $SE(3)$”

参考"State Estimation for Robotics"² ，给出了 $SE(3)$ 上的 Jacobian
$${\mathbf{J}}_l(\mathbf{\xi })=\sum _{n=0}^{\infty }\frac{1}{(n+1)!}({\mathbf{\xi }}^{⋏}{)}^n$$
其中
$$\mathbf{\xi }=\left[\begin{array}{c}\mathbf{\rho }\\ \mathbf{\theta }\end{array}\right]\in {\mathbb{R}}^6,{\mathbf{\xi }}^{⋏}=\left[\begin{array}{cc}{\mathbf{\theta }}^{\wedge }& {\mathbf{\rho }}^{\wedge }\\ 0& {\mathbf{\theta }}^{\wedge }\end{array}\right]\in {\mathbb{R}}^{6\times 6}$$
上式子写成
$${\mathbf{J}}_l(\mathbf{\rho },\mathbf{\theta })=\sum _{n=0}^{\infty }\frac{1}{(n+1)!}{\left[\begin{array}{cc}{\mathbf{\theta }}^{\wedge }& {\mathbf{\rho }}^{\wedge }\\ 0& {\mathbf{\theta }}^{\wedge }\end{array}\right]}^n=\left[\begin{array}{cc}{\mathbf{J}}_l(\mathbf{\theta })& \mathbf{Q}(\mathbf{\rho },\mathbf{\theta })\\ 0& {\mathbf{J}}_l(\mathbf{\theta })\end{array}\right]\text{\hspace{1em}(XI-1)}$$
为了表达清晰，本记录中 ${\mathbf{x}}^{\vee }$ 记为 $\hat{\mathbf{x}}$. 展开计算 (XI-1)
$$\begin{array}{rl}{\left[\begin{array}{cc}\hat{\mathbf{\theta }}& \hat{\mathbf{\rho }}\\ 0& \hat{\mathbf{\theta }}\end{array}\right]}^0& ={\mathbf{I}}_{6\times 6}=\left[\begin{array}{cc}{I}_{3\times 3}& 0\\ 0& I_{3\times 3}\end{array}\right]\\ {\left[\begin{array}{cc}\hat{\mathbf{\theta }}& \hat{\mathbf{\rho }}\\ 0& \hat{\mathbf{\theta }}\end{array}\right]}^1& =\left[\begin{array}{cc}\hat{\mathbf{\theta }}& \hat{\mathbf{\rho }}\\ 0& \hat{\mathbf{\theta }}\end{array}\right]\\ {\left[\begin{array}{cc}\hat{\mathbf{\theta }}& \hat{\mathbf{\rho }}\\ 0& \hat{\mathbf{\theta }}\end{array}\right]}^2& =\left[\begin{array}{cc}\hat{\mathbf{\theta }}& \hat{\mathbf{\rho }}\\ 0& \hat{\mathbf{\theta }}\end{array}\right]\left[\begin{array}{cc}\hat{\mathbf{\theta }}& \hat{\mathbf{\rho }}\\ 0& \hat{\mathbf{\theta }}\end{array}\right]=\left[\begin{array}{cc}{\hat{\mathbf{\theta }}}^2& \hat{\mathbf{\theta }}\hat{\mathbf{\rho }}+\hat{\mathbf{\rho }}\hat{\mathbf{\theta }}\\ 0& {\hat{\mathbf{\theta }}}^2\end{array}\right]\\ {\left[\begin{array}{cc}\hat{\mathbf{\theta }}& \hat{\mathbf{\rho }}\\ 0& \hat{\mathbf{\theta }}\end{array}\right]}^3& =\left[\begin{array}{cc}{\hat{\mathbf{\theta }}}^2& \hat{\mathbf{\theta }}\hat{\mathbf{\rho }}+\hat{\mathbf{\rho }}\hat{\mathbf{\theta }}\\ 0& {\hat{\mathbf{\theta }}}^2\end{array}\right]\left[\begin{array}{cc}\hat{\mathbf{\theta }}& \hat{\mathbf{\rho }}\\ 0& \hat{\mathbf{\theta }}\end{array}\right]=\left[\begin{array}{cc}{\hat{\mathbf{\theta }}}^3& {\hat{\mathbf{\theta }}}^2\hat{\mathbf{\rho }}+\hat{\mathbf{\theta }}\hat{\mathbf{\rho }}\hat{\mathbf{\theta }}+\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^2\\ 0& {\hat{\mathbf{\theta }}}^3\end{array}\right]\\ {\left[\begin{array}{cc}\hat{\mathbf{\theta }}& \hat{\mathbf{\rho }}\\ 0& \hat{\mathbf{\theta }}\end{array}\right]}^3& =\left[\begin{array}{cc}{\hat{\mathbf{\theta }}}^3& {\hat{\mathbf{\theta }}}^2\hat{\mathbf{\rho }}+\hat{\mathbf{\theta }}\hat{\mathbf{\rho }}\hat{\mathbf{\theta }}+\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^2\\ 0& {\hat{\mathbf{\theta }}}^3\end{array}\right]\left[\begin{array}{cc}\hat{\mathbf{\theta }}& \hat{\mathbf{\rho }}\\ 0& \hat{\mathbf{\theta }}\end{array}\right]=\left[\begin{array}{cc}{\hat{\mathbf{\theta }}}^4& {\hat{\mathbf{\theta }}}^3\hat{\mathbf{\rho }}+{\hat{\mathbf{\theta }}}^2\hat{\mathbf{\rho }}\hat{\mathbf{\theta }}+\hat{\mathbf{\theta }}\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^2+\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^3\\ 0& {\hat{\mathbf{\theta }}}^4\end{array}\right]\\ & ⋮\end{array}\text{\hspace{1em}(XI-2)}$$
显然

$${\mathbf{J}}_l(\mathbf{\theta })=\sum _{n=0}^{\infty }\frac{1}{(n+1)!}{\hat{\mathbf{\theta }}}^n$$

就是本 notes 中 VIII 部分的内容. 下面重点是 $\mathbf{Q}(\mathbf{\rho },\mathbf{\theta })$ 的计算. 由 (XI-2) 归纳

$$\begin{array}{rl}\mathbf{Q}(\mathbf{\rho },\mathbf{\theta })& =\frac{1}{2!}\hat{\mathbf{\rho }}+\frac{1}{3!}(\hat{\mathbf{\theta }}\hat{\mathbf{\rho }}+\hat{\mathbf{\rho }}\hat{\mathbf{\theta }})+\frac{1}{4!}({\hat{\mathbf{\theta }}}^2\hat{\mathbf{\rho }}+\hat{\mathbf{\theta }}\hat{\mathbf{\rho }}\hat{\mathbf{\theta }}+\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^2)+\frac{1}{5!}({\hat{\mathbf{\theta }}}^3\hat{\mathbf{\rho }}+{\hat{\mathbf{\theta }}}^2\hat{\mathbf{\rho }}\hat{\mathbf{\theta }}+\hat{\mathbf{\theta }}\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^2+\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^3)+\dots \\ & =\sum _{\begin{array}{r}i+j=0\\ i⩾0,j⩾0\end{array}}\frac{1}{(i+j+2)!}{\hat{\mathbf{\theta }}}^i\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^j+\sum _{\begin{array}{r}i+j=1\\ i⩾0,j⩾0\end{array}}\frac{1}{(i+j+2)!}{\hat{\mathbf{\theta }}}^i\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^j+\sum _{\begin{array}{r}i+j=2\\ i⩾0,j⩾0\end{array}}\frac{1}{(i+j+2)!}{\hat{\mathbf{\theta }}}^i\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^j+\dots \\ & =\sum _{n=0}^{\infty }\sum _{m=0}^{\infty }\frac{1}{(m+n+2)!}{\hat{\mathbf{\theta }}}^n\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^m\end{array}\text{\hspace{1em}(XI-3)}$$

再继续归纳前，先总结以下简单规律 (都可以计算获得，复杂的可以借助符号计算软件，诸如 maxima).

$$\hat{\mathbf{\theta }}=\theta [\mathbf{u}{]}_{\times }\text{\hspace{1em}(XI-4)}$$

$$\begin{array}{rlrl}{\left[\mathbf{u}\right]}_{\times }^0& ={\mathbf{I}}_{3\times 3}& [\mathbf{u}{]}_{\times }^3& =-[\mathbf{u}{]}_{\times }\\ [\mathbf{u}{]}_{\times }^4& =-[\mathbf{u}{]}_{\times }^2& [\mathbf{u}{]}_{\times }^5& =[\mathbf{u}{]}_{\times }\\ [\mathbf{u}{]}_{\times }^6& =[\mathbf{u}{]}_{\times }^2& [\mathbf{u}{]}_{\times }^7& =-[\mathbf{u}{]}_{\times }\\ [\mathbf{u}{]}_{\times }^8& =-[\mathbf{u}{]}_{\times }^2& [\mathbf{u}{]}_{\times }^9& =[\mathbf{u}{]}_{\times }\end{array}\text{\hspace{1em}(XI-5)}$$

$$\begin{array}{rl}{\left[\mathbf{u}\right]}_{\times }^2\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }& =[\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^2\\ [\mathbf{u}{]}_{\times }^2\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^2& =-[\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }\end{array}\text{\hspace{1em}(XI-6)}$$

对 (XI-3) 继续展开，为了看清规律，我们展开足够多项

$$\begin{array}{rl}\mathbf{Q}(\mathbf{\rho },\mathbf{\theta })=& \sum _{n=0}^{\infty }\sum _{m=0}^{\infty }\frac{1}{(m+n+2)!}{\hat{\mathbf{\theta }}}^n\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^m\\ & \\ =& \frac{1}{2!}\hat{\mathbf{\rho }}\\ & +\frac{1}{3!}(\hat{\mathbf{\theta }}\hat{\mathbf{\rho }}+\hat{\mathbf{\rho }}\hat{\mathbf{\theta }})\\ & +\frac{1}{4!}({\hat{\mathbf{\theta }}}^2\hat{\mathbf{\rho }}+\hat{\mathbf{\theta }}\hat{\mathbf{\rho }}\hat{\mathbf{\theta }}+\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^2)\\ & +\frac{1}{5!}({\hat{\mathbf{\theta }}}^3\hat{\mathbf{\rho }}+{\hat{\mathbf{\theta }}}^2\hat{\mathbf{\rho }}\hat{\mathbf{\theta }}+\hat{\mathbf{\theta }}\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^2+\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^3)\\ & +\frac{1}{6!}({\hat{\mathbf{\theta }}}^4\hat{\mathbf{\rho }}+{\hat{\mathbf{\theta }}}^3\hat{\mathbf{\rho }}\hat{\mathbf{\theta }}+{\hat{\mathbf{\theta }}}^2\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^2+\hat{\mathbf{\theta }}\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^3+\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^4)\\ & +\frac{1}{7!}({\hat{\mathbf{\theta }}}^5\hat{\mathbf{\rho }}+{\hat{\mathbf{\theta }}}^4\hat{\mathbf{\rho }}\hat{\mathbf{\theta }}+{\hat{\mathbf{\theta }}}^3\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^2+{\hat{\mathbf{\theta }}}^2\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^3+\hat{\mathbf{\theta }}\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^4+\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^5)\\ & +\frac{1}{8!}({\hat{\mathbf{\theta }}}^6\hat{\mathbf{\rho }}+{\hat{\mathbf{\theta }}}^5\hat{\mathbf{\rho }}\hat{\mathbf{\theta }}+{\hat{\mathbf{\theta }}}^4\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^2+{\hat{\mathbf{\theta }}}^3\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^3+{\hat{\mathbf{\theta }}}^2\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^4+\hat{\mathbf{\theta }}\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^5+\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^6)\\ & +\frac{1}{9!}({\hat{\mathbf{\theta }}}^7\hat{\mathbf{\rho }}+{\hat{\mathbf{\theta }}}^6\hat{\mathbf{\rho }}\hat{\mathbf{\theta }}+{\hat{\mathbf{\theta }}}^5\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^2+{\hat{\mathbf{\theta }}}^4\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^3+{\hat{\mathbf{\theta }}}^3\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^4+{\hat{\mathbf{\theta }}}^2\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^5+\hat{\mathbf{\theta }}\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^6+\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^7)\\ & ⋮\end{array}\text{\hspace{1em}(XI-7)}$$

利用 (XI-4) 代换 (XI-7) 中各项

$$\begin{array}{rl}\mathbf{Q}(\mathbf{\rho },\mathbf{\theta })=& \frac{1}{2}\hat{\mathbf{\rho }}\\ & +\frac{1}{3!}\theta ([\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}+\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times })\\ & +\frac{1}{4!}{\theta }^2([\mathbf{u}{]}_{\times }^2\hat{\mathbf{\rho }}+[\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }+\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^2)\\ & +\frac{1}{5!}{\theta }^3([\mathbf{u}{]}_{\times }^3\hat{\mathbf{\rho }}+[\mathbf{u}{]}_{\times }^2\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }+[\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^2+\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^3)\\ & +\frac{1}{6!}{\theta }^4([\mathbf{u}{]}_{\times }^4\hat{\mathbf{\rho }}+[\mathbf{u}{]}_{\times }^3\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }+[\mathbf{u}{]}_{\times }^2\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^2+[\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^3+\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^4)\\ & +\frac{1}{7!}{\theta }^5([\mathbf{u}{]}_{\times }^5\hat{\mathbf{\rho }}+[\mathbf{u}{]}_{\times }^4\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }+[\mathbf{u}{]}_{\times }^3\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^2+[\mathbf{u}{]}_{\times }^2\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^3+[\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^4+\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^5)\\ & +\frac{1}{8!}{\theta }^6([\mathbf{u}{]}_{\times }^6\hat{\mathbf{\rho }}+[\mathbf{u}{]}_{\times }^5\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }+[\mathbf{u}{]}_{\times }^4\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^2+[\mathbf{u}{]}_{\times }^3\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^3+[\mathbf{u}{]}_{\times }^2\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^4+[\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^5+\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^6)\\ & +\frac{1}{9!}{\theta }^7([\mathbf{u}{]}_{\times }^7\hat{\mathbf{\rho }}+[\mathbf{u}{]}_{\times }^6\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }+[\mathbf{u}{]}_{\times }^5\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^2+[\mathbf{u}{]}_{\times }^4\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^3+[\mathbf{u}{]}_{\times }^3\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^4+[\mathbf{u}{]}_{\times }^2\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^5+[\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^6+\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^7)\\ & ⋮\end{array}$$

利用 (XI-5) 和 (XI-6) 继续代换

$$\begin{array}{rl}\mathbf{Q}(\mathbf{\rho },\mathbf{\theta })=& \frac{1}{2}\hat{\mathbf{\rho }}\\ & +\frac{1}{3!}\theta ([\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}+\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times })\\ & +\frac{1}{4!}{\theta }^2([\mathbf{u}{]}_{\times }^2\hat{\mathbf{\rho }}+[\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }+\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^2)\\ & +\frac{1}{5!}{\theta }^3(-[\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}+[\mathbf{u}{]}_{\times }^2\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }+[\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^2-\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times })\\ & +\frac{1}{6!}{\theta }^4(-[\mathbf{u}{]}_{\times }^2\hat{\mathbf{\rho }}-[\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }-[\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }-[\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }-\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^2)\\ & +\frac{1}{7!}{\theta }^5([\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}-[\mathbf{u}{]}_{\times }^2\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }-[\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^2-[\mathbf{u}{]}_{\times }^2\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }-[\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^2+\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times })\\ & +\frac{1}{8!}{\theta }^6([\mathbf{u}{]}_{\times }^2\hat{\mathbf{\rho }}+[\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }+[\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }+[\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }+[\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }+[\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }+\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^2)\\ & +\frac{1}{9!}{\theta }^7(-[\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}+[\mathbf{u}{]}_{\times }^2\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }+[\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^2+[\mathbf{u}{]}_{\times }^2\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }+[\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^2+[\mathbf{u}{]}_{\times }^2\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }+[\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^2-\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times })\\ & ⋮\end{array}$$

简单合并同类项

$$\begin{array}{rl}\mathbf{Q}(\mathbf{\rho },\mathbf{\theta })=& \frac{1}{2}\hat{\mathbf{\rho }}\\ & +\frac{1}{3!}\theta ([\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}+\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times })\\ & +\frac{1}{4!}{\theta }^2([\mathbf{u}{]}_{\times }^2\hat{\mathbf{\rho }}+[\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }+\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^2)\\ & +\frac{1}{5!}{\theta }^3(-[\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}+[\mathbf{u}{]}_{\times }^2\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }+[\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^2-\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times })\\ & +\frac{1}{6!}{\theta }^4(-[\mathbf{u}{]}_{\times }^2\hat{\mathbf{\rho }}-3[\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }-\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^2)\\ & +\frac{1}{7!}{\theta }^5([\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}-2[\mathbf{u}{]}_{\times }^2\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }-2[\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^2+\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times })\\ & +\frac{1}{8!}{\theta }^6([\mathbf{u}{]}_{\times }^2\hat{\mathbf{\rho }}+5[\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }+\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^2)\\ & +\frac{1}{9!}{\theta }^7(-[\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}+3[\mathbf{u}{]}_{\times }^2\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }+3[\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^2-\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times })\\ & ⋮\end{array}\text{\hspace{1em}(XI-8)}$$
经过这一通计算整理，可以发现 (XI-8) 只剩下为数不多的不同项，所以可以对上式合并同类项
$$\begin{array}{rl}\mathbf{Q}(\mathbf{\rho },\mathbf{\theta })=& \frac{1}{2}\hat{\mathbf{\rho }}\\ & +(\frac{1}{3!}\theta -\frac{1}{5!}{\theta }^3+\frac{1}{7!}{\theta }^5-\frac{1}{9!}{\theta }^7+\dots )([\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}+\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times })\\ & +(\frac{1}{4!}{\theta }^2-\frac{1}{6!}{\theta }^4+\frac{1}{8!}{\theta }^6-\dots )([\mathbf{u}{]}_{\times }^2\hat{\mathbf{\rho }}+\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^2)\\ & +(\frac{1}{4!}{\theta }^2-\frac{3}{6!}{\theta }^4+\frac{5}{8!}{\theta }^6-\dots )([\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times })\\ & +(\frac{1}{5!}{\theta }^3-\frac{2}{7!}{\theta }^5+\frac{3}{9!}{\theta }^7-\dots )([\mathbf{u}{]}_{\times }^2\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }+[\mathbf{u}{]}_{\times }\hat{\mathbf{\rho }}[\mathbf{u}{]}_{\times }^2)\\ \stackrel{(\mathrm{XI}-4)}{=}& \frac{1}{2}\hat{\mathbf{\rho }}\\ & +(\frac{1}{3!}-\frac{1}{5!}{\theta }^2+\frac{1}{7!}{\theta }^4-\frac{1}{9!}{\theta }^6+\dots )(\hat{\mathbf{\theta }}\hat{\mathbf{\rho }}+\hat{\mathbf{\rho }}\hat{\mathbf{\theta }})\\ & +(\frac{1}{4!}-\frac{1}{6!}{\theta }^2+\frac{1}{8!}{\theta }^4-\dots )({\hat{\mathbf{\theta }}}^2\hat{\mathbf{\rho }}+\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^2)\\ & +(\frac{1}{4!}-\frac{3}{6!}{\theta }^2+\frac{5}{8!}{\theta }^4-\dots )(\hat{\mathbf{\theta }}\hat{\mathbf{\rho }}\hat{\mathbf{\theta }})\\ & +(\frac{1}{5!}-\frac{2}{7!}{\theta }^2+\frac{3}{9!}{\theta }^4-\dots )({\hat{\mathbf{\theta }}}^2\hat{\mathbf{\rho }}\hat{\mathbf{\theta }}+\hat{\mathbf{\theta }}\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^2)\end{array}\text{\hspace{1em}(XI-9)}$$
另外已知 Talyor Expansion
$$\sin \theta =\theta -\frac{1}{3!}{\theta }^3+\frac{1}{5!}{\theta }^5-\frac{1}{7!}{\theta }^7+\frac{1}{9!}{\theta }^9-\dots$$

$$\cos \theta =1-\frac{1}{2!}{\theta }^2+\frac{1}{4!}{\theta }^4-\frac{1}{6!}{\theta }^6+\frac{1}{8!}{\theta }^8-\dots$$

反过来对待验证的 eq(180) 进行展开
$$\begin{array}{rl}\frac{\theta -\sin \theta }{{\theta }^3}& =\frac{\theta -(\theta -\frac{1}{3!}{\theta }^3+\frac{1}{5!}{\theta }^5-\frac{1}{7!}{\theta }^7+\frac{1}{9!}{\theta }^9-\dots )}{{\theta }^3}\\ & =\frac{\frac{1}{3!}{\theta }^3-\frac{1}{5!}{\theta }^5+\frac{1}{7!}{\theta }^7-\frac{1}{9!}{\theta }^9+\dots }{{\theta }^3}\\ & =\frac{1}{3!}-\frac{1}{5!}{\theta }^2+\frac{1}{7!}{\theta }^4-\frac{1}{9!}{\theta }^6+\dots \end{array}\text{\hspace{1em}(XI-10)}$$

$$\begin{array}{rl}-\frac{1-\frac{{\theta }^2}{2}-\cos \theta }{{\theta }^4}& =-\frac{1-\frac{1}{2}{\theta }^2-(1-\frac{1}{2!}{\theta }^2+\frac{1}{4!}{\theta }^4-\frac{1}{6!}{\theta }^6+\frac{1}{8!}{\theta }^8-\dots )}{{\theta }^4}\\ & =\frac{\frac{1}{4!}{\theta }^4-\frac{1}{6!}{\theta }^6+\frac{1}{8!}{\theta }^8-\dots }{{\theta }^4}\\ & =\frac{1}{4!}-\frac{1}{6!}{\theta }^2+\frac{1}{8!}{\theta }^4-\dots \end{array}\text{\hspace{1em}(XI-11)}$$

$$\begin{array}{rl}\frac{\theta -\sin \theta }{{\theta }^3}\cdot 1-\frac{1-\frac{{\theta }^2}{2}-\cos \theta }{{\theta }^4}\cdot (-3)& =\frac{3-\frac{1}{2}{\theta }^2-\theta \sin \theta -3\cos \theta }{{\theta }^4}\\ & =\frac{3-\frac{1}{2}{\theta }^2-\theta (\theta -\frac{1}{3!}{\theta }^3+\frac{1}{5!}{\theta }^5-\frac{1}{7!}{\theta }^7+\dots )-3(1-\frac{1}{2!}{\theta }^2+\frac{1}{4!}{\theta }^4-\frac{1}{6!}{\theta }^6+\frac{1}{8!}{\theta }^8-\dots )}{{\theta }^4}\\ & =\frac{3-\frac{1}{2}{\theta }^2-{\theta }^2+\frac{1}{3!}{\theta }^4-\frac{1}{5!}{\theta }^6+\frac{1}{7!}{\theta }^8-\dots -3+\frac{3}{2!}{\theta }^2-\frac{3}{4!}{\theta }^4+\frac{3}{6!}{\theta }^6-\frac{3}{8!}{\theta }^8+\dots )}{{\theta }^4}\\ & =\frac{\frac{1}{4!}{\theta }^4-\frac{3}{6!}{\theta }^6+\frac{5}{8!}{\theta }^8-\dots }{{\theta }^4}\\ & =\frac{1}{4!}-\frac{3}{6!}{\theta }^2+\frac{5}{8!}{\theta }^4-\dots \end{array}\text{\hspace{1em}(XI-12)}$$

$$\begin{array}{rl}-\frac{1}{2}\left(\frac{1-\frac{{\theta }^2}{2}-\cos \theta }{{\theta }^4}-3\frac{\theta -\sin \theta -\frac{{\theta }^3}{6}}{{\theta }^5}\right)& =-\frac{1}{2}\left(-\frac{1}{4!}+\frac{1}{6!}{\theta }^2-\frac{1}{8!}{\theta }^4+\dots -3\frac{-\frac{1}{5!}{\theta }^5+\frac{1}{7!}{\theta }^7-\frac{1}{9!}{\theta }^9+\dots }{{\theta }^5}\right)\\ & =-\frac{1}{2}\left(-\frac{1}{4!}+\frac{1}{6!}{\theta }^2-\frac{1}{8!}{\theta }^4+\dots +\frac{3}{5!}-\frac{3}{7!}{\theta }^2+\frac{3}{9!}{\theta }^4-\dots \right)\\ & =-\frac{1}{2}\left(-\frac{2}{5!}+\frac{4}{7!}{\theta }^2-\frac{6}{9!}{\theta }^4+\dots \right)\\ & =\frac{1}{5!}-\frac{2}{7!}{\theta }^2+\frac{3}{9!}{\theta }^4-\dots \end{array}\text{\hspace{1em}(XI-13)}$$

最后，将 (XI-10) ~ (XI-13) 分别代入 (XI-9) 得到
$$\begin{array}{rl}\mathbf{Q}(\mathbf{\rho },\mathbf{\theta })=& \frac{1}{2}\hat{\mathbf{\rho }}\\ & +(\frac{1}{3!}-\frac{1}{5!}{\theta }^2+\frac{1}{7!}{\theta }^4-\frac{1}{9!}{\theta }^6+\dots )(\hat{\mathbf{\theta }}\hat{\mathbf{\rho }}+\hat{\mathbf{\rho }}\hat{\mathbf{\theta }})\\ & +(\frac{1}{4!}-\frac{1}{6!}{\theta }^2+\frac{1}{8!}{\theta }^4-\dots )({\hat{\mathbf{\theta }}}^2\hat{\mathbf{\rho }}+\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^2)\\ & +(\frac{1}{4!}-\frac{3}{6!}{\theta }^2+\frac{5}{8!}{\theta }^4-\dots )(\hat{\mathbf{\theta }}\hat{\mathbf{\rho }}\hat{\mathbf{\theta }})\\ & +(\frac{1}{5!}-\frac{2}{7!}{\theta }^2+\frac{3}{9!}{\theta }^4-\dots )({\hat{\mathbf{\theta }}}^2\hat{\mathbf{\rho }}\hat{\mathbf{\theta }}+\hat{\mathbf{\theta }}\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^2)\\ =& \frac{1}{2}\hat{\mathbf{\rho }}\\ & +\frac{\theta -\sin \theta }{{\theta }^3}(\hat{\mathbf{\theta }}\hat{\mathbf{\rho }}+\hat{\mathbf{\rho }}\hat{\mathbf{\theta }})\\ & -\frac{1-\frac{{\theta }^2}{2}-\cos \theta }{{\theta }^4}({\hat{\mathbf{\theta }}}^2\hat{\mathbf{\rho }}+\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^2)\\ & +\left[\frac{\theta -\sin \theta }{{\theta }^3}\cdot 1-\frac{1-\frac{{\theta }^2}{2}-\cos \theta }{{\theta }^4}\cdot (-3)\right](\hat{\mathbf{\theta }}\hat{\mathbf{\rho }}\hat{\mathbf{\theta }})\\ & -\frac{1}{2}\left(\frac{1-\frac{{\theta }^2}{2}-\cos \theta }{{\theta }^4}-3\frac{\theta -\sin \theta -\frac{{\theta }^3}{6}}{{\theta }^5}\right)({\hat{\mathbf{\theta }}}^2\hat{\mathbf{\rho }}\hat{\mathbf{\theta }}+\hat{\mathbf{\theta }}\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^2)\\ =& \frac{1}{2}\hat{\mathbf{\rho }}\\ & +\frac{\theta -\sin \theta }{{\theta }^3}(\hat{\mathbf{\theta }}\hat{\mathbf{\rho }}+\hat{\mathbf{\rho }}\hat{\mathbf{\theta }}+\hat{\mathbf{\theta }}\hat{\mathbf{\rho }}\hat{\mathbf{\theta }})\\ & -\frac{1-\frac{{\theta }^2}{2}-\cos \theta }{{\theta }^4}({\hat{\mathbf{\theta }}}^2\hat{\mathbf{\rho }}+\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^2-3\hat{\mathbf{\theta }}\hat{\mathbf{\rho }}\hat{\mathbf{\theta }})\\ & -\frac{1}{2}\left(\frac{1-\frac{{\theta }^2}{2}-\cos \theta }{{\theta }^4}-3\frac{\theta -\sin \theta -\frac{{\theta }^3}{6}}{{\theta }^5}\right)({\hat{\mathbf{\theta }}}^2\hat{\mathbf{\rho }}\hat{\mathbf{\theta }}+\hat{\mathbf{\theta }}\hat{\mathbf{\rho }}{\hat{\mathbf{\theta }}}^2)\end{array}$$
最终完成 eq(180) 闭形式的计算.

— End —

<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