不变扩展卡尔曼滤波（一）：李群与李代数

本文同步更新与github pages，欢迎收藏关注。

群的定义

设有一个集合$G$，集合中的元素为$g$，元素之间存在乘法运算$\cdot$，称$G$是一个群如果其满足以下条件：

• 封闭性 $\forall g_1,g_2\in G,g_1\cdot g_2\in G$
• 结合律 $g_1\cdot (g_2\cdot g_3)=(g_1\cdot g_2)\cdot g_3$
• 存在幺元 $\forall g\in G,e\cdot g=g\cdot e=g$
• 存在逆元 $\forall g\in G,\exists g^{-1}\in G,g\cdot g^{-1}=g^{-1}\cdot g=e$

SLAM中常用的群

只考虑实数域。所有$n\times n$的正交矩阵$R$构成了一个群，称为正交群，记为$O(n)$，在这些正交矩阵中，行列式为正1的矩阵本身也构成一个群，称为特殊正交群，记为$SO(n)$，也就是说$SO(n)$是$O(n)$的一个子群（准确的说是正规子群）。

在SLAM以及很多机器人技术中，最常用到的就是$SO(3)$，也就是所有三维旋转矩阵构成的群，可以用来表示机器人或者飞行器的姿态。

当需要描述一个刚体变换（旋转加平移）时，光靠一个旋转矩阵是不够的，当我们使用齐次坐标表示空间中的点时，对该点的刚体变换可用下面的$4\times 4$矩阵表示

$$T=\left(\begin{array}{cc}R& t\\ 0& 1\end{array}\right)$$

其中$R\in {\mathbb{R}}^{3\times 3}$是旋转矩阵，$t\in {\mathbb{R}}^3$是平移矢量。容易验证，矩阵$T$也是满足构成群的四个条件的，其中逆元为

$$T^{-1}=\left(\begin{array}{cc}{R}^T& -R^{T}t\\ 0& 1\end{array}\right)$$

幺元就是单位阵，因此所有刚体变换矩阵也构成一个群，称为特殊欧式群，记为$SE(3)$。当然也存在$SE(n)$，看做是任意维度下的刚体变换。如果不要求$T$里面的$R$必须是一个旋转矩阵，甚至不要求是正交矩阵，只要可逆就行，那么我们就得到了更一般的仿射群，在这就不深入讨论了。

为了方便之后介绍不变卡尔曼滤波，这里对$SE(3)$做一个扩展，我们发现型如

$$\chi =\left(\begin{array}{cccccc}R& t& p_1& p_2& ...& p_k\\ 0& 1& 0& 0& ...& 0\\ 0& 0& 1& 0& ...& 0\\ 0& 0& 0& 1& ...& 0\\ ⋮& & & & & \\ 0& 0& 0& 0& ...& 1\end{array}\right)$$

的矩阵，大小为$(4+k)\times (4+k)$，$p_k\in {\mathbb{R}}^3$，也满足群定义，逆元为

$${\chi }^{-1}=\left(\begin{array}{cccccc}{R}^T& -R^{T}t& -R^T{p}_1& -R^T{p}_2& ...& -R^T{p}_k\\ 0& 1& 0& 0& ...& 0\\ 0& 0& 1& 0& ...& 0\\ 0& 0& 0& 1& ...& 0\\ ⋮& & & & & \\ 0& 0& 0& 0& ...& 1\end{array}\right)$$

幺元也是单位阵，我们将这种矩阵构成的群记为$S{E}_{k+1}(3)$。

切空间与李代数

$SO(3)$和$SE(3)$这类群，还有一个特点是平滑连续，即他们是一个平滑的流形，这类群称为李群。在流形上微分可以得到流形上某一点的切空间（类比于对曲线微分得到切线）。其中在幺元处的切空间最重要（与后面的指数映射有关）。下面分别来求一下$SO(3)$，$SE(3)$，以及$S{E}_k(3)$在幺元处的切空间。

$SO(3)$的切空间

具体写出$SO(3)$的定义为

S O ( 3 ) = { R ∈ R 3 × 3 ∣ R T R = I , d e t ( R ) = 1 } SO(3)=\{R\in\mathbb{R}^{3\times 3}|R^TR=I, det(R)=1\} SO(3)={ R∈R3×3∣RTR=I,det(R)=1}

现在假设$R$是一个随时间变化的量$R(t)$，则$R(t{)}^{T}R(t)=I$两边对时间求导，并令$\partial R/\partial$