px4: Ecl ekf2学习记录
本文记录px4: Ecl ekf2的学习过程。
预备知识：
标准KF系统：
ekf系统：
预测：
更新：
对于ekf，新增的任务就是计算状态转换矩阵F，控制输入矩阵G和测量矩阵H的Jacobian矩阵。
px4状态估计（ECL）使用扩展卡尔曼滤波器（EKF）算法来进行多传感器融合状态估计。Ecl 利用IMU作状态预测，其它传感器作为观测更新。
Ecl ekf2：定义24状态向量：
四元数定义从北，东，地 (NED) 局部地球坐标系到 X，Y ，Z 机体坐标系的旋转
IMU 的速度 — 北，东，地 (NED)(m/s)
IMU 的位置 — 北，东，地 (NED)(m)
IMU 增量角度偏差估计 —X，Y ，Z(rad)
IMU 增量速度偏差估计 —X，Y ，Z(m/s)
地球磁场分量 — 北，东，地 (NED)(gauss)
飞行器机体坐标系磁场偏差 —X，Y ，Z(gauss)
风速 — 北, 东 (NE)(m/s)
状态更新用的观测传感器：
Ecl ekf2求状态转换矩阵F，控制输入矩阵G和测量矩阵H的Jacobian矩阵，使用python符号求导。因为ekf2涉及旋转的Jacobian矩阵非常复杂，很难手写推导。而与之相对应的eskf算法，由于误差大多在零点附近变化，因此有较好的近线性特征，因此求解Jacobian矩阵会更简单。
下面简单梳理一下px4中，预测、更新流程。
Ecl ekf2代码入口：src/modules/ekf2/EKF2.cpp
Ecl EKF 的核心算法流程是一个事件处理的大循环，其线程被唤醒后就进行事件处理，其入口函数void EKF2::Run()。唤醒事件有各种传感器数据输入，还有一个 10 Hz 的定时事件。当
一段时间内没有任何传感器数据输入时，系统以 10 Hz 频率进行惯性航位推算，并对外报告异常。
算法的处理流程如下 (在不同的版本之间会有一些差异)：
void EKF2::Run()
1.核对参数更新
2.Ecl还支持多传感器冗余设计（如：双IMU，双磁力计），运行多个24-EKF实例，通过比较每个 24-EKF 实例的内部一致性，EKF 选择器能够确定具有最佳数据一致性的 24-EKF 和传感器组合。这样可以检测和隔离 IMU 偏差、饱和或数据卡顿等故障，通过标志位_multi_mode进行设置。
3._ekf.update()
predictState();
predictCovariance();
controlFusionModes();
(a) 预测新的状态 (24-EKF 方程 1 )。
(b) 预测新的协方差矩阵 (24-EKF 方程 2 )。
(c) 受控制的观测数据的融合 (24-EKF 方程 3, 4, 5 )。
i. 检查倾斜对准并在对准后选择高度数据源。
ii. 检查 baro/GPS/MAG/range/flow/EV/airspeed 等传感器的可用情况。
iii. 检查并在必要时切换高度数据源 (baro/EV/GPS/range)。
iv. 运行验证偏航角 ψ 估计器 (EKF-GSF)。
v. 磁力融合。
vi. 光流融合。
vii. GPS 融合。
viii. 空速融合。
ix. beta 融合。
x. drag 融合。
xi. 高度融合。
xii. 外部视觉融合。
xiii. 辅助速度融合。
xiv. 零新息航向角更新。当飞行器处于静止状态时，航向角的新息很小，需要设置特别参
数，以便估计器能正常运行。
xv. 零速度更新。当飞行器处于静止状态时，速度的新息很小，需要设置特别参数，以便
估计器能正常运行。
xvi. 虚拟位置融合。
xvii. 虚拟高度融合。
xviii. 更新航位推算状态，检查我们是否不再融合直接约束速度漂移的测量。
2. 运行地形高差估计器 (1-KF)。
runTerrainEstimator();
3. 运行输出观测器，计算当前时间范围的输出补偿 (100 Hz)。
calculateOutputStates()
_ekf.update()完成后，将融合结合发布出去。
此处只是进行简单的代码流程梳理，具体各个传感器细节可参考如下链接，或留言讨论。
参考：https://github.com/shuyong/study-notes-math/blob/master/kalman/ESKF/12/notes
附：eskf一点思考
1.eskf与ekf的区别
ekf在处理状态变量中的旋转是最大的问题，因为表示旋转的旋转矩阵或者四元数都不满足加减法，如计算协方差矩阵：![E\left [ (x-{\hat{x}})(x-{\hat{x}})^{T} \right ]](http://img.e-com-net.com/image/info8/446c65b04fae428dae9dbeee176a1ca1.png){kind=small}
，
表示啥意思？所以严格从数学上是存在问题的。所以为了解决这个问题，eskf将ekf中定义的旋转状态量q，转变为误差状态，在组合导航中也称作为失准角，它是一个三个元数表示的矢量。
用误差矢量从数学上看，好像也没解决计算协方差矩阵问题，但有三个好处：
a.误差状态量通常变化比较缓慢，每次更新量都被更新到名义状态monimal-state上，所以在进行更新的过程中，是可以低于预测执行频率的，这点很重要，有利于多传感器融合。
b.误差状态error-state量一般很小，可以保证泰勒展开的二阶部分直接忽略，使得计算雅克比矩阵非常简单快速。这一点可从px4 ekf中求雅克比矩阵看出巨大区别。
c.误差状态error-state量一般很小，趋近于0，可以避免一些可能出现的奇点存在。
2.ESKF常用设计流程
通用形式以IMU作为预测，所以预测方程不变，不同的观测，意味着观测方程不同，两个步骤解决，1.定义观测方程形式，2.求解观测量对状态量的雅克比矩阵（简单的线性关系，可直接求解，复杂用链式求导法则）。
所以根据观测测量值不同可如下分类：
1.通用eskf，以位置+速度为观测量，（IMU+GPS）观测方程对观测量的雅克比矩阵简单，线性关系。
2.+编码器，增加机体系速度，观测方程推导出机体系观测速度与状态量之间的关系，即可推导出观测量对于状态量的雅克比矩阵。
3.运动约束，如零速更新，车体侧向速度和垂直方向速度为0，即可认为机体侧向速度和垂直方向速度可观测，于是机体系观测速度与状态量之间的关系只需取2中这两维即可。
4.+激光雷达，由于激光雷达本身可以提供位姿，于是出现两种融合方案，松耦合、紧耦合
松耦合：将激光雷达计算的位姿作为观测量融合，融合方式同上。
紧耦合：提取激光雷达的线面特征，作为观测量，进行融合，由于观测量为线面特征量，所以重点在于定义a.定义观测方程形式，b.求解观测量对状态量的雅克比矩阵（简单的线性关系，可直接求解，复杂用链式求导法则）。
紧耦合通常定义线特征和面特残差，所以只需计算出线面特征对状态量的雅克比矩阵即可。
如LINS(需要注意的是，这里定义的状态变量，pvqbabg不是世界坐标系位姿，而是前后两帧之间的位姿)，这里求雅克比矩阵通常采用链式求导法则，通常复杂一点（也是唯一区别、难点），类似工作还有fastLio。