IMU算法：终 | 算法设计实践：采用ESKF的解算

基础知识，其中在Joan Sola - Quaternion kinematics for the error-state Kalman filter中提到了ESKF的实现，这里将按照文中的思路尝试构建一个简单的六轴算法并给出对应的解释

原理与思路

基础思路

ESKF(Error-State Kalman Filter)也是一种Kalman滤波器，基础的内容也类似如何理解Kalman滤波滤波中提到的，本质上与EKF类似，但ESKF最为核心的部分是，状态设定为实际状态的变化值即，根据推导出系统Error-State状态方程观测出系统，最后将此误差值与系统状态融合得到更新后的状态。
对于线性系统而言ESKF实际上与普通的Kalman滤波并无太大区别，但是在非线性的系统中观测比观测非线性程度会进一步减弱，并且后面我们也可以发现，最后一步误差值“加上”系统状态的定义可以是广义的，旋转的误差可以采用乘法（四元数乘法等）融合进实际姿态里，使得观测过程更加精确

另外一点，文中算法定义的旋转的误差，是一个三维的矢量，也就是之前一文中提到的so(3)的向量空间，比起其他的采用四元数直接相加的融合算法少了约束，因此这个整个滤波过程看上去更加像是拥有符合Kalman滤波器所需要的线性

原理

文中对运动速度，位置以及传感器偏置等都作出观测，为了简单起见这里只对观测姿态误差进行观测。文中对于四元数误差和姿态误差做出了两个近似

结合单位四元数的结构来看，当旋转角度极小时就可以得到上述两个式子.根据文中推导的结果，姿态误差的状态转移的连续方程为

实际算法是需要写成离散形式的方程，故需要对方程进行离散化得到ESKF的预测过程。离散化实际上是将状态转移的连续方程进行积分，文中提到了一阶积分，N阶多项式积分，四阶龙格库塔积分以及指数积分。通常情况下采用一阶积分也即

而文中使用指数积分确保精度（即上述连续微分方程中的求解结果，类似状态空间设计基础笔记中提到的零阶保持法）

对于预测过程还有一点是，角速度噪声变换到状态的矩阵是单位阵，而对于四元数的解算却是一个与状态相关的矩阵。原因就是本身是so(3)的向量空间，就是等同于角速度对时间积分，因此的噪声方差就是角速度噪声的方差（直观上的理解，具体解释文中有推导）

对于状态更新过程，其过程与EKF接近一致。但是由于每一次观测过程的本地坐标系都存在变化（这里的是相对于当前姿态的坐标系下），前后的姿态误差并不会累加，因此在更新状态后还会重置使其为0.因此实际算法中的预测过程(1)(2)式计算出的结果总是0，通常会直接跳过计算。下第二式中省略也是这个原因

这里代表的是真实四元数的状态，文中提到的是最优观测值。但是按照Kalman滤波的算法，对于状态更新输出的误差应该是由先验观测的结果计算而来，即对于真实状态的在so(3)上的向量空间进行线性的更新

这里就能够巧妙地将非线性的旋转转换为线性，并用Kalman完成更新。于是将先验观测出的状态代替，即直接用角速度对四元数积分获得。四元数变换至重力加速度方向的函数(四元数作为自变量)表示为

类似EKF，矩阵H也可以通过求一阶偏导数得到

在计算出误差状态后，实际状态的更新只需要将加入姿态四元数后归一化即可。另外，对于ESKF额外的一步是将误差重置，其过程除了将先验的置0外还需要考虑到这样操作对更新出的后验方差的影响，可以通过投影的方法修正，这个方法在之前的文章里也出现过

实现过程

简而言之，ESKF的IMU算法总体上可以用如下表示

预测过程需要计算姿态的先验观测值，以及误差状态的先验方差

其中角速度向量的反对称3×3矩阵。上面提到的采用的是一阶积分近似，如果需要更高精度的积分可以使用通过微分方程求解的指数积分，即
根据测量数据（加速度计）更新误差状态及其后验方差

更新重置误差后的方差，并将误差加上当前姿态并归一化四元数

在实现中还有另外一个问题是参数的给定，上述提到的ESKF的参数相当直观，Q矩阵直接代表了三轴角速度计的噪声方差，而R矩阵直接代表了三轴加速度计的噪声方差。考虑到实际对于信号的处理是离散的，因此两个矩阵可设定两个对角阵

实际平台实现

经过测试可用的代码在Github的项目内，同样基于之前的文章提到的采用的硬件QMI8658C+STM32F302，利用arm math库的矩阵运算进行实时解算。解算速度虽然仔细没有测量，但是目前跑满IMU 2kHz更新频率的解算应该是没有问题的。当然一个更为经济快捷的方式是使用Matlab Coder将Matlab仿真代码直接转为可运行的C/C++代码。

最近验证算法的时候发现，动态调整加速度计的协方差矩阵并不能完全克服高运动下的干扰，具体现象是高运动或震动下的加速度计测量值会使Yaw轴姿态改变。在之前提到的基于四元数的解算内内同样会出现这样的问题。究其原因，还是因为用了所有姿态的四元数作为Kalman滤波器的状态，而在IMU算法：Q | 算法设计实践：基于DCM的解算中则由于状态量设定为重力加速矢量，Yaw轴的积分是在滤波更新完成后才进行。在Kalman滤波器更新误差状态的式子中假设了加速度矢量变化极其微小而使用一阶微分近似，而加速度变化过大的情况下显然线性的更新不能够准确还原误差状态，因此出现Yaw轴被干扰的情况

总结与讨论

ESKF完全解决了Kalman滤波器对于融合非线性姿态的问题，使得整个过程符合Kalman滤波器所要求的线性。当然这里没有做全的是IMU的零偏估计。最主要的原因在于在没有引入其他可以观测到Yaw角度的传感器（如地磁传感器）的观测结果时，仅靠六轴观测IMU角速度计零偏并不完全可靠，在特定姿态下连续旋转极其有可能出现某一个轴的偏置观测值不断增长的问题。引入其他观测量或者限制零偏范围也许能够解决。
至于高运动检测，仍然可以通过根据加速度大小动态调节Q和R矩阵实现，但是过大的加速度也会干扰Yaw轴的解算。

参考

Joan Sola - Quaternion kinematics for the error-state Kalman filter
LEFFERTS E J, MARKLEY F L, SHUSTER M D. Kalman Filtering for Spacecraft Attitude Estimation[J/OL]. Journal of Guidance, Control, and Dynamics, 1982, 5(5): 417-429. DOI:10.2514/3.56190.