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

文章目录

    • 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滤波和卡尔曼滤波公式,其次列出了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 u_k uk为控制矢量; ω k ω_k ωk为过程噪声, v k v_k vk为量测噪声,均假设为0均值的高斯白噪声,其协方差分别为 Q k Q_k Qk R k R_k Rk

滤波步骤为:

(1)预测:
在这里插入图片描述
(2)更新:
PX4 EKF中的多传感器融合方法_第1张图片
其中状态转移矩阵量测矩阵定义为如下雅可比矩阵:
在这里插入图片描述

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)更新:
PX4 EKF中的多传感器融合方法_第2张图片

1.3 PX4中的状态量及其预测

1.3.1 状态量

状态量X∈R^(24×1),如下图所示。
PX4 EKF中的多传感器融合方法_第3张图片

1.3.2 姿态四元数一步预测

四元数的更新方程如图片中(9.3.40)所示(参考《惯性导航第二版 秦永元》):PX4 EKF中的多传感器融合方法_第4张图片
由于时间较短、非高动态情况下,角度积分值较小,可以用一阶近似,即 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的文档中,采用如下公式更新姿态四元数:

计算陀螺积分:
PX4 EKF中的多传感器融合方法_第5张图片
计算四元数增量:
PX4 EKF中的多传感器融合方法_第6张图片
计算四元数一步预测值:
PX4 EKF中的多传感器融合方法_第7张图片

1.3.3 速度、位置一步预测

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

计算加速度积分:
PX4 EKF中的多传感器融合方法_第8张图片

计算速度一步预测值
PX4 EKF中的多传感器融合方法_第9张图片
计算位置一步预测值
PX4 EKF中的多传感器融合方法_第10张图片

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 Pkk1=FkPk1∣k1FkT+GkQkGk+Qs

其中, P k ∣ k − 1 P_{k|k-1} Pkk1为预测的协方差阵, 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(xk1)+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}) zkk1=h(xkk1)
在这里插入图片描述
其中:
PX4 EKF中的多传感器融合方法_第11张图片
将式(1.21)展开后可得:
PX4 EKF中的多传感器融合方法_第12张图片
以X轴为例,求测量雅可比矩阵则需要用式(1.23)求偏导:
PX4 EKF中的多传感器融合方法_第13张图片
则X轴的测量雅可比矩阵( H x k H_xk Hxk)为:在这里插入图片描述
Y轴、Z轴的雅可比矩阵求法类似,则三轴量测雅可比矩阵为:
在这里插入图片描述

2.2 磁融合滤波方程

假设 x k ∣ k − 1 x _{k|k-1} xkk1 P k ∣ k − 1 P_{k|k-1} Pkk1已知,仅进行EKF滤波中的更新部分。设磁力计的量测噪声协方差为 R k R_k Rk,则:
PX4 EKF中的多传感器融合方法_第14张图片

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所示,利用状态量中的四元数计算航向角φ估计值:
在这里插入图片描述
求φ对各状态量偏导,具体求导过程参见附录,量测雅可比矩阵为:
PX4 EKF中的多传感器融合方法_第15张图片

3.3 航向融合滤波方程

PX4 EKF中的多传感器融合方法_第16张图片

3.4 对应代码

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

4 空速融合

4.1 空速预测值计算

状态量如图1.1所示,则由状态量可知空速预测值TAS_pred为:
在这里插入图片描述
求TAS_pred相对于状态量的偏导,则有:
PX4 EKF中的多传感器融合方法_第17张图片
量测雅可比矩阵为:
PX4 EKF中的多传感器融合方法_第18张图片

4.2 空速融合方程

PX4 EKF中的多传感器融合方法_第19张图片

4.3 对应代码

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

5 位置、速度融合

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

5.1 位置、速度融合方程

PX4 EKF中的多传感器融合方法_第20张图片

5.2 对应代码

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

附录

PX4 EKF中的多传感器融合方法_第21张图片

你可能感兴趣的:(PX4,EKF,GNSS相关,px4,EKF,组合导航)