为什么PX4的EKF可以让传感器数据稳定

什么是EKF

传感器在成功驱动后，通常会输出相应的数据，但这些原始数据往往含有噪声。噪声的来源多种多样，可能来自物理过程（如机械振动）、化学反应，也可能是由传感器本身的测量精度限制造成的。因此，传感器采集的数据通常不能直接使用。例如，无人机在飞行过程中会依赖惯性测量单元（IMU）的数据，而微机电系统（MEMS）陀螺仪在运行时会不可避免地受到惯性效应、摩擦或温度漂移等因素的影响，导致其在需要精密控制的场景下无法直接使用。为了克服这些问题，我们需要一种能够有效抑制各类自然噪声和人为干扰的方法，这种方法就是滤波。

但很显然滤波器本身并不足以应对所有问题。为了保证系统的鲁棒性，我们需要让传感器具备一定冗余。这样，当某个传感器出现故障时，设备仍可正常工作。于是在实际应用中，通常会引入多种传感器进行互补协同。在多传感器系统中，我们更希望能实现传感器之间的相互校验与纠错，这便催生了多源传感器数据融合方法。

在我的学习过程中，接触到三种简单且实用的多传感器数据融合算法：经典的卡尔曼滤波（Kalman Filter）、适用于非线性系统的扩展卡尔曼滤波（EKF），以及性能强大但计算复杂度较高的无迹卡尔曼滤波（UKF）。这些方法都基于状态转移方程进行迭代更新，属于无偏估计范畴。而目前在PX4飞控系统中广泛使用的EKF2算法，正是基于扩展卡尔曼滤波（EKF）的改进实现。

PX4的EKF2

px4的EKF2使用了ECL EKF库，即包含状态观测器+数据融合+滤波器的复合算法。在进行EKF学习之前我们需要介绍一下卡尔曼滤波器和EKF表达式的区别。

KF的表达式

从卡尔曼的表达式可以看出，这是一种无偏估计，整体上包含先验估计和后验估计两部分，并且需要一个假定的状态方程，所以从数值上看，我们只需要给定初始的Q值和R值，以及整个系统的状态方程就可以进行卡尔曼滤波或者融合了，整个系统的状态方程取决于物理情况，比如匀加速运动就需要让状态转移方程满足牛顿定律，如果是旋转运动就需要让状态转移为旋转矩阵的反对称形式（原因是旋转是非欧运动，无法直接通过线性的矩阵去转移）。

EKF的表达式

从EKF的表达式上看去卡尔曼滤波并没有显著区别，仅是状态转移方程从线性变成了弱的非线性。并且在计算过程中从直接使用常数的状态系数变成当前估计点处动态计算的状态转移函数的雅可比矩阵。