为什么四元数要用ESKF？什么时候用四元数什么时候用李代数？一篇解决所有问题

首先讲一下基础：

Q1:为什么四元数要用ESKF？？

首先KF不能用四元数。这里稍微多解释一下，KF要求运动方程是线性的，观测方程线性和非线性无所谓，一般情况下观测方程都是非线性的，所以要用泰勒展开，求雅克比矩阵来做一个近似；而EKF连运动方程都不要求是线性，所以还要求一个运动方程的雅克比矩阵，如下图所示：

因为四元数肯定是非线性的，所以不能直接用KF。实际上可以用EKF，但是以四元数为状态变量的话其雅克比矩阵也比较复杂（有没有用四元数为状态量进行EKF的呢？其实是有的，是在无人机里常出现的，直接用全量而不是误差量进行建模的情况，比如直接用姿态的四元数做状态量）

最常用的是ESKF的形式。为什么要用errorState呢，errorState有很多优点，但我觉得最重要的是误差状态方程是近似线性的，所以可以直接用卡尔曼滤波（注意，eskf并不是EKF，而是KF）,而且测量更新部分的雅克比矩阵的计算也非常简单计算，因为errorState总是很小的，所以高阶项可以忽略。

Q1:什么时候用李代数，什么时候用四元数？

1、非线性优化问题

一般使用李代数会好一些。这是因为优化问题最终就是求H，从而也就是要求雅克比矩阵J。在李代数下，J的扰动求导是很方便且现成的，而用四元数求导比较麻烦，在计算精度上李代数也更高。一般优化得到增量的估计（李代数）之后，先指数映射转到李群，然后在李群上做乘法。

为什么用四元数？

通常SLAM优化问题里解出来的增量是旋转向量  是一个R3

这不是真正意义的李代数  实际上还有一个做反对称的步骤  但是是一一对应的

而转到四元数的话更简单  除以2然后做三角函数运算就好了

那为什么不用旋转向量（以及其所代表的李代数） 而用四元数？

是因为，

1、 旋转向量虽然是最小表示，但真正在做向量旋转时候还是要用旋转矩阵，所以还是要把旋转向量通过罗德里格斯公式变为旋转矩阵（或者用指数映射转成李群），而四元数有现成的运算；

2. 求得增量之后四元数的更新也比李群表示的更新方便快捷很多，同时四元数仅仅存四个数就可以，比存储李群那样的矩阵要方便一些；

3. 从便于理解的角度来说，我们最容易感受的其实是欧拉角，而不是旋转向量表示的给你一个旋转轴和旋转角度，那么从这个角度出发，四元数依旧是直接的公式，而旋转向量依旧需要一步罗德里格斯公式；

个人认为两者皆可表示旋转，只不过四元数有更多现成的运算，不必像旋转向量（或者楼主所说的李代数）一样处处罗德里格斯

另一个解读：

2. 四元数无法直接用卡尔曼滤波器做最优估计。于是有聪明人想到了姿态的误差，可以用旋转向量表示，这是一个向量，可以用卡尔曼滤波器做最优估计。然后将误差向量转换回四元数再乘回姿态四元数，这样消除误差，姿态四元数就是最优估计。

为什么四元数不能直接用卡尔曼滤波？ 按这个说法 eskf里的角度误差是旋转向量来表示的 也就是3维 这和那个代码实现是统一的，对这个旋转向量做最优估计，然后转换成四元数，然后再做四元数乘法

卡尔曼滤波是线性系统下的，但是四元数表示的状态模型是非线性的，可以用EKF，但是用四元数做状态变量的话需要求一个计算复杂的雅可比矩阵，在实际的高维问题中不实际

3. 近些年来流行的情况是，用李群重新推导上一步的公式。这是SLAM软件中流行的做法，因为有一位做SLAM的博士为自己的毕设写了一份很好的教材。于是用李群的方法就流行开来。

4. 用李群的方法可以得到更高阶的计算精度，但是更吃算力。但是在SLAM世界里，这点算力要求比起后面算法的要求来说是九牛一毛。而传统的姿态估计，是要求在MCU上完成计算。

5. 不过精度这个问题，要从源头算起。如果传感器本身的数据精度就不咋的，用传统方法也是可以的。噪声本来就很大，何必算这么辛苦，把算力让给其它算法不好么。于是就有提问者说的，有些SLAM软件还是使用四元数做姿态估计。

所以，用四元数和用李群肯定是等价的，只不过用李群的话就是用扰动来求导

一般优化用李代数  正常的旋转表示用四元数比较多。对于优化问题，最终要求H，也就是要求雅克比J，在李代数下，J的扰动求导是很方便的，而四元数的求导比较麻烦（或许）；而在滤波问题下，首先KF肯定不能用四元数，因为四元数非线性，用EKF的话四元数的状态变量也需要雅克比比较复杂。而用ESKF的话，一方面es是零均值的高斯分布，KF工作在原点附近，保证了线性化的合理性和有效性，而且es总是很小，所以高阶项可以忽略，雅克比矩阵的计算非常简单迅速

所以我们得到了最终为什么四元数要用eskf，无论以四元数为状态变量来做ekf，还是以四元数的误差为状态量来做eskf，都需要计算雅克比矩阵，对于ekf，这个雅克比的计算非常复杂，而对于以误差为状态量的eskf，因为状态量在0附近，所以可以省略高阶项，使得雅克比矩阵的计算非常简单和快速