PX4 EKF中的多传感器融合方法

本文首先列出了EKF滤波和卡尔曼滤波公式，其次列出了PX4中姿态四元数、速度、位置的一步预测方程，以及磁传感器、空速传感器、GPS的融合公式，主要参考了Ekf.cpp中的void Ekf::predictState()函数，以及PX4 ECL中的文档 Process and Observatopm Models.pdf。

PX4 ECL库代码及文档Github下载链接：PX4-ECL

1 滤波方法及状态预测

1.1 EKF 滤波方程

系统状态、量测方程如下：
（1）状态方程：

（2）量测方程：

其中，$u_k$为控制矢量；${\omega }_k$为过程噪声，$v_k$为量测噪声，均假设为0均值的高斯白噪声，其协方差分别为$Q_k$和$R_k$。

滤波步骤为：

（1）预测：

（2）更新：

其中状态转移矩阵和量测矩阵定义为如下雅可比矩阵：

1.2 KF 滤波方程

系统状态、量测方程如下：

（1）状态方程：

（2）量测方程：

其中，$u_k$为控制矢量；${\omega }_k$为过程噪声，$v_k$为量测噪声，均假设为0均值的高斯白噪声，其协方差分别为$Q_k$和$R_k$。

滤波步骤为：

（1）预测：

（2）更新：

1.3 PX4中的状态量及其预测

1.3.1 状态量

状态量X∈R^(24×1)，如下图所示。

1.3.2 姿态四元数一步预测

四元数的更新方程如图片中（9.3.40）所示（参考《惯性导航第二版 秦永元》）：
由于时间较短、非高动态情况下，角度积分值较小，可以用一阶近似，即 $cos\frac{\Delta \theta }{2}$ = 0, $sin\frac{\Delta \theta }{2}$ = $\frac{\Delta \theta }{2}$。所以在PX4的文档中，采用如下公式更新姿态四元数：

计算陀螺积分：

计算四元数增量：

计算四元数一步预测值：

1.3.3 速度、位置一步预测

主要是对陀螺仪和加速度计的量测值进行补偿，通过积分对速度、位置进行计算，比较简单，可参考代码Ekf.cpp中的void Ekf::predictState()函数。

计算加速度积分：

计算速度一步预测值

计算位置一步预测值

1.3.4 协方差阵预测

协方差阵预测相对较为复杂，需要推导雅可比方程，目前暂未进行推导。代码可以参考Covariance.cpp中的void Ekf::predictCovariance()函数。

PX4中使用的协方差预测方程为：

$P_{k|k-1}=F_k{P}_{k-1|k-1}{F}_k^T+G_k{Q}_k{G}_k+Q_s$

其中，$P_{k|k-1}$为预测的协方差阵，$Q_k$为IMU的噪声协方差，$Q_s$为稳定滤波器所添加的额外过程噪声协方差阵。

2 磁三轴数据融合

2.1 量测量估计值及量测矩阵

EKF滤波中的量测方程为：

$z_k=h(x_{k-1})+v_k$

设机体系的磁感值为量测量，磁力计测得的磁感值为[$m_x^b$ $m_y^b$ $m_z^b$ ]。由状态估计值可通过下式，计算得到量测量估计值$z_{k|k-1}=h(x_{k|k-1})$：

其中：

将式（1.21）展开后可得：

以X轴为例，求测量雅可比矩阵则需要用式（1.23）求偏导：

则X轴的测量雅可比矩阵（$H_{x}k$）为:
Y轴、Z轴的雅可比矩阵求法类似，则三轴量测雅可比矩阵为：

2.2 磁融合滤波方程

假设$x_{k|k-1}$，$P_{k|k-1}$已知，仅进行EKF滤波中的更新部分。设磁力计的量测噪声协方差为$R_k$，则：

2.3 对应代码

参考PX4 EKF 代码Mag_fusion.cpp中的void Ekf::fuseMag()函数。如果使用的是3轴地磁融合，但是没有外部辅助传感器，还必须融合磁偏角来防止长时间的航向偏移（具体融合代码见void Ekf::fuseDeclination()）。而在使用航向融合时，不必融合磁偏角。

2.4 磁偏角融合

量测量磁偏角mag_delination
根据状态量计算磁偏角 atan2f(magE, magN)
$y_k$ = mag_delination-atan2f(magE,magN)

3 航向融合

3.1 磁力计航向计算

由于磁力计在机体系中测量得到三轴数据，因此须将该测量量首先旋转到水平面，再进行磁航向计算。

（1）旋转计算水平磁强度

机体系到水平面的旋转矩阵为$C_b^n$，但磁力计测量量旋转时，航向旋转角度应设为0。设俯仰角为θ，横滚为∅，则旋转矩阵为：

（2）航向角测量值计算
设mag_declination为磁北和真北的航偏角，则水平面的磁感值为：

航向角为：

3.2 航向预测值计算

状态量如图1.1所示，利用状态量中的四元数计算航向角φ估计值：

求φ对各状态量偏导，具体求导过程参见附录，量测雅可比矩阵为:

3.3 航向融合滤波方程

3.4 对应代码

参考PX4 EKF 代码Mag_fusion.cpp中的void Ekf::fuseHeading()函数。

4 空速融合

4.1 空速预测值计算

状态量如图1.1所示，则由状态量可知空速预测值TAS_pred为：

求TAS_pred相对于状态量的偏导，则有：

量测雅可比矩阵为:

4.2 空速融合方程

4.3 对应代码

参考PX4 EKF 代码Airspeed_fusion.cpp中的void Ekf::fuseAirspeed()函数

5 位置、速度融合

PX4中的水平位置、速度信息由GPS提供，高度信息由GPS、气压高度计、测距仪或视觉传感器提供。由于位置、速度融合的量测方程为线性，因此可以直接使用KF滤波，不用再求雅可比矩阵。

5.1 位置、速度融合方程

5.2 对应代码

参考PX4 EKF 代码Vel_pos_fusion.cpp中的void Ekf::fuseVelPosHeight()函数。

附录