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

什么是EKF

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

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

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

PX4的EKF2

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

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

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

首先我们介绍一下数据源的单位和传感器，正如下表所示，下面是飞控所使用到的所有传感器和对应的单位、符号，这些传感器将会被EKF2采用。在初始状态下，EKF会尝试检查传感器的最小可行组合，也就是可以满足自身收敛的情况，在保证初始倾斜、偏航和高度都对准后进入提供旋转，垂直速度、垂直位置、IMU角度偏差和IMU速度偏差估计的模式。

测量值符号单位传感器最小频率用途
角速度$\boldsymbol{\omega}$$\mathrm{rad}/s$IMU100Hz预测
加速度$\boldsymbol{a}$$m/s^{2}$IMU100Hz预测
地球磁场$\boldsymbol{M}_{meas}$$\mathrm{gauss}$磁力计5Hz校正
海拔高度$H_{meas}$$m$GPS/气压计5Hz校正
距地高度$H_{meas}^{agl}$$m$测距仪/外部视觉5Hz校正
速度测量$\boldsymbol{V}_{meas}$$m/s$GPS/(光流+测距仪)/外部视觉5Hz校正
位置测量$\boldsymbol{P}_{meas}$$m$GPS/外部视觉5Hz校正
偏航角$\psi_{meas}$$\mathrm{rad}$GPS/磁力计/外部视觉5Hz校正
磁偏角$\psi^{DECLINATION}$$\mathrm{rad}$GPS/用户设置5Hz校正
等效空速$V_{meas}^{TAS}$$m/s$空速传感器5Hz校正
合成侧滑$\mathrm{beta}$$\mathrm{rad}$虚拟校正
阻力比力$\mathrm{drag}$虚拟校正

在PX4的State中已经声明了系统的状态向量，整个状态向量包含24维度，写作下式：

其中：

$q$表示系统的姿态四元数，被用于表示机体坐标系FRU对于NED导航坐标系的坐标变换，即相对于起飞点的姿态：

$\boldsymbol{V}_{NED}$表示飞机在NED坐标系下的机体速度，分别为北速度、东速度和地速度

$\boldsymbol{P}_{NED}$表示飞机在NED坐标系下的的位置，具体为：

$\boldsymbol{\Delta\theta}_{b}$表示在机体坐标系下角度增量偏差，这个偏差源自于陀螺不可剔除的偏置，为了避免在姿态更新时的梯形积分发散，需要进行额外的修正，因此引入了此增量偏差，此偏差可以通过一些数学方法计算，比如SVD。

$\boldsymbol{\Delta a}_{b}$表示在机体坐标系下加速度的偏差：

$\boldsymbol{M}_{I}$表示在NED坐标系下的地磁向量：

$\boldsymbol{M}_{B}$表示在机体坐标系下的磁偏角：

最后$\boldsymbol{V}_{wind}$表示在NED坐标系下北向风速和东向风速：

EKF2在PX4内的更新非常依赖时间戳，也就是timestamp。

先验估计

姿态更新

从机体坐标系FRU到导航坐标系NED的旋转矩阵根据飞机的姿态四元数给出，这里的方程具有两种写法，分别为：

这两个方程在数学上完全等价，但是第二个方程在随时间的变化中数值更稳定，且更加的简单，因此在EKF2中选择了第二个表达式，如果需要反向进行坐标变换只需要计算转置矩阵即可：

在时间更新的过程中，我们会定义采样的间隔时间为$\Delta t$，在获得IMU提供的角速度后，我们就可以得到增量角度的观测值：

现在我们已经有了imu的测量值$\boldsymbol{\Delta\theta}_{meas}$和在EKF内估计的增量角度偏差$\boldsymbol{\Delta\theta}_{b}$（这个数值是EKF在进入最小组合的估计模式后提供的），从而得到真增量角速度$\boldsymbol{\Delta\theta}_{truth}$：

而我们的增量四元数$\Delta q$定义了从k到k+1时刻下的旋转，使用小角度近似的办法就可以计算出：

在有了时间更新的旋转矩阵后，根据四元数的运算法则就可以的得到：

速度更新

再从EKF中取出加速度数据，就可以计算速度增量的观测值：

根据IMU的测量值$\boldsymbol{\Delta V}_{meas}$和加速度偏差$\boldsymbol{\Delta a}_{b}$计算出真增量速度$\boldsymbol{\Delta V}_{truth}$：

从k到k+1时刻，将真增量速度向量$\boldsymbol{\Delta V}_{truth}$从机体坐标系旋转到地球坐标系，并减去重力，计算速度状态变化：

展开后得到：

位置更新

这样我们就得到了方向速度，在使用梯形积分法后就可以得到较为准确的位置信息了：

在QGC中可以通过查看vel_test_radio和pos_test_radio判断融合效果。

其他更新

至于其他的比如IMU偏差等内容，状态转移只需要时刻更新：

协方差计算

我们在状态转移方程中除了噪声以外，还有传感器引入的测量方差，我们通过计算一段时间内陀螺仪的方差$\sigma_{gyro}^2$和加速度计的方差$\sigma_{accel}^2$就有增量角度的方差，即：

还有增量速度的测量方差：

这些方差共同组成我们状态转移方程中的干扰向量u，整个状态方程写作：

这样设定的目的就是假设系统的误差增长是由增量角度和增量速度的噪声驱动的，这种办法是可行的，我们把噪声写作过程噪声对角矩阵：

并且定义$\boldsymbol{G}\boldsymbol{Q}\boldsymbol{G}^{\mathrm{T}}$为状态误差矩阵。

从EKF的计算可以得知，我们需要对F进行求导，得到雅可比矩阵：

这个矩阵中有5个分块矩阵，分别为：$\boldsymbol{F}_{q_{k}}^{q_{k+1}}$、$\boldsymbol{F}_{q_{k}}^{V_{k+1}}$、$\boldsymbol{F}_{V_{k}}^{P_{k+1}}$、$\boldsymbol{F}_{\Delta\theta_{b}^{k}}^{q_{k+1}}$和$\boldsymbol{F}_{\Delta a_{b}^{k}}^{V_{k+1}}$

1、首先是$\boldsymbol{F}_{q_{k}}^{q_{k+1}}$，这时k+1时刻对k时刻的雅可比矩阵，大小为$4\times4$，可得：

其中$M\left(\boldsymbol{\theta}\right)$表示为一个反对称矩阵的函数，角度为$\theta$，旋转矩阵的微分就是旋转矩阵乘旋转角的反对称矩阵。

2、其次是速度四元数的状态转移矩阵，$\boldsymbol{F}_{q_{k}}^{V_{k+1}}$

其中：

3、随后是关于位置的状态转移矩阵$\boldsymbol{F}_{V_{k}}^{P_{k+1}}$

4、四元数关于增量角度偏差的状态转移矩阵$\boldsymbol{F}_{\Delta\theta_{b}^{k}}^{q_{k+1}}$

这里面$\boldsymbol{\Xi}\left(q\right)$出现在四元数与向量的乘法中：

$\boldsymbol{\Xi}$和$\boldsymbol{\Omega}$之间的关系为：

5、最后是速度关于增量速度偏差的状态转移矩阵$\boldsymbol{F}_{\Delta a_{b}^{k}}^{V_{k+1}}$：

干扰矩阵$\boldsymbol{G}$需要对增量角度和增量速度的测量值求偏导，因为测量值中包含有增量角度噪声和加速度噪声。它是一个$24\times6$的稀疏矩阵：

$\boldsymbol{G}_{\Delta\theta_{meas}^{k}}^{q_{k+1}}$和$\boldsymbol{G}_{\Delta V_{meas}^{k}}^{V_{k+1}}$这2个分块矩阵见下面的推导。

$\boldsymbol{G}_{\Delta\theta_{meas}^{k}}^{q_{k+1}}$为姿态四元数关于增量角度噪声的控制输入矩阵，即k+1时刻的旋转相对于陀螺仪k时刻增量角度噪声的协方差矩阵，大小为$4\times3$：

$\boldsymbol{G}_{\Delta V_{meas}^{k}}^{V_{k+1}}$为速度关于加速度噪声的控制输入矩阵，即k+1时刻的速度相对于加速度计k时刻加速度噪声的协方差矩阵，大小为$3\times3$：

最后为预测步骤计算过程噪声：

其中未知的方差可以由用户自行设定。到此，先验估计部分的计算已经结束。

后验估计

首先介绍一下在EKF2中融合的特点，首先EKF2确保每个传感器都可以融入状态估计器进行状态估计，系统具有非常强的容错。因为无论何种情况都有传感器进行数据修正。其次在使用EKF2是我们知道EKF2将XY平面和Z方向分开估计，也就是horizontal estimation和vertical estimation，原因也比较简单，作为无人机最小位置信息源GPS在高度测量上并没有水平方向精准。最后一点就是每个传感器只更新对应观测的内容，每个观测向量在计算过程都是一维一维的矫正。

首席那是磁力计的数据融合：

磁力计融合分为两种情况：

1、使用三轴磁力计的数据作为观测（三维）

2、使用磁力计转化的磁偏角作为观测（一维）

飞机在位于地面的时候就是使用方法2，他对磁场一场的数据和初始脱落偏差较大的情况具有更好的鲁棒性。第一种办法精度更高，但是对于外部磁场异常的时候表现较差。三轴磁场的更新根据是否有磁偏角的约束分为两个阶段。在初始化或差工况时，先进行无磁偏角约束的磁力计融合阶段，再进行有磁偏角约束的融合阶段；好工况时则相反。

三维磁力融合

磁力融合方程

无磁偏角约束的三轴磁场的融合阶段，不会考虑磁偏角，也就是假设地理的南北极和地磁的南北极重合，现在定义：机体的磁场强度为$\boldsymbol{M}_{meas}$，且通过计算均值得到磁力计测量方差为：$\boldsymbol{R}_{MAG}$，预测的机体磁场强度为$\boldsymbol{M}_{pred}$，而状态向量中地磁强度为$\boldsymbol{M}_{I}$，磁偏角为$\boldsymbol{M}_{B}$，我们得到机体旋转更新：

磁场观测矩阵$\boldsymbol{H}_{MAG}$是一个$3\times24$的矩阵，格式如下：

这里面出现了三个分块矩阵，本质上就是磁场对分量的导数，磁场可以得到姿态、地磁强度和地磁偏角，他们三个矩阵如下：

1、$\boldsymbol{H}_{q}$为磁力计的观测相对于姿态四元数的观测矩阵，即磁场测量值相对于旋转的 Jacobian ，大小为$3\times4$

其中：

2、$\boldsymbol{H}_{M_{I}}$为磁力计的观测相对于地球磁场向量的观测矩阵，即磁场测量值相对于地球磁场的 Jacobian ，大小$3\times3$：

3、$\boldsymbol{H}_{M_{B}}$为磁力计的观测相对于机体磁场偏差的观测矩阵，即磁场测量值相对于磁场偏差的 Jacobian ，大小$3\times3$：

在源码中，磁场观测矩阵实际上是分成三个一维矩阵进行观测的：

偏差测量融合方程

在此阶段，融合时需要考虑磁偏角的影响了，事实上地理南北极和地磁南北极位置并不重合，因此由一个磁偏角$\psi^{DECLINATION}$，因此如果考虑磁偏角的时候需要在融合完三轴磁力计的基础上在此进行磁偏角的融合，在没有绝对位置估计或者精确的速度测量的时候可以保持正确的机头朝向，如果使用光流计就可以防止在航向观测时地磁估计不准导致机头旋转。

磁偏角的获取有两种办法，第一种是GPS提供，另一种就是进行磁罗盘校准保存的磁偏角数据，首先定义一下磁偏角的预测值：

磁偏角的观测矩阵$\boldsymbol{H}_{\psi_{pred}^{DECLINATION}}$是一个$1\times24$的矩阵，格式如下：

这里只有一个矩阵需要计算，就是地磁强度对磁偏角的雅可比，计算如下：

融合后的结果可以通过下图的函数直接调用，数据有两种方法，一种是GPS提供，一种是三角预测值：

1

一维磁罗盘航向融合方程

航向角也就是Yaw角，可以直接通过磁力计的测量值$\boldsymbol{M}_{meas}$计算出，在这里提出一个重要假设是在上一时刻为零偏航角。然后在当前时刻，分别计算偏航角的测量值和预测值，再求残差。不过因为零偏航角假设与当前实际偏航角相差太大，因此该算法不太准确。

首先把磁场旋转到NED坐标系下：

此时的Yaw角为：

初次之外，Yaw角还可以从四元数的旋转矩阵计算出来：

航向角的观测相对于状态向量$\boldsymbol{x}$的观测矩阵$\boldsymbol{H}_{\psi}$的雅可比可以写作：

$\boldsymbol{H}_{q}^{\psi}$为航向角的观测相对于姿态四元数q的观测矩阵，尺寸为$1\times4$，其数值为：

这个偏导展开太复杂，很难手工推导。使用sympy生成的计算式如下：

光流融合

光流计包含两部分，视觉的轴位移和激光测距仪，分别提供XY平面上的速度测量和高度，光流主要输出包括：

  • 视轴(LOS)速度，数值为围绕光流计镜头的坐标系下的速度量
  • 在X、Y轴方向产生的位移量
  • 测地距离

首先计算传感器坐标系下的相对速度$\boldsymbol{V}^{B}$：

将速度除以距离得到X与Y轴的角速度：

现在给出LOS的角速度观测对转台向量的观测矩阵$\boldsymbol{H}_{LOS}$与雅可比，这是一个$2\times24的矩阵：

其中包含两个分块，数值如下：

1、$\boldsymbol{H}_{q}^{V^{LOS}}$为视轴角速度的观测相对于姿态四元数的观测矩阵，即视轴角速度相对于旋转的 Jacobian ，大小为$2\times4$

其中：

2、$\boldsymbol{H}_{V}^{V^{LOS}}$为视轴角速度的观测相对于 NED 坐标系下机体速度的观测矩阵，即视轴角速度相对于 NED 坐标系下机体速度的 Jacobian ，大小为$2\times3$

GPS速度与位置融合

GPS的速度和位置在传感器上都可以直接读取，但是由于z方向的误差比较大，一般不采用，因此需要忽略掉，只保留水平NE方向的数据，下面是对应的观测矩阵：

合成侧滑

在EKF2可以进行假定的风速估计后，需要将$\boldsymbol{V}_{EAS}$旋转到机体坐标系中得到机体的风速$\boldsymbol{V}_{wind_{B}}$

用小角度近似侧滑角度，也就是侧滑的新息，随后就是观测矩阵和对应的雅可比矩阵$1\times24$：

这里面引入了三个合成侧滑的雅可比，符号较为复杂，暂且不写

阻力比力融合

与合成侧滑类似，需要拿到飞机的机体风速$\boldsymbol{V}_{wind_{B}}$，随后将计算得到的速度增量$\boldsymbol{\Delta V}_{meas}$假设为加速度的测量值$\boldsymbol{a}_{meas}$，用此值减去加速度偏差$\boldsymbol{\Delta a}_{b}$得到阻力比力：

并用空气密度$\rho$和弹道系数$\mathrm{coef}$估算空速$\mathrm{airspeed}$

预测的加速度$\boldsymbol{a}_{pred}$为：

则阻力比力新息$\mathrm{drag_innov}$为：

阻力比力的观测相对于状态向量的观测矩阵$\boldsymbol{H}_{drag}$为：

风速估值器

首先假设无人机在水平面上只有姿态变化，没有位置变化，这种假设叫做零侧滑。随后假设风场近似为水平风场，在NED坐标系下意味着风仅包含NE方向的风速。真空速表示飞行器飞行时相对气流的速度，此值与空气密度和温度均有关系。

首先进行初始化，在输入外部航向角$\psi$后可以设定状态方程：

其中$V_{meas}^{TAS}$表示测量的空速值，现在求状态转移方程系数的雅可比：

有了这个矩阵就可以进行EKF更新了

机头更新

为了解决没有额外的磁力估计器或者其他的Yaw轴数据情况下进行机头估计、或者因为故障导致的导航丢失。EKF引入了EKF-GSF（Gaussian Sum Filter）也就是高斯和滤波器进行自动校正偏航误差。他可以让无人机在不亮磁偏航引起的起飞故障快速恢复。

首先EKF-GSF在EKF之外，主要包括：

  • 使用互补滤波的AHRS解，预测机头和向前、向右的加速度。
  • 使用状态为NE方向的速度和偏航作为状态，使用GPS的NE速度作为观测
  • GSF的总权重为1，输出的偏航估计值为单个EKF的加权平均值

2

首先我们假设偏航角是由NE方向的速度和绕Z轴旋转的角度所驱动的，也就是说测量误差与这三个值的测量噪声有关：

这三个值和三个噪声都是从IMU提供的，所以在XY平面上会预先设置5个航向角，分别进行时间更新。随后测量阶段观测GPS提供的XY方向的速度，因为残差的存在，这5个航向角最终会逐渐收敛到真实的偏航角上。

流程可以近似看作下图：

3

左图为初始状态，右图为优化后的状态，利用GSF将优化后的每个航向角按照权重甲醛融合得到一个复合航向角$\psi_{GSF}$

4

这个算法在水平速度变化越大，收敛越快。在初始状态，5个向量等距分布，权重都是0.2

5

AHRS倾角对准

倾角对准就是考虑如何从增量速度计算旋转矩阵，旋转矩阵直接从加速度测量中构造，对于所有模型都是相同的，因此只需计算一次。有两个假设分别为：

  • 假设偏航角为零，当速度融合开始时，每个模型的偏航角稍后对准。
  • 机体没有加速，因此所有测量的加速度都是由重力引起的。

用$\Delta t$表示传感器事件间隔时间。从主模块得到的加速度$\boldsymbol{a}^{I}$计算得到增量速度$\boldsymbol{\Delta V}^{I}$

这是地球坐标系中的数据，需要转换到机体坐标系中并归一得到重力轴方向的单位向量

然后计算地球坐标系下N方向的单位向量，旋转为机体坐标系，此时数值与$\boldsymbol{D}^{B}$正交：

然后计算东方向的单位向量，可以直接用$\boldsymbol{D}^{B}$与$\boldsymbol{N}^{B}$的叉乘计算：

从地球坐标系到机体坐标系的旋转矩阵的每一列都代表了旋转到机体坐标系的相应的地球坐标单位向量投影。我们这一步需要机体坐标系到地球坐标系的旋转矩阵，因此我们按照对应的列填写到旋转矩阵：

可以看到我们这一步也生成了一个姿态矩阵，这个姿态是根据最新的偏航角得到的，我们接下来把他融合到EKF中

预测方程

首先定义一些变量：

  • 令$\psi$表示机体坐标系相对于地球坐标系下的偏航角
  • 令$\boldsymbol{V}_{NE}$表示机体在世界坐标系中的向北(N)和向东(E)的速度，
  • 令$\Delta\psi$表示IMU测量的偏航增量
  • 令$\sigma_{\Delta\psi}^{2}$表示测量偏航角的方差

接下来我们就有机体到导航方向的姿态变换矩阵(2维情况)：

姿态更新方程：

速度更新方程：

现在就可以有状态向量表达了：

现在就可以计算状态转移矩阵了：

干扰控制向量写作：

推导协方差预测方程。在消除偏差效应后，假设惯性解中的误差增长由增量角度和增量速度中的“噪声”驱动。推导IMU噪声到状态噪声的$3\times3$控制(干扰)影响矩阵：

然后定义干扰矩阵：

得到协方差矩阵：

协方差更新矩阵如下：

GSF本质上是加权卡尔曼滤波器，更新过程我们姑且不写，首先给出卡尔曼增益的分母表达式：

然后计算马氏距离：

然后用此距离计算对应的高斯密度：

用密度更新权重并进行归一化：

我们在使用权重更新向量的时候假设偏航向量是单位向量，只需要做基础的变换即可：

然后计算新息序列：

融合时延与互补滤波

该算法使用当前时间范围内的最新IMU数据实现捷联惯导算法(strapdown INS algorithm)。该算法缓冲惯性导航状态，并计算延迟融合时间范围内(delayed fusion time horizon)与EKF状态的差异。根据差值计算增量角度、增量速度和速度校正，并在当前时间范围内应用它们，以便惯性导航状态在延迟融合时间范围内跟踪EKF状态。

从软件的角度看，因为现在的系统越来越复杂，原本在RTOS时代所做的假设越来越不能得到满足。现在传感器采样，滤波器计算位姿，用户获取位姿，都分别处于不同的线程中，有不同的工作频率。众多传感器各自工作在不同的频率，异步采样，数据异步到达。滤波器有自己的工作频率，上层软件也有另外的工作频率。所有这些，都会有一个时间差，因此在用户获取当前位姿信息的瞬间，滤波器需要对当前所维护的信息做一个补偿，这就是输出互补滤波器。它的工作原理就是维护一个历史窗口，从历史数据中计算互补参数，然后对输出信息进行校正。在这里没有什么复杂的计算公式，一行的互补公式较多且大多简单细碎，经验参数众多，有很多工程细节，依赖专业背景和工程经验方可调参。下面简单描述该算法的实现。

在传统的姿态估计的应用场景中，为简化计算并提高估计精度，有一个并行采样的假设。即所有的传感器数据都是同时采样。在高端设备上，并行采样是通过设计硬件电路来保证的。在低成本的方案里，实际上执行的是串行采样，陀螺仪/加速计/地磁计这三者串行采样，有可能前后采样的时间差有$1\ ms$左右。如果对估计精度不讲究，把它们当成并行采样处理也是可以的。那些低成本的无人机的姿态估计模块也都是这么做的。

此外，还有一个数据采样周期不一致的问题会影响估计精度。在低成本方案里，陀螺仪/加速计可以达到$100\sim200\ \mathrm{Hz}$的采样率，而地磁计的采样率可能只有$10\ \mathrm{Hz}$，而GPS更低。但是嵌入式软件系统为了简化设计，往往只围绕一种采样率进行设计。如果陀螺仪/加速计/地磁计这三者都以$100\ \mathrm{Hz}$进行采样，因地磁计来不及采集数据，可能连续$10$次上报同一个数据，直到寄存器锁定后才上报新的数据。这样的数据组合，既会影响估计精度，也会增加系统负荷。

因此，为提高估计精度，必须处理多速率多延迟的向量测量这种情况。多延迟如下图所示，如果同时对所有传感器发出采样指令，IMU是反应最灵敏的，可以认为是立即返回，其它传感器相对于IMU都有不同程度的延迟。

6

由此可以看出，像GPS或外部视觉这类复杂传感器反应都很慢。

甚至在当前的复杂系统中，采样时间点也不可能同步，而ECL EKF估计系统的运行频率与这些传感器不一样也不同步。ECL EKF估计系统的设计主要考虑两种频率：IMU的采样频率和EKF的运行频率。其中，IMU的常见采样频率为 $100\sim250\ \mathrm{Hz}$，而EKF的运行频率在理想情况下应该是IMU采样频率的整数倍，为 $50\sim200\ \mathrm{Hz}$。我们将其称之为延迟融合时间范围(delayed fusion time horizon)。为简化设计，在实现中两者都采用 $100\ \mathrm{Hz}$ 的设计。

因为增量角度和增量速度依赖时间间隔 $\mathrm{d}t$ 的积分，所以IMU的 $\mathrm{d}t_{\mathrm{imu}}$ 的精度影响滤波器的估计精度。如果IMU的名义采样率为 $100\ \mathrm{Hz}$，因为系统的各种原因，实际采样率可能在 $98\sim102\ \mathrm{Hz}$ 之间抖动。所以在代码中 $\mathrm{d}t_{\mathrm{imu}}$ 的数值不能写死，而是采用采样事件之间的时间戳差值，并使用低通滤波计算：

其中，$\alpha=0.8$，这意味着我们假定采样时钟比较稳定，偶尔才会出现抖动。

同样的，ECL的运行频率，$\mathrm{d}t_{\mathrm{ekf}}$，也需要使用低通滤波计算：

其中，$\alpha=0.99$，这意味着我们假定系统时钟很稳定，偶尔才会出现抖动。

7

在实现中，ECL EKF估计系统的运行线程会被各种传感器多次唤醒。如果我们对输入系统以IMU周期为栅格进行考察，其它传感器测量值将随机落到不同的栅格中。因为各种传感器采样速率差别巨大，实际上有很多栅格里并没有校正传感器测量值到达，也就是在这时间段内，位姿只有预测而没有校正。如果滤波器以预测-校正为一个完整迭代周期，则因为采样率与采样时间点的差异，以及ECL EKF估计系统运行的延迟，最终所实现的滤波系统为在一段时间内估计精度可接受的近似最优估计系统。

姿态补偿

用户设置的增益系数，$\mathrm{d}t_{\mathrm{ekf}}<\tau_{V},\tau_{P}<10$，由此计算速度和位置的增益系数：

后面将遍历输出滤波器状态历史，$i\in1\sim N$，并将校正应用于速度和位置状态。在这里使用与姿态校正不一样的方法。因为上述用于校正姿态状态的方法需要四元数运算，对于速度和位置来说太昂贵了。这里的方法由于它允许使用更高的跟踪增益，因此消除了”校正回路”中的时间延迟，并减少了相对于EKF状态的跟踪误差。

计算$z$轴的速度和位置跟踪增量：

计算将应用于输出状态历史的速度校正，使用一个PD反馈调参到$5\%$超调：

计算要应用于$V_{D}^{\prime}$的校正，该校正导致垂直位置增量$\sum_{i=1}^{k}\left(\delta P_{D}\right)_{i}$使用用于速度和位置状态跟踪的替代算法，以在融合时间范围内跟踪EKF的$z$轴位置状态。该算法对$V_{D}^{\prime}$状态历史应用校正，并使用当前校正后的$V_{D}^{\prime}$向前传播$\sum_{i=1}^{k}\left(\delta P_{D}\right)_{i}$。这提供了一种替代的垂直速度输出，该输出更接近于位置的一阶导数，但相对于24-EKF状态确实降低了跟踪成本。

从最早的垂直输出滤波器状态历史开始循环，$i\in1\sim N$，并将校正应用于$V_{D}^{\prime}$状态，并使用校正后的$V_{D}^{\prime}$向前传播$\sum_{i=1}^{k}\left(\delta P_{D}\right)_{i}$。

  • 校正当前的垂直速度：

  • 使用校正后的速度和梯形积分器向前传播位置积分：

最后将当前垂直速度的输出状态更新为INS输出缓冲区中最新的校正值。

加速度、速度和位置

用加速度偏差校正机体坐标系的增量速度

将校正后的增量速度旋转到地球坐标系

叠加重力引起的测量加速度

计算机体在地球坐标系中的速度导数(加速度)

通过测量加上校正，增加惯性导航系统的速度状态

对于替代校正算法所使用的垂直速度状态也同样如此

使用梯形积分计算惯性导航系统的位置状态

对于替代校正算法所使用的垂直位置状态(垂直速度积分)也同样如此

用IMU偏移校正原点速度。计算上一次IMU更新的平均角速度变化率(角加速度)：

计算IMU点在机体坐标系中相对于机体原点的速度。我们有IMU位于机体坐标系的位置$\boldsymbol{P}_{imu}^{B}$。使用向量叉乘$\otimes$计算速度：

将相对速度旋转到地球坐标系中，得到NED地球坐标系中IMU点相对于机体原点的速度：

增量增益系数

INS状态存储环形缓冲区与IMU数据缓冲区具有相同长度$N$和时间坐标。从INS环形缓冲区获取最旧的INS状态数据，为$k-N+1$时刻的数据，这些数据将与24-EKF当前$k$时刻的融合时间范围状态相比较，其中两者的时间误差为$\mathrm{d}t_{\mathrm{delay}}$。该算法主要是为了放大误差值。

在EKF融合时间范围内计算INS四元数$\left(q_{INS}\right)_{k-N+1}$和EKF四元数$\left(q_{ekf}\right)_{k}$之间的四元数增量：

将四元数增量转换为增量角度（小角度近似）：

计算一个姿态增益，提供对24-EKF估计器姿态状态的严密跟踪，并调整时间延迟的变化，以保持约$0.7$的一致阻尼比：

用其计算增量角度的校正向量，这将导致INS跟踪24-EKF姿态四元数，并且该增量角度校正向量将在下一迭代被使用到：

计算速度和位置的互补滤波器增益。用户设置的增益系数，$\mathrm{d}t_{\mathrm{ekf}} < \tau_{V}, \tau_{P} < 10$，由此计算速度和位置的增益系数：

后面将遍历输出滤波器状态历史，$i \in 1 \sim N$，并将校正应用于速度和位置状态。在这里使用与姿态校正不一样的方法。因为上述用于校正姿态状态的方法需要四元数运算，对于速度和位置来说太昂贵了。这里的方法由于它允许使用更高的跟踪增益，因此消除了”校正回路”中的时间延迟，并减少了相对于EKF状态的跟踪误差。

计算$z$轴的速度和位置跟踪增量：

计算将应用于输出状态历史的速度校正，使用一个PD反馈调参到$5\%$超调：

计算要应用于$V_{D}^{\prime}$的校正，该校正导致垂直位置增量$\sum_{i=1}^{k}\left(\delta P_{D}\right)_{i}$使用用于速度和位置状态跟踪的替代算法，以在融合时间范围内跟踪EKF的$z$轴位置状态。该算法对$V_{D}^{\prime}$状态历史应用校正，并使用当前校正后的$V_{D}^{\prime}$向前传播$\sum_{i=1}^{k}\left(\delta P_{D}\right)_{i}$。这提供了一种替代的垂直速度输出，该输出更接近于位置的一阶导数，但相对于24-EKF状态确实降低了跟踪成本。

从最早的垂直输出滤波器状态历史开始循环，$i \in 1 \sim N$，并将校正应用于$V_{D}^{\prime}$状态，并使用校正后的$V_{D}^{\prime}$向前传播$\sum_{i=1}^{k}\left(\delta P_{D}\right)_{i}$。

垂直速度补偿

首先计算$z$轴的速度和位置跟踪增量：

计算将应用于输出状态历史的速度校正，使用一个PD反馈调参到$5\%$超调：

计算要应用于$V_{D}^{\prime}$的校正，该校正导致垂直位置增量$\sum_{i=1}^{k}\left(\delta P_{D}\right)_{i}$，使用用于速度和位置状态跟踪的替代算法，以在融合时间范围内跟踪EKF的$z$轴位置状态。该算法对$V_{D}^{\prime}$状态历史应用校正，并使用当前校正后的$V_{D}^{\prime}$向前传播$\sum_{i=1}^{k}\left(\delta P_{D}\right)_{i}$。这提供了一种替代的垂直速度输出，该输出更接近于位置的一阶导数，但相对于24-EKF状态确实降低了跟踪成本。

从最早的垂直输出滤波器状态历史开始循环，$i \in 1 \sim N$，并将校正应用于$V_{D}^{\prime}$状态：

  • 校正当前的垂直速度：

  • 使用校正后的速度和梯形积分器向前传播位置积分：

三维速度补偿

  • 计算水平速度和位置跟踪误差：

    计算将应用于输出状态历史的速度校正：

    计算将应用于输出状态历史的位置校正：

    计算要应用于速度和位置输出状态历史的校正。速度和位置状态历史被分别校正，以便它们在融合时间范围内跟踪EKF状态。此选项提供最准确的EKF状态跟踪。

    遍历输出滤波器状态历史，$i \in 1 \sim N$，并将校正应用于速度和位置状态：

    • 应用常量速度校正：

    • 应用常量位置校正：

    最后将当前三维速度和位置的输出状态更新为INS输出缓冲区中最新的校正值。

状态输出

在经过互补滤波校正后，EKF 输出以下常用数据：

  1. 输出姿态四元数

    • 格式：$\left(q_{INS}\right)_{k}$
    • 用户可用来转换成易读的欧拉角。

  2. 输出速度
    在局部NED地球坐标系中机体坐标系原点的速度：

  3. 输出加速度
    机体在地球坐标系中的速度导数 $\boldsymbol{a}^{I}$。

  4. 输出垂直方向速度
    局部NED地球坐标系中机体坐标系原点垂直位置的导数：

  5. 输出位置
    在局部地球坐标系中机体坐标系原点的位置：

    • 将IMU相对于机体原点的位置 $\boldsymbol{P}_{imu}^{B}$ 旋转到地球坐标系中：

    • 从EKF位置（即IMU位置）减去，得到机体原点的位置：

  6. 输出预测姿态四元数
    使用最新的IMU增量角数据向前预测先前的四元数输出状态：

    • 校正偏差误差的增量角数据：

    • 使用校正后的增量角向量校正姿态四元数：

    • 归一化修改后的姿态四元数：

里程计信息:

  • 位置：$\boldsymbol{P}_{NED}^{O}$

  • 姿态四元数：$q_{INS}$

  • 速度：$\boldsymbol{V}_{NED}^{O}$

  • 角速度：

  • 速度方差：24-EKF的 $\boldsymbol{P}$ 矩阵速度方阵的对角线向量。

  • 位置方差：24-EKF的 $\boldsymbol{P}$ 矩阵位置方阵的对角线向量。

  • 姿态方差：24-EKF的 $\boldsymbol{P}$ 矩阵姿态方阵的对角线向量。

局部位置信息:

  • 位置：$\boldsymbol{P}_{NED}^{O}$
  • 速度：$\boldsymbol{V}_{NED}^{O}$
  • 垂直方向速度：$V_{D}^{O}$
  • 加速度：$\boldsymbol{a}^{I}$
  • 经纬度与海拔高度。

预测姿态信息:

  • 预测姿态四元数：$q_{pred}$。

零状态更新

为了解决飞机静止状态下IMU数据的静态飘移，在飞控EKF检测到静止状态的每200ms都会进入零状态更新，在此阶段会进行飘移估计：

  • 陀螺估计：200ms内陀螺的变化速率，这个速率开根号得到方差叠加到EKF的方差上，同时这个速率本身叠加到EKF的偏差上进行更新
  • 零速更新：200ms内飞机都处于稳定状态时，将速度观测值设为0，并手动设置方差

总结

本期博客详细介绍了PX4的EKF2算法，希望读者可以学会EKF2并运用到实际之中，并不仅是无人机。

笔记
#C++ #数学 #PX4
为什么PX4的EKF2可以让传感器数据稳定
https://blog.minloha.cn/posts/1824197f0175482026032437.html
作者
Minloha
发布于
2026年3月24日
更新于
2026年3月24日
许可协议