流形上的运动学 - Notes for “Kalman Filter on Differentiable Manifolds“ - IKFoM (I)

题目：流形上的运动学 - Notes for “Kalman Filter on Differentiable Manifolds” - IKFoM (I)

---

---

前言

Dongjiao He, Wei Xu, Fu Zhang 的论文 “Kalman Filter on Differentiable Manifolds”^[1] 中构建的 IKFoM (“流形上的迭代 Kalman 滤波”) 的系统架构比较复杂. 另外, 为了通用化的缘故, 同时在欧几里得空间、$SO(3)$、${\mathbb{S}}^2$ 上推导, 增加了论文阅读的难度. 尝试在论文原作者简练的推导过程上, 以自己的理解进行详细和啰嗦地推导, 作为自己学习理解的记录.

本文是涉及到论文 “Kalman Filter on Differentiable Manifolds”^[1] 的 “IV. CANONICAL REPRESENTATION OF ON-MANIFOLD SYSTEMS” 部分的阅读笔记. 主要关于

- $SO(3)$ 上的姿态运动学

- ${\mathbb{S}}^2$ 上重力向量运动学

- ${\mathbb{S}}^2$ 上方位向量运动学

本文是相关主题的第一篇, 后一篇请参考
流形上的复合函数偏导数 - Notes for “Kalman Filter on Differentiable Manifolds“ - IKFoM (II)

---

I. About eq (12) in “Example 2: Attitude kinematics in a global reference frame”

下面是关于流形上的姿态运动学. 这里对符号进行了简单替换, 以使得表示更直观和符合习惯.

1. 姿态矩阵的导数 Time Derivative of an Attitude Matrix

文献 [2] 中对姿态矩阵的导数有进行推导, 是本小节的参考来源.

运动物体相对于全局坐标系 {G} 的姿态 —— ${}_B^G\mathbf{R}$ (原文 $\mathbf{x}$) —— 运动物体的姿态在全局坐标系下的描述

运动物体在全局坐标系下的角速度 —— ${}^G\mathbf{\omega }$ —— 在采样期间 $\Delta t$ 内保持不变

假设运动物体上任一固定点 $P$ 在运动物体局部坐标系 {B} 上的坐标为 ${}^B\mathbf{P}$, 则在全局坐标系下的坐标为
$${}^G\mathbf{P}={}_B^G\mathbf{R}{}^B\mathbf{P}\text{\hspace{1em}(1-1)}$$
因为 ${}^B\mathbf{P}$ 在局部坐标系 {B} 上固定, 故
$${}^B\dot{\mathbf{P}}=0\text{\hspace{1em}(1-2)}$$
对式 (1-1) 两边求导得到
$${}^G\dot{\mathbf{P}}={}_B^G\dot{\mathbf{R}}{}^B\mathbf{P}+{}_B^G\mathbf{R}{}^B\dot{\mathbf{P}}={}_B^G\dot{\mathbf{R}}{}^B\mathbf{P}\text{\hspace{1em}(1-3)}$$
又由全局坐标系下角速度和线速度的关系
$${}^G\dot{\mathbf{P}}={}^G\mathbf{\omega }\times {}^G\mathbf{P}=[{}^G\mathbf{\omega }{]}_{\times }{}^G\mathbf{P}\stackrel{(1-1)}{=}[{}^G\mathbf{\omega }{]}_{\times }{}_B^G\mathbf{R}{}^B\mathbf{P}\text{\hspace{1em}(1-4)}$$
结合式 (I-3) 和 (I-4) 得
$${}_B^G\dot{\mathbf{R}}{}^B\mathbf{P}=[{}^G\mathbf{\omega }{]}_{\times }{}_B^G\mathbf{R}{}^B\mathbf{P}\text{\hspace{1em}(1-5)}$$
因为 ${}^B\mathbf{P}$ 是任意的, 故要使式 (I-5) 成立需要满足
$${}_B^G\dot{\mathbf{R}}=[{}^G\mathbf{\omega }{]}_{\times }{}_B^G\mathbf{R}\text{\hspace{1em}(1-6)}$$
即为原文中
$$\dot{\mathbf{x}}={\lfloor }^G\mathbf{\omega }\rfloor \cdot \mathbf{x}$$

2. 离散化 Zero-Order Hold Discretization

由式 (1-6) 离散化得到
$$\begin{array}{rl}\frac{{}_B^G{\mathbf{R}}_{k+1}-{}_B^G{\mathbf{R}}_k}{\Delta t}& =[{{}^G\mathbf{\omega }}_k{]}_{\times }{{}_B^G\mathbf{R}}_k\\ \Rightarrow {}_B^G{\mathbf{R}}_{k+1}& ={}_B^G{\mathbf{R}}_k+\Delta t[{{}^G\mathbf{\omega }}_k{]}_{\times }{{}_B^G\mathbf{R}}_k\\ \Rightarrow {}_B^G{\mathbf{R}}_{k+1}& =\left(\mathbf{I}+\Delta t[{{}^G\mathbf{\omega }}_k{]}_{\times }\right){{}_B^G\mathbf{R}}_k\end{array}\text{\hspace{1em}(1-7)}$$
由 $\mathrm{Exp}(\cdot )$ 的泰勒展开
$$\mathrm{Exp}(\Delta t{}^G{\mathbf{\omega }}_k)=\exp (\Delta t[{}^G{\mathbf{\omega }}_k{]}_{\times })=\sum _{k=0}^{\infty }\frac{{\Delta t}^k}{k!}[{}^G{\mathbf{\omega }}_k{]}_{\times }^k\text{\hspace{1em}(1-8)}$$
取一阶泰勒展开为
$$\mathrm{Exp}(\Delta t{}^G{\mathbf{\omega }}_k)\approx \mathbf{I}+\Delta t[{}^G{\mathbf{\omega }}_k{]}_{\times }\text{\hspace{1em}(1-9)}$$
则式 (1-7) 继续变换为
$${}_B^G{\mathbf{R}}_{k+1}=\mathrm{Exp}(\Delta t{}^G{\mathbf{\omega }}_k){{}_B^G\mathbf{R}}_k\text{\hspace{1em}(1-10)}$$
即原文中
$${\mathbf{x}}_{k+1}=\mathrm{Exp}(\Delta t{}^G{\mathbf{\omega }}_k)\cdot {\mathbf{x}}_k$$

3. 指数映射的变换 Transformation of a Exponential Map

由 “机器人中常用矩阵等式-I (汇总)” 中 Identity 11^[3], 可知
$$[{\mathbf{R}}_k^{\mathrm{T}}{}^G{\mathbf{\omega }}_k{]}_{\times }={\mathbf{R}}_k^{\mathrm{T}}[{}^G{\mathbf{\omega }}_k{]}_{\times }{\mathbf{R}}_k\text{\hspace{1em}(1-11)}$$
由旋转矩阵的正交特性可知
$$[{\mathbf{R}}_k^{\mathrm{T}}{}^G{\mathbf{\omega }}_k{]}_{\times }^m={\mathbf{R}}_k^{\mathrm{T}}[{}^G{\mathbf{\omega }}_k{]}_{\times }^m{\mathbf{R}}_k(m=0,1,2,\cdots )\text{\hspace{1em}(1-12)}$$
则指数映射有
$$\begin{array}{rl}\mathrm{Exp}\left(\Delta t({\mathbf{R}}_k^{\mathrm{T}}{}^G{\mathbf{\omega }}_k)\right)& =\exp \left(\Delta t[{\mathbf{R}}_k^{\mathrm{T}}{}^G{\mathbf{\omega }}_k{]}_{\times }\right)\\ & =\sum _{k=0}^{\infty }\frac{{\Delta t}^k}{k!}[{\mathbf{R}}_k^{\mathrm{T}}{}^G{\mathbf{\omega }}_k{]}_{\times }^k\\ & ={\mathbf{R}}_k^{\mathrm{T}}\left(\sum _{k=0}^{\infty }\frac{{\Delta t}^k}{k!}[{}^G{\mathbf{\omega }}_k{]}_{\times }^k\right){\mathbf{R}}_k\\ & ={\mathbf{R}}_k^{\mathrm{T}}\mathrm{Exp}(\Delta t{}^G{\mathbf{\omega }}_k){\mathbf{R}}_k\end{array}\text{\hspace{1em}(1-13)}$$
两边同时左乘以 $R_k$ 得到
$${\mathbf{R}}_k\mathrm{Exp}\left(\Delta t({\mathbf{R}}_k^{\mathrm{T}}{}^G{\mathbf{\omega }}_k)\right)=\mathrm{Exp}(\Delta t{}^G{\mathbf{\omega }}_k){\mathbf{R}}_k\text{\hspace{1em}(1-14)}$$
即原文中
$${\mathbf{x}}_{k+1}=\mathrm{Exp}(\Delta t{}^G{\mathbf{\omega }}_k)\cdot {\mathbf{x}}_k={\mathbf{x}}_k\mathrm{Exp}\left(\Delta t({\mathbf{x}}_k^{\mathrm{T}}\cdot {}^G{\mathbf{\omega }}_k)\right)$$
这样完整得到了原文中式 (12).

---

II. About eq (13) in “Example 3: Attitude kinematics in body frame”

物体相对于全局坐标系运动的角速度在全局坐标系 {G} 中的描述记为 ${}^G\mathbf{\omega }$;

物体相对于全局坐标系运动的角速度在运动物体局部坐标系 {B} 中的描述记为 ${}^B\mathbf{\omega }$.

两者相互之间的转换关系为
$${}^G\mathbf{\omega }={}_B^G\mathbf{R}{}^B\mathbf{\omega }\text{\hspace{1em}(2-1)}$$

1. 姿态矩阵的导数 Time Derivative of an Attitude Matrix

将式 (2-1) 代入式 (1-6) , 再结合式 (1-11) 得到
$$\begin{array}{rl}{}_B^G\dot{\mathbf{R}}& =[{}^G\mathbf{\omega }{]}_{\times }{}_B^G\mathbf{R}\\ (2-1)& =[{}_B^G\mathbf{R}{}^B\mathbf{\omega }{]}_{\times }{}_B^G\mathbf{R}\\ (1-11)& ={}_B^G\mathbf{R}[{}^B\mathbf{\omega }{]}_{\times }{{}_B^G\mathbf{R}}^{\mathrm{T}}{}_B^G\mathbf{R}\\ & ={}_B^G\mathbf{R}[{}^B\mathbf{\omega }{]}_{\times }\end{array}\text{\hspace{1em}(2-2)}$$
即原文中的
$$\dot{\mathbf{x}}=\mathbf{x}\cdot \lfloor {}^B\mathbf{\omega }\rfloor$$

2. 离散化 Zero-Order Hold Discretization

由式 (2-2) 做离散化可知
$$\begin{array}{rl}\frac{{}_B^G{\mathbf{R}}_{k+1}-{}_B^G{\mathbf{R}}_k}{\Delta t}& ={}_B^G{\mathbf{R}}_k[{}^B{\mathbf{\omega }}_k{]}_{\times }\\ \Rightarrow {{}_B^G\mathbf{R}}_{k+1}& ={}_B^G{\mathbf{R}}_k+{}_B^G{\mathbf{R}}_k\Delta t[{}^B{\mathbf{\omega }}_k{]}_{\times }\\ \Rightarrow {{}_B^G\mathbf{R}}_{k+1}& ={}_B^G{\mathbf{R}}_k\left(\mathbf{I}+\Delta t[{{}^B\mathbf{\omega }}_k{]}_{\times }\right)\\ (\text{1st-Order Taylor Approximation})\Rightarrow {{}_B^G\mathbf{R}}_{k+1}& ={}_B^G{\mathbf{R}}_k\exp (\Delta t[{{}^B\mathbf{\omega }}_k{]}_{\times })\\ \Rightarrow {{}_B^G\mathbf{R}}_{k+1}& ={}_B^G{\mathbf{R}}_k\mathrm{Exp}(\Delta t{{}^B\mathbf{\omega }}_k)\end{array}\text{\hspace{1em}(2-3)}$$
即原文中
$${\mathbf{x}}_{k+1}={\mathbf{x}}_k\cdot \mathrm{Exp}(\Delta t{{}^B\mathbf{\omega }}_k)$$
这样就得到了原文中式 (13).

题外话一: right-$\oplus$ 和 left-$\oplus$

文献 [4] 分别针对相对坐标系下的切向量扰动和全局坐标系下的切向量扰动定义了 right-$\oplus$、right-$\ominus$、left-$\oplus$、left-$\ominus$.

The right operators are
$$\begin{array}{rl}\text{right-}\oplus & :\mathcal{Y}=\mathcal{X}\oplus {}^{\mathcal{X}}\tau \triangleq \mathcal{X}\circ \mathrm{Exp}({}^{\mathcal{X}}\tau )\in \mathcal{M}\\ \text{right-}\ominus & :{}^{\mathcal{X}}\tau =\mathcal{Y}\ominus \mathcal{X}\triangleq \mathrm{Log}({\mathcal{X}}^{-1}\circ \mathcal{Y})\in {\mathrm{T}}_{\mathcal{X}}\mathcal{M}\end{array}$$
where ${}^{\mathcal{X}}\tau$ belongs to the tangent space at $\mathcal{X}$. That is, ${}^{\mathcal{X}}\tau$ is expressed in the local frame at $\mathcal{X}$.

The left operators are
$$\begin{array}{rl}\text{left-}\oplus & :\mathcal{Y}={}^{\mathbf{\epsilon }}\tau \oplus \mathcal{X}\triangleq \mathrm{Exp}({}^{\mathbf{\epsilon }}\tau )\circ \mathcal{X}\in \mathcal{M}\\ \text{left-}\ominus & :{}^{\mathbf{ϵ}}\tau =\mathcal{Y}\ominus \mathcal{X}\triangleq \mathrm{Log}(\mathcal{Y}\circ {\mathcal{X}}^{-1})\in {\mathrm{T}}_{\mathbf{\epsilon }}\mathcal{M}\end{array}$$
We say that ${}^{\mathbf{\epsilon }}\tau$ is expressed in the global frame.

式 (1-10) 中的 ${}^G{\mathbf{\omega }}_k$ 是全局坐标系下的角速度 (角速度向量相对于姿态矩阵, 视作为扰动), 故而是 left-$\oplus$.

式 (2-3) 中的 ${{}^B\mathbf{\omega }}_k$ 是运动物体局部坐标系下的角速度, 故而是 right-$\oplus$.

翻来覆去, 这些定义是自洽的.

---

III. About eq (15) in “Example 5: Vectors of known magnitude in body frame”

1. 姿态矩阵的导数 Time Derivative of an Attitude Matrix

重力向量在全局坐标系 {G} 下的描述记为 ${}^G\mathbf{g}\in {\mathbb{S}}^2(\parallel \mathbf{g}\parallel )$. 重力向量在局部坐标系 {B} 下的描述记为 ${}^B\mathbf{g}$ (原文中记为 $\mathbf{x}$). 两者之间的坐标转换关系
$${}^G\mathbf{g}={}_B^G\mathbf{R}{}^B\mathbf{g}\text{\hspace{1em}(3-1)}$$
重力向量在全局坐标系下是不变的, 即
$${}^G\dot{\mathbf{g}}=0\text{\hspace{1em}(3-2)}$$
对式 (3-1) 两边求导可得
$${}_B^G\dot{\mathbf{R}}{}^B\mathbf{g}+{}_B^G\mathbf{R}{}^B\dot{\mathbf{g}}=0\text{\hspace{1em}(3-3)}$$
式 (2-2) 代入式 (3-3) 得
$${}_B^G\mathbf{R}[{}^B\mathbf{\omega }{]}_{\times }{}^B\mathbf{g}+{}_B^G\mathbf{R}{}^B\dot{\mathbf{g}}=0\text{\hspace{1em}(3-4)}$$
两边同乘以 ${}_B^G{\mathbf{R}}^{\mathrm{T}}$ 可得
$${}^B\dot{\mathbf{g}}=-[{}^B\mathbf{\omega }{]}_{\times }{}^B\mathbf{g}\text{\hspace{1em}(3-5)}$$
即原文中的
$$\dot{\mathbf{x}}=-{\lfloor }^B\mathbf{\omega }\rfloor \mathbf{x}$$

2. 离散化 Zero-Order Hold Discretization

对式 (3-5) 离散化得到
$$\begin{array}{rl}\frac{{}^B{\mathbf{g}}_{k+1}-{}^B{\mathbf{g}}_k}{\Delta t}& =-[{}^B{\mathbf{\omega }}_k{]}_{\times }{}^B{\mathbf{g}}_k\\ \Rightarrow {}^B{\mathbf{g}}_{k+1}& ={}^B{\mathbf{g}}_k-\Delta t[{}^B{\mathbf{\omega }}_k{]}_{\times }{}^B{\mathbf{g}}_k\\ \Rightarrow {}^B{\mathbf{g}}_{k+1}& =\left(\mathbf{I}-\Delta t[{}^B{\mathbf{\omega }}_k{]}_{\times }\right){}^B{\mathbf{g}}_k\\ (\text{1st-Order Taylor Approximation})\Rightarrow {}^B{\mathbf{g}}_{k+1}& =\mathrm{Exp}(-\Delta t{}^B{\mathbf{\omega }}_k){}^B{\mathbf{g}}_k\end{array}\text{\hspace{1em}(3-6)}$$
即原文中的
$${\mathbf{x}}_{k+1}=\mathrm{Exp}(-\Delta t{}^B{\mathbf{\omega }}_k){\mathbf{x}}_k$$
这样就得到了原文中式 (15).

---

IV. About eqs (16)~(17) in “Example 6: Bearing-distance parameterization of visual landmarks”

视觉特征点 (Visual landmark) 在相机坐标系 {C} 中的方位向量 (Bearing vector) 记作 $\mathbf{x}\in {\mathbb{S}}^2(1)$ (即 $\mathbf{x}\in {\mathbb{R}}^3$ 且 $|\mathbf{x}|=1$);

特征点在相机坐标系 {C} 中的深度 (Depth) 记作 $d(\rho )\in \mathbb{R}$, 为基于参数 $\rho \in \mathbb{R}$ 的标量.

相机在全局坐标系 {G} 中的姿态矩阵记作 ${}_C^G\mathbf{R}$ (原文中记作 ${}^G{\mathbf{R}}_C$);

相机在全局坐标系 {G} 中的位置向量记作 ${}_C^G\mathbf{P}$ (原文中记作 ${}^G{\mathbf{P}}_C$).

Fig 1. 方位向量 Bearing vector

1. 方位向量的性质 Properties of a Bearing Vector

根据 Fig 1 所示, 特征点在相机坐标系 {C} 中的方位向量
$$\mathbf{x}=\left[\begin{array}{c}\sin \varphi \cos \theta \\ \sin \varphi \sin \theta \\ \cos \varphi \end{array}\right]\text{\hspace{1em}(4-1)}$$
对方位向量求时间导数, 得到
$$\dot{\mathbf{x}}=\left[\begin{array}{c}\cos \varphi \cos \theta \dot{\varphi }-\sin \varphi \sin \theta \dot{\theta }\\ \cos \varphi \sin \theta \dot{\varphi }+\sin \varphi \cos \theta \dot{\theta }\\ -\sin \varphi \dot{\varphi }\end{array}\right]\text{\hspace{1em}(4-2)}$$
计算
$${\mathbf{x}}^{\mathrm{T}}\dot{\mathbf{x}}=\left[\begin{array}{ccc}\sin \varphi \cos \theta & \sin \varphi \sin \theta & \cos \varphi \end{array}\right]\left[\begin{array}{c}\cos \varphi \cos \theta \dot{\varphi }-\sin \varphi \sin \theta \dot{\theta }\\ \cos \varphi \sin \theta \dot{\varphi }+\sin \varphi \cos \theta \dot{\theta }\\ -\sin \varphi \dot{\varphi }\end{array}\right]=0\text{\hspace{1em}(4-3)}$$

另外根据 机器人中常用矩阵等式-I (汇总) 中 “[Identity 12] Identity About Skew-Symmetric Matrix” 可知
$${\mathbf{x}}^{\mathrm{T}}[{}^C\mathbf{\omega }{]}_{\times }\mathbf{x}=0\text{\hspace{1em}(4-4)}$$

2. 深度参数的导数 Time Derivative of a Depth Parameter

特征点在全局坐标系中的位置为 ${}_C^G\mathbf{R}(\mathbf{x}d(\rho ))+{}_C^G\mathbf{P}$. 为固定于全局坐标系中的几何点, 所以位置坐标对时间求导为零, 即
$$\begin{array}{rl}\frac{d({}_C^G\mathbf{R}(\mathbf{x}d(\rho ))+{}_C^G\mathbf{P})}{dt}& =0\\ \Rightarrow {}_C^G\dot{\mathbf{R}}(\mathbf{x}d(\rho ))+{}_C^G\mathbf{R}(\dot{\mathbf{x}}d(\rho ))+{}_C^G\mathbf{R}(\mathbf{x}{d}^{\prime }(\rho )\dot{\rho })+{}_C^G\dot{\mathbf{P}}& =0\end{array}\text{\hspace{1em}(4-5)}$$
由式 (2-2) 可知
$${}_C^G\dot{\mathbf{R}}={}_C^G\mathbf{R}[{}^C\mathbf{\omega }{]}_{\times }\text{\hspace{1em}(4-6)}$$
式 (4-6) 代入式 (4-5) 得到
$$\underline{{}_C^G\mathbf{R}[{}^C\mathbf{\omega }{]}_{\times }}\mathbf{x}d(\rho )+{}_C^G\mathbf{R}\dot{\mathbf{x}}d(\rho )+{}_C^G\mathbf{R}\mathbf{x}{d}^{\prime }(\rho )\dot{\rho }+{}_C^G\dot{\mathbf{P}}=0\text{\hspace{1em}(4-7)}$$
左右两边都左乘 ${{}_C^G\mathbf{R}}^{\mathrm{T}}$, 并利用姿态矩阵的正交性, 得到
$$[{}^C\mathbf{\omega }{]}_{\times }\mathbf{x}d(\rho )+\dot{\mathbf{x}}d(\rho )+\mathbf{x}{d}^{\prime }(\rho )\dot{\rho }+{{}_C^G\mathbf{R}}^{\mathrm{T}}{}_C^G\dot{\mathbf{P}}=0\text{\hspace{1em}(4-8)}$$
定义相机相对于全局坐标系 {G} 的运动速度向量在相机坐标系 {C} 中进行描述所得的速度向量记为
$${}^C\mathbf{v}\triangleq {}_G^C\mathbf{R}{}_C^G\dot{\mathbf{P}}={{}_C^G\mathbf{R}}^{\mathrm{T}}{}_C^G\dot{\mathbf{P}}\text{\hspace{1em}(4-9)}$$
式 (4-9) 代入式 (4-8)
$$[{}^C\mathbf{\omega }{]}_{\times }\mathbf{x}d(\rho )+\dot{\mathbf{x}}d(\rho )+\mathbf{x}{d}^{\prime }(\rho )\dot{\rho }+{}^C\mathbf{v}=0\text{\hspace{1em}(4-10)}$$
式 (4-10) 两边左乘 ${\mathbf{x}}^{\mathrm{T}}$ 得到标量方程
$${\mathbf{x}}^{\mathrm{T}}[{}^C\mathbf{\omega }{]}_{\times }\mathbf{x}d(\rho )+{\mathbf{x}}^{\mathrm{T}}\dot{\mathbf{x}}d(\rho )+{\mathbf{x}}^{\mathrm{T}}\mathbf{x}{d}^{\prime }(\rho )\dot{\rho }+{\mathbf{x}}^{\mathrm{T}}{}^C\mathbf{v}=0\text{\hspace{1em}(4-11)}$$
由式 (4-3) 及式 (4-4) 可知式 (4-11) 的第一、二项为零, 以及${\mathbf{x}}^{\mathrm{T}}\mathbf{x}=1$ 可知
$$d^{\prime }(\rho )\dot{\rho }+{\mathbf{x}}^{\mathrm{T}}{}^C\mathbf{v}=0\text{\hspace{1em}(4-12)}$$
最后得到
$$\dot{\rho }=-{\mathbf{x}}^{\mathrm{T}}{}^C\mathbf{v}/d^{\prime }(\rho )\text{\hspace{1em}(4-13)}$$

3. 方位向量的时间导数 Time Derivative of a Bearing Vector

将式 (4-13) 代入式 (4-10) 得到
$$\begin{array}{r}{\left[{}^C\mathbf{\omega }\right]}_{\times }\mathbf{x}d(\rho )+\dot{\mathbf{x}}d(\rho )-\mathbf{x}{\mathbf{x}}^{\mathrm{T}}{}^C\mathbf{v}+{}^C\mathbf{v}=0\\ \Rightarrow \dot{\mathbf{x}}=-{\left[{}^C\mathbf{\omega }\right]}_{\times }\mathbf{x}+\frac{1}{d(\rho )}\left(\mathbf{x}{\mathbf{x}}^{\mathrm{T}}-\mathbf{I}\right){}^C\mathbf{v}\end{array}\text{\hspace{1em}(4-14)}$$
其中
$$\begin{array}{rl}& \left(\mathbf{x}{\mathbf{x}}^{\mathrm{T}}-\mathbf{I}\right){}^C\mathbf{v}\\ ([\mathbf{x}{]}_{\times }^2=\mathbf{x}{\mathbf{x}}^{\mathrm{T}}-\mathbf{I})=& [\mathbf{x}{]}_{\times }^2{}^C\mathbf{v}\\ \text{Associative law of multiplication of matrices}=& [\mathbf{x}{]}_{\times }\left([\mathbf{x}{]}_{\times }{}^C\mathbf{v}\right)\\ ([\mathbf{x}{]}_{\times }\mathbf{y}=-[\mathbf{y}{]}_{\times }\mathbf{x})=& -{\left[[\mathbf{x}{]}_{\times }{}^C\mathbf{v}\right]}_{\times }\mathbf{x}\end{array}\text{\hspace{1em}(4-15)}$$

式 (4-15) 代入式 (4-14) 得到
$$\begin{array}{rl}\dot{\mathbf{x}}& =-{\left[{}^C\mathbf{\omega }\right]}_{\times }\mathbf{x}-\frac{1}{d(\rho )}{\left[[\mathbf{x}{]}_{\times }{}^C\mathbf{v}\right]}_{\times }\mathbf{x}\\ & =-\left({\left[{}^C\mathbf{\omega }\right]}_{\times }+\frac{1}{d(\rho )}{\left[[\mathbf{x}{]}_{\times }{}^C\mathbf{v}\right]}_{\times }\right)\mathbf{x}\\ & =-{\left[{}^C\mathbf{\omega }+\frac{1}{d(\rho )}[\mathbf{x}{]}_{\times }{}^C\mathbf{v}\right]}_{\times }\mathbf{x}\end{array}\text{\hspace{1em}(4-16)}$$
推到得到和原文中 (17) 一致的式子.

4. 离散化 Zero-Order Hold Discretization

先简化式 (4-16)
$$\mathbf{\Omega }\triangleq {}^C\mathbf{\omega }+\frac{1}{d(\rho )}[\mathbf{x}{]}_{\times }{}^C\mathbf{v}\text{\hspace{1em}(4-17)}$$
则式 (4-16) 简写为
$$\dot{\mathbf{x}}=-{\left[\mathbf{\Omega }\right]}_{\times }\mathbf{x}\text{\hspace{1em}(4-18)}$$
对式 (4-18) 离散化得到
$$\begin{array}{rl}\frac{{\mathbf{x}}_{k+1}-{\mathbf{x}}_k}{\Delta t}& =-[{\mathbf{\Omega }}_k{]}_{\times }{\mathbf{x}}_k\\ \Rightarrow {\mathbf{x}}_{k+1}& ={\mathbf{x}}_k-\Delta t[{\mathbf{\Omega }}_k{]}_{\times }{\mathbf{x}}_k\\ \Rightarrow {\mathbf{x}}_{k+1}& =\left(\mathbf{I}-\Delta t[{\mathbf{\Omega }}_k{]}_{\times }\right){\mathbf{x}}_k\\ (\text{1st-Order Taylor Approximation})\Rightarrow {\mathbf{x}}_{k+1}& =\mathrm{Exp}(-\Delta t{\mathbf{\Omega }}_k){\mathbf{x}}_k\end{array}\text{\hspace{1em}(4-19)}$$
式 (4-17) 代入式 (4-19) 得到
$${\mathbf{x}}_{k+1}=\mathrm{Exp}\left(-\Delta t\left({}^C{\mathbf{\omega }}_k+\frac{1}{d(\rho )}[{\mathbf{x}}_k{]}_{\times }{}^C{\mathbf{v}}_k\right)\right){\mathbf{x}}_k\text{\hspace{1em}(4-20)}$$
和原文中 (17) 一致.

题外话二: ${\mathbb{S}}^2$ is not a Lie group 三维空间的球面 (2-Sphere) 不是李群

文献 [5] 中说明了 ${\mathbb{S}}^2$ 不是李群, 也就是上面提到的重力向量、方位向量都不是李群. 这里鹦鹉学舌讲一下大概原理.

毛球定理 (Hairy ball theorem): 在三维空间中球面 ${\mathbb{S}}^2$ 上不存在处处连续的非零并且与球面相切的向量场. 比如一个长满毛圆球, 把毛都贴在圆球表面理顺的话, 必然有 “漩”. 再比如, 地球表面永远存在气旋和风眼, 在风眼处风平浪静, 但四周都有风环绕.

而李群上都存在一个处处连续且非零的切向量场. 这就和毛球定理矛盾, 故而三维空间的球面 (2-Sphere) 不是李群.

因为不是李群, 所以重力向量相关的式 (3-6) 和方位向量相关的式 (4-20) 与标准 right-$\oplus$ 和 left-$\oplus$ 形式有所不同.

---

总结

本文主要是阅读论文 “Kalman Filters on Differentiable Manifolds”^[1] 过程中, 对

- $SO(3)$ 上的姿态运动学

- ${\mathbb{S}}^2$ 上重力向量的运动学

- ${\mathbb{S}}^2$ 上方位向量的运动学

进行的详细啰嗦版推导记录.

---

参考文献

[1] Dongjiao He, Wei Xu, Fu Zhang, “Kalman Filters on Differentiable Manifolds”, https://arxiv.org/abs/2102.03804, 2021, Version V3

[2] Shiyu Zhao, “Time Derivative of Rotation Matrices: A Tutorial”, https://arxiv.org/abs/1609.06088, 2016

[3] wzf@robotics_notes, “机器人中常用矩阵等式-I (汇总)”, https://blog.csdn.net/woyaomaishu2/article/details/132351402?spm=1001.2014.3001.5502

[4] Joan Solà, Jeremie Deray, Dinesh Atchuthan, “A micro Lie theory for state estimation in robotics”, https://arxiv.org/abs/1812.01537, 2021, Version v9

[5] Seewoo Lee, “$S^2$ is not a Lie group”, https://math.berkeley.edu/~seewoo5/S2notLiegrp.pdf#:~:text=Since%20%28S2%29%20%3D%202%2C%20it,can%27t%20admit%20a%20Lie%20group%20structure, 2018