ESKF(error-state Kalman Filter)

Reference

首先确定存在系统方程：xk=f(xk−1,uk,wk)
以及观测方程：yk=h(xk,vk)
其中，x表示系统状态，y表示观测量，w表示系统传递噪声，v表示观测误差。
beiyesi

表示形式
Q=a+bi+cj+dk
q = q w + qv
q=[qw qv]
四元数的乘法运算，可以写作矩阵计算的形式
通过四元数的微分方程 q ˙ = q ⊗ Ω \dot q=q \otimes \Omega q˙=q⊗Ω可以看到，四元数与SO(3)一样，可以写作exp映射的形式，并且，两者的旋转向量是一样的，不过对于旋转矩阵的映射而言，首先会将旋转向量映射到so(3)上，之后SO(3)与so(3)通过exp来一一对应，而四元数是将旋转向量先映射到切线空间之后再与四元数进行exp映射

其中SO3是指三维特殊正交群，so3是指三维特殊正交群的李代数
支持李群和李代数运算的库文件是Sophs,支持SO(3),so(3),SE(3)和se(3)的定义和运算，继承于Eigen，所以可以使用Eigen定义的类和函数。

三维旋转群SO3：Sophus::SO3
刚体变换群SE3：Sophus::SE3
李代数so(3)：Sophus::Vector3d so3
李代数se(3)：Sophus::Vector6d se3

Sophus库转换关系图

这里稍微解释一下误差方程，由于在做IMU的解算时，做了一些近似，而那些舍掉的东西就给IMU解算带来了误差。ESKF要做的就是对误差进行滤波，获得当前情况下误差的最优估计，然后将计算出来的位移、速度、姿态等量减去这个误差，修正由于近似带来的不准确。