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

本文同步更新与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)$的定义为

$$SO(3)=\{R\in {\mathbb{R}}^{3\times 3}|R^{T}R=I,det(R)=1\}$$

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

$${\dot{R}}^{T}R+R^T\dot{R}=0$$

由于要求幺元处的切空间，令上式中的$R=I$，得

$${\dot{R}}^T+\dot{R}=0$$

即$\dot{R}$是一个斜对称矩阵，有如下形式

$$\dot{R}=\left(\begin{array}{ccc}0& -z& y\\ z& 0& -x\\ -y& x& 0\end{array}\right)$$

这就是$SO(3)$幺元处的切空间所具有的形式。以上斜对称矩阵实际只有3个维度，我们定义一个映射

$${\left[\begin{array}{c}x\\ y\\ z\end{array}\right]}_{\times }=\left(\begin{array}{ccc}0& -z& y\\ z& 0& -x\\ -y& x& 0\end{array}\right)$$

则矢量${\xi }_R=(x,y,z{)}^T$就是我们常说的轴角。矢量的方向代表旋转的轴，矢量的模长代表要旋转的角度。

$SE(3)$与$S{E}_{k+1}(3)$的切空间

有了$SO(3)$的切空间后，$SE(3)$与$S{E}_{k+1}(3)$的切空间就很好求了。幺元处的切空间实际上是幺元在流形上发生微小摄动产生的增量，比如对$SO(3)$而言，幺元$I$在流形上的微小摄动产生$I+\Lambda ({\xi }_R)$，那么平移量$t$本身属于向量空间，微小的摄动仍然是一个三维矢量。因此$SE(3)$切空间的形式就是

$$\dot{T}=\left(\begin{array}{cc}[{\xi }_R{]}_{\times }& {\xi }_t\\ 0& 0\end{array}\right)$$

其中${\xi }_R,{\xi }_t\in {\mathbb{R}}^3$。

而$S{E}_{k+1}(3)$的切空间自然就是

$$\dot{\chi }=\left(\begin{array}{ccccc}[{\xi }_R{]}_{\times }& {\xi }_t& {\xi }_{p_1}& ...& {\xi }_{p_k}\\ 0& 0& 0& ...& 0\\ ⋮& & & & \\ 0& 0& 0& ...& 0\end{array}\right)$$

上面的表达是很冗余的，我们也可以像$SO(3)$那样定义一个映射

$$\Lambda \left(\begin{array}{c}{\xi }_R\\ {\xi }_t\\ {\xi }_{p_1}\\ ⋮\\ {\xi }_{p_k}\end{array}\right)=\left(\begin{array}{ccccc}[{\xi }_R{]}_{\times }& {\xi }_t& {\xi }_{p_1}& ...& {\xi }_{p_k}\\ 0& 0& 0& ...& 0\\ ⋮& & & & \\ 0& 0& 0& ...& 0\end{array}\right)$$

我们可以发现，所有这些群的切空间都是向量空间，即满足矢量的线性运算（对加法和数乘封闭），实际上，它们还对另外一种运算封闭，被称为李括号，定义为$[X,Y]=XY-YX$，其中$X,Y$是群的切空间的矩阵形式。并可以验证有如下关系成立

$$\begin{array}{rl}[X,Y]+[Y,X]& =0\\ [X,[Y,Z]]+[Y,[Z,X]]+[Z,[X,Y]]& =0\end{array}$$

如果将李括号也视为一种乘法（和矩阵一样不满足交换律），那么切空间就同时对加法运算和乘法运算封闭，因此构成了一个环（Ring）！所以，这样的切空间也被称为李代数。$SO(3),SE(3),S{E}_{k+1}(3)$的李代数分别用$so(3),se(3),s{e}_{k+1}(3)$表示。

指数映射

先对指数函数做泰勒展开，有

$$e^x=1+x+\frac{1}{2!}{x}^2+...+\frac{1}{n!}{x}^n+...$$

将其中的$x$替换为某个矩阵$A$，可得到

$$e^A=I+A+\frac{1}{2!}{A}^2+...+\frac{1}{n!}{A}^n+...$$

可证明上式对任意$n\times n$实矩阵都是收敛的。

上式构成了指数映射的基础。当矩阵$A$属于某个李群的李代数时，比如$A\in so(3)$，指数映射将其映射到对应的李群上，即$e^A\in SO(3)$。还记得李代数是李群在幺元处的切空间，也就是说，通过指数映射，整个李群完全可以通过幺元处的切空间得到！（实际上，指数映射一般只能将李代数映射到李群的一部分上，但对于$SO(n),SE(n),S{E}_{k+1}(n)$而言，能够映射到全部，即任何一个属于这些群的元素，一定能在对应李代数中找到一个对应元素）。下面分别给出$SO(n),SE(n),S{E}_{k+1}(n)$指数映射的解析形式。

$so(3)$到$SO(3)$

为了能够利用李代数的矢量表示（即轴角），我们一般定义其指数映射为

$$\text{Exp}({\xi }_{\mathbf{R}})=\exp ([{\xi }_{\mathbf{R}}{]}_{\times })$$

设${\xi }_{\mathbf{R}}=\theta \mathbf{u}$，其中$\theta$是一个标量，代表旋转的角度，$\mathbf{u}$是单位矢量，表示旋转的轴。则利用指数函数的泰勒展开式，有

$$\text{Exp}({\xi }_{\mathbf{R}})=\mathbf{I}+\theta [\mathbf{u}{]}_{\times }+\frac{1}{2!}{\theta }^2[\mathbf{u}{]}_{\times }^2+...+\frac{1}{\mathbf{n}!}{\theta }^{\mathbf{n}}[\mathbf{u}{]}_{\times }^{\mathbf{n}}+...$$

注意到

$$\begin{array}{rl}[\mathbf{u}{]}_{\times }^2& =\mathbf{u}{\mathbf{u}}^{\mathbf{T}}-\mathbf{I}\\ [\mathbf{u}{]}_{\times }^3& =-[\mathbf{u}{]}_{\times }\end{array}$$

有

$$\begin{array}{rl}[\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 }\\ ...& \end{array}$$

于是我们对展开式分离奇偶项，可得

$$\text{Exp}({\xi }_{\mathbf{R}})=\mathbf{I}+(\theta -\frac{1}{3!}{\theta }^3+\frac{1}{5!}{\theta }^5-...)[\mathbf{u}{]}_{\times }+(\frac{1}{2!}{\theta }^2-\frac{1}{4!}{\theta }^4+...)[\mathbf{u}{]}_{\times }^2$$

又注意到

$$\begin{array}{rl}\sin \theta & =\theta -\frac{1}{3!}{\theta }^3+\frac{1}{5!}{\theta }^5-...\\ \cos \theta & =1-\frac{1}{2!}{\theta }^2+\frac{1}{4!}{\theta }^4-...\end{array}$$

最终可得

$$\text{Exp}({\xi }_{\mathbf{R}})=\mathbf{I}+\sin \theta [\mathbf{u}{]}_{\times }+(1-\cos \theta )[\mathbf{u}{]}_{\times }^2$$

这就是众所周知的罗德里格斯变换。

$se(3)$到$SE(3)$

设$\xi =({\xi }_R^T,{\xi }_t^T{)}^T$。同样定义

$$\text{Exp}(\xi )=\exp (\Lambda (\xi ))$$

推导方式与处理$SO(3)$时完全一样，首先计算泰勒级数，可以得到

$$\text{Exp}(\xi )=\left(\begin{array}{cc}\sum \frac{1}{n!}{\theta }^n[\mathbf{u}{]}_{\times }^{\mathbf{n}}& (\sum \frac{1}{n!}{\theta }^{n-1}[\mathbf{u}{]}_{\times }^{\mathbf{n}-1}){\xi }_{\mathbf{t}}\\ 0& 1\end{array}\right)$$

矩阵中左上角元素就是$so(3)$的指数映射，而另外一项，同样利用三角函数极数的对照关系，可以得到

$$\text{Exp}(\xi )=\left(\begin{array}{cc}R& V{\xi }_t\\ 0& 1\end{array}\right)$$

其中

$$\begin{array}{rl}R& =I+\sin \theta [\mathbf{u}{]}_{\times }+(1-\cos \theta )[\mathbf{u}{]}_{\times }^2\\ V& =I+\frac{1-\cos \theta }{\theta }[\mathbf{u}{]}_{\times }+\frac{\theta -\sin \theta }{\theta }[\mathbf{u}{]}_{\times }^2\end{array}$$

$s{e}_{k+1}(3)$到$S{E}_{k+1}(3)$

简单计算即可发现，$s{e}_{k+1}(3)$的指数映射就是$se(3)$的简单扩展，这里直接给出结果。设$\xi =({\xi }_R^T,{\xi }_t^T,{\xi }_{p_1}^T,...,{\xi }_{p_k}^T{)}^T$，则

$$\text{Exp}(\xi )=\left(\begin{array}{ccccc}R& V{\xi }_t& V{\xi }_{p_1}& ...& V{\xi }_{p_k}\\ 0& 1& 0& ...& 0\\ 0& 0& 1& ...& 0\\ ⋮& & & & \\ 0& 0& 0& ...& 1\end{array}\right)$$

其中

$$\begin{array}{rl}R& =I+\sin \theta [\mathbf{u}{]}_{\times }+(1-\cos \theta )[\mathbf{u}{]}_{\times }^2\\ V& =I+\frac{1-\cos \theta }{\theta }[\mathbf{u}{]}_{\times }+\frac{\theta -\sin \theta }{\theta }[\mathbf{u}{]}_{\times }^2\end{array}$$

一些指数映射的其他性质

当两个矩阵满足交换律时，即$AB=BA$时，有

$$e^A{e}^B=e^{A+B}$$

指数映射的行列式与矩阵的迹还有如下一个美妙关系

$$det(e^A)=e^{tr(A)}$$

群伴随

这里省去很多关于伴随的导出过程，直接给出其定义。

$SO(3)$的伴随

设$R\in SO(3),\xi \in so(3)$，则在$R$处的伴随记为$A{d}_R$，定义为

$$R\cdot \text{Exp}(\xi )=\text{Exp}(A{d}_R\cdot \xi )\cdot R$$

下面给出$A{d}_R$的具体形式。由上式可得

$$\begin{array}{rl}\text{Exp}(A{d}_R\cdot \xi )& =R\cdot \text{Exp}(\xi )\cdot R^{-1}\\ & =R(\sum _n^{\infty }\frac{1}{n!}[\xi {]}_{\times }^n)R^{-1}\\ & =\sum _n^{\infty }\frac{1}{n!}(R[\xi {]}_{\times }{R}^{-1}{)}^n\\ & =\exp (R[\xi {]}_{\times }{R}^{-1})\\ & =\exp ([R\xi {]}_{\times })\\ & =\text{Exp}(R\xi )\end{array}$$

所以有

$$A{d}_R=R$$

关于$R[\xi {]}_{\times }{R}^{-1}=[R\xi {]}_{\times }$的证明可见Cross Product一文。

$SE(3)$与$S{E}_{k+1}(3)$的伴随

推导方式与$SO(3)$类似，设$T\in SE(3)$，即

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

又设$\xi =({\xi }_R^T,{\xi }_t^T{)}^T\in se(3)$，有

$$\begin{array}{rl}\text{Exp}(A{d}_T\cdot \xi )& =\exp (T\Lambda (\xi )T^{-1})\\ & =\exp \left(\begin{array}{cc}R[{\xi }_R{]}_{\times }{R}^{-1}& -R[{\xi }_R{]}_{\times }{R}^{-1}t+R{\xi }_t\\ 0& 0\end{array}\right)\\ & =\exp \left(\begin{array}{cc}[R{\xi }_R{]}_{\times }& -[R{\xi }_R{]}_{\times }t+R{\xi }_t\\ 0& 0\end{array}\right)\\ & =\exp \left(\begin{array}{cc}[R{\xi }_R{]}_{\times }& [t{]}_{\times }R{\xi }_R+R{\xi }_t\\ 0& 0\end{array}\right)\\ & =\exp \left(\Lambda \right[\left(\begin{array}{cc}R& 0\\ [t{]}_{\times }R& R\end{array}\right)\left(\begin{array}{c}{\xi }_R\\ {\xi }_t\end{array}\right)\left]\right)\\ & =\text{Exp}\left(\left(\begin{array}{cc}R& 0\\ [t{]}_{\times }R& R\end{array}\right)\left(\begin{array}{c}{\xi }_R\\ {\xi }_t\end{array}\right)\right)\end{array}$$

因此有

$$A{d}_T=\left(\begin{array}{cc}R& 0\\ [t{]}_{\times }R& R\end{array}\right)$$

简单计算可以发现$S{E}_{k+1}(3)$的伴随也是$SE(3)$的扩展，设$\chi \in S{E}_{k+1}(3)$，有

$$A{d}_{\chi }=\left(\begin{array}{cccccc}R& 0& 0& 0& ...& 0\\ [t{]}_{\times }R& R& 0& 0& ...& 0\\ [p_1{]}_{\times }R& 0& R& 0& ...& 0\\ [p_2{]}_{\times }R& 0& 0& R& ...& 0\\ ⋮& & & & & \\ [p_k{]}_{\times }R& 0& 0& 0& ...& R\end{array}\right)$$

References

1. Lie Groups for 2D and 3D Transformations
2. Quaternion kinematics for the error-state Kalman filter
3. Naive Lie Theory by John Stillwell
4. The Invariant Extended Kalman Filter as a Stable Observer
5. An EKF-SLAM algorithm with consistency property