PX4 EKF中的多传感器融合方法
时间：2022-12-26 00:30:00
文章目录
-
- 1 滤波方法及状态预测
-
- 1.1 EKF 滤波方程
- 1.2 KF 滤波方程
- 1.3 PX4中的状态量及其预测
-
- 1.3.1 状态量
- 1.3.2 一步预测姿态四元数
- 1.3.3 一步一步地预测速度和位置
- 1.3.4 协方差阵预测
- 2 磁三轴数据集成
-
- 2.1 测量估计值和测量矩阵
- 2.2 磁融合滤波方程
- 2.3 对应代码
- 2.4 磁偏角融合
- 3 航向融合
-
- 3.1 计算磁力计航向
- 3.2 计算航向预测值
- 3.3 航向集成滤波方程
- 3.4 对应代码
- 4 空速融合
-
- 4.1 计算空速预测值
- 4.2 空速融合方程
- 4.3 对应代码
- 5 整合位置和速度
-
- 5.1 位置、速度融合方程
- 5.2 对应代码
- 附录
首先列出了这篇文章EKF其次列出了滤波器和卡尔曼滤波器公式PX以及磁传感器、空速传感器、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 u_k uk控制矢量； ω k ω_k ωk过程噪声， v k v_k vk为了测量噪声，假设为0平均值的高斯白噪声，其协方差分别为 Q k Q_k Qk和 R k R_k Rk。
滤波步骤为：
（1）预测：
（2）更新：
其中状态转移矩阵和量测矩阵定义为如下雅可比矩阵：
1.2 KF 滤波方程
系统状态、量测方程如下：
（1）状态方程：
（2）量测方程：
其中， u k u_k uk为控制矢量； ω k ω_k ωk为过程噪声， v k v_k vk为量测噪声，均假设为0均值的高斯白噪声，其协方差分别为 Q k Q_k Qk和 R k R_k Rk。
滤波步骤为：
（1）预测：
（2）更新：
1.3 PX4中的状态量及其预测
1.3.1 状态量
状态量X∈R^(24×1)，如下图所示。
1.3.2 姿态四元数一步预测
四元数的更新方程如图片中（9.3.40）所示（参考《惯性导航第二版 秦永元》）：
由于时间较短、非高动态情况下，角度积分值较小，可以用一阶近似，即 c o s Δ θ 2 cos\frac{\Delta\theta}{2} cos2Δθ = 0, s i n Δ θ 2 sin\frac{\Delta\theta}{2} sin2Δθ = Δ θ 2 \frac{\Delta\theta}{2} 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}=F_k P_{k-1|k-1} F_k^T+G_k Q_k G_k+Q_s Pk∣k−1=FkPk−1∣k−1FkT+GkQkGk+Qs
其中， P k ∣ k − 1 P_{k|k-1} Pk∣k−1为预测的协方差阵， Q k Q_k Qk为IMU的噪声协方差， Q s Q_s Qs为稳定滤波器所添加的额外过程噪声协方差阵。
2 磁三轴数据融合
2.1 量测量估计值及量测矩阵
EKF滤波中的量测方程为：
z k = h ( x k − 1 ) + v k z_k=h(x_{k-1} )+v_k zk=h(xk−1)+vk
设机体系的磁感值为量测量，磁力计测得的磁感值为[ m x b m_x^b mxb m y b m_y^b myb m z b m_z^b mzb ]。由状态估计值可通过下式，计算得到量测量估计值 z k ∣ k − 1 = h ( x k ∣ k − 1 ) z_{k|k-1} = h(x_{k|k-1}) zk∣k−1=h(xk∣k−1)：
其中：
将式（1.21）展开后可得：
以X轴为例，求测量雅可比矩阵则需要用式（1.23）求偏导：
则X轴的测量雅可比矩阵（ H x k H_xk Hxk）为:
Y轴、Z轴的雅可比矩阵求法类似，则三轴量测雅可比矩阵为：
2.2 磁融合滤波方程
假设 x k ∣ k − 1 x _{k|k-1} xk∣k−1， P k ∣ k − 1 P_{k|k-1} Pk∣k−1已知，仅进行EKF滤波中的更新部分。设磁力计的量测噪声协方差为 R k R_k Rk，则：
2.3 对应代码
参考PX4 EKF 代码Mag_fusion.cpp中的void Ekf::fuseMag()函数。如果使用的是3轴地磁融合，但是没有外部辅助传感器，还必须融合磁偏角来防止长时间的航向偏移（具体融合代码见void Ekf::fuseDeclination()）。而在使用航向融合时，不必融合磁偏角。
2.4 磁偏角融合
量测量磁偏角mag_delination
根据状态量计算磁偏角 atan2f(magE, magN)
y k y _k yk = mag_delination-atan2f(magE,magN)
3 航向融合
3.1 磁力计航向计算
由于磁力计在机体系中测量得到三轴数据，因此须将该测量量首先旋转到水平面，再进行磁航向计算。
（1）旋转计算水平磁强度
机体系到水平面的旋转矩阵为 C b n C_b^n Cbn，但磁力计测量量旋转时，航向旋转角度应设为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()函数。
