状态估计中的李群：理论基础

引入

这篇文章主要是记录看完(1)之后的笔记。在之前的提到的各种姿态估计的算法中，姿态的非线性是一个非常难处理的问题。试图用Kalman滤波器等线性方法估计状态只是一种近似的手段，IEKF也算是另外一种手段。
但上述这些方法都或多或少的有些奇怪，因为观测的状态本身总是不符合Kalman滤波器所要求的线性，而我们希望设定的姿态能满足可加和的性质。实际上在误差状态Kalman中就已经提到了一些关于流形(Manifolds)上的操作，比较专业一些的算法例如FAST-LIO设定的状态也是在流形上的。不过目前正是因为缺少这部分的基础知识，看不明白这些论文的方法，于是找了(1)(2)(3)几本书补一下。
文(2)(3)覆盖内容比较全面一些，而文(1)主要覆盖的是工程运用里面所用到的基础知识，比较适合速成，这里也主要是围绕(1)展开。

基本概念

流形(Manifold)：状态降维的抽象概念，表征高维数据中的低维结构，可以理解为为带约束的高维空间
李群(Lie Group)：一个光滑（可微分）的流形,且其元素满足如下性质：
$组合的封闭性恒等变换存在幺元，逆变换结合律$
切空间(Tangent space)：给定流形上随移动一点，其导数所构成的空间。可以理解为与流形相切的平面构成的空间，是流形在某一点处线性化的向量空间。流形上一点的切空间记作
李代数(Lie algebra)：流形在其幺元(Identity)处的切空间，可以理解为流形在其“原点”处线性化的向量空间。流形的李代数可记作

需要注意的是，流形的组合操作类似矩阵乘法，一般情况并不满足交换律。
实际上李群的概念并不是具体指代一种集合，而是说符合上述性质的一类抽象的集合，具体在现实中可能有很多种符合上述条件的集合。这些集合上很多操作都是非线性的，例如最常见的姿态表示的空间中旋转的组合都是采用乘法叠加的，这就为许多线性的信号处理的方法带来了麻烦。而在流形切空间上进行姿态的线性处理是一个非常合适的解决方法。虽然如扩展Kalman也可以直接对当前姿态状态求Jacobian近似线性化，但是流形切空间的优势在于由于流形上每一点总是相似的，其切空间也是相近的，这样其非线性程度远小于直接线性化。

上图(1)说明流形、李代数以及实数向量空间之间的关系。李代数的元素可以与一个实数向量空间的元素一一对应。从实向量空间至是通过基元素的线性组合完成的，这个操作记为Hat()，其逆变换记为Vee()
由于流形的元素组合操作我们一般认为类似“乘法”的操作，而切空间是一种线性空间，其组合我们一般认为是“加法”，因此从直观上看流形与其切空间的映射可以说是指数/对数映射关系（把一个空间上的乘法操作映射为另一个空间上的加法操作），可记为exp/log（流形直接映射实数向量空间的操作记为Exp/Log）。下面几乎大部分的运算都是将流形元素转换为线性的切空间上进行操作。
若即李代数为流形的李代数，则，上述映射关系可以写为

流形元素与切空间元素流形右加/右减操作
流形元素与切空间元素左加/左减操作

用于操作流形元素与切空间元素两者间的关系，可以理解为流形的“加”是用一个流形的元素与其上的一个微小变化（切空间的元素）组合，而流形的“减”则是计算流形的微小差别并投射在切空间上。左和右操作一般不等价，右加/右减操作的切空间是位于处的，也即我们常说的本地坐标系(Local frame)。而左加/左减的操作对应的切空间是位于幺元（也即原点处），可以理解为我们说的全局坐标系(Global frame)。
左减和右减的操作在写法上存在歧义，由于目前左减用的比较多，(1)中无特殊说明均认为是左减。

流形元素的伴随(Adjoint)：定义如下操作为流形元素在处的伴随

其伴随满足如下性质
$线性同态$
伴随矩阵：伴随操作的矩阵形式。由于伴随满足线性性质，这种变换可以写为一个矩阵形式的线性变换
根据上面提到的左加和右加可以发现，伴随是将本地坐标系下的切空间转换为全局坐标系下的变换，即。同时它也是一种线性变换，这也是后面各种线性操作的基础。

李群微积分

李群元素的右Jacobian矩阵
无穷小时其一阶近似可以写为

李群元素的左Jacobian矩阵
无穷小时其一阶近似可以写为

李群元素的Jacobian实际上相当于是对从一个流形映射到另外一个流形的函数的“导数”，与矢量的Jacobian矩阵类似，区别仅在于矢量的加减换为流形上的，以及右加/左加的区别。右Jacobian矩阵相当于是在本地坐标系下的求导，对应本地的切空间(local tangent space)，而左Jacobian相当于是在全局坐标系下的求导，对应全局切空间(global tangent space)。这个Jacobian矩阵有点像是上面提到的伴随矩阵，是在流形的切空间上的一种线性变换。或者说是描述和两者分别对应的切空间之间的变换关系。

如果加入上述伴随矩阵的关系，函数和对应的流形的导数关系如下图所示

具体的数学表述如下，其中第一条式子表示的是left jacobian和right jacobian之间的关系，对应上图中的是实线的两条路径。而后两条则是交叉jacobian矩阵的计算，对应上图中的两条虚线及其包括实线在内的不同路径。

微分链式法则

为了方便书写，以下导数矩阵将简写为
若,即有，微分链式法则可表示为

这里的微分链式法则与标量的基本一致，不过需要注意的是导数相乘的次序。

令，为本地坐标系下的微小扰动，满足

协方差矩阵
全局坐标系下的协方差
方差传播

协方差的计算在后面工程应用中如Kalman滤波器里也会涉及到，这里实际上是给出其类似线性状态量下的方差计算，但其微小变化定义在流形的切空间上。而其全局坐标系与本地坐标系的转换，以及方差通过函数的传播均可通过近似线性的投影完成，这个一点与EKF中的方差计算类似

离散时间积分：

应用实例

常见Jacobian元素

逆
组合
流形的左/右Jacobian
这里的Jacobian是用于表示流形上微小差异的点对应的切空间之间的线性映射关系，从数学表达式上来讲是把的变化映射到切空间上。同样地，右Jacobian是在本地坐标系下的关系而左Jacobian是在全局坐标系下的关系。(1)里的写法有些不太好理解，实际上其Jacobian的定义应该是这样的：

另外一点是，(1)里面的(68)式到(69)式，以及(72)式到(73)式的过程写法上比较有迷惑性。流形左/右Jacobian的存在的意义源于在为矢量的情况下，不一定等于(推导见Baker-Campbell-Hausdorff公式以及(3)中的解释)，而(1)这里的写法一开始也让我误解了两者是相等的。合理一些的的表达应该是交换(68)式的左右两边以及交换(72)式的左右两边，或者注明令。

流形的左/右Jacobian满足的一些性质如下：

对数映射，操作的Jacobian

常见李群与其实现

(1)中的上表列举了一些常见的旋转群，实际上工程里面常用的是特征正交群SO(3)与特殊欧式群SE(3)，分别代表了三维空间中的旋转（或者说是姿态）和三维空间中的刚体运动（同时包括旋转和平移）。对于只描述姿态的李群SO(3)，其切空间是一个三维的矢量空间，这个矢量一个非常直观的理解就是轴角定义下的矢量，即矢量大小代表旋转角度而其方向代表旋转轴。姿态的具体表示有四元数和旋转矩阵两种形式，其对应的李群指数/对数映射分别表示为

同时描述姿态和位置的刚体运动群SE(3)则在旋转矩阵的基础上增加了表示位置的三维矢量，其元素可以用旋转矩阵R和三维位置矢量t表示为一个4*4的矩阵

其李代数和切空间分别表示为

其对应的李群指数/对数映射分别表示为

算法举例

基于SE(2)的ESKF
这个例子描述了一个二维平面内的姿态和位置的估计问题，给定的输入的机器人旋转角速度，沿纵向方向的速度以及若干个地标坐标点位置，传感器测量机器人本地坐标系下各个地标的位置。系统在时刻的状态记为，控制输入为，每个地标测量结果记为，则系统方程为

构造一个Kalman滤波器来观测，即设定误差状态. 文中的误差状态Kalman(ESKF)中的误差指的是对状态更新部分的误差，实际上只是把姿态的融合变为流形上的，本质上与扩展Kalman并无区别，我认为其原因是强调误差状态的Kalman应该是因为作者认为只有线性的部分才被归入Kalman滤波器的范畴。当然比起(2)里面对ESKF的解释以及推导一大堆误差状态方程，这里的推导就十分简单。写出系统的方程后按照EKF的构造方法，首先写出Kalman的预测过程

这里的矩阵可以根据上述求Jacobian的方式轻松获得

同理更新过程也类似EKF，但是这里引入了误差状态，并且存在多个地标点，每个地标的坐标都需要按照如下更新过程进行更新

这里的矩阵同样是求Jacobian获得

参考

(1) Joan Sola,Jeremie Deray,Dinesh Atchuthan - A micro Lie theory for state estimation in robotics
(2) Joan Sola - Quaternion kinematics for the error-state Kalman filter
(3) Timothy D. Barfoot- STATE ESTIMATION FOR ROBOTICS(Second Edition),中文译本