PX4 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）量测方程： 其中， 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）更新：

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

四元数的更新方程如图片中（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的文档中，采用如下公式更新姿态四元数：

计算陀螺积分： 计算四元数增量： 计算四元数一步预测值：

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

计算加速度积分：

计算速度一步预测值 计算位置一步预测值

协方差阵预测相对较为复杂，需要推导雅可比方程，目前暂未进行推导。代码可以参考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​=Fk​Pk−1∣k−1​FkT​+Gk​Qk​Gk​+Qs​

其中， P k ∣ k − 1 P_{k|k-1} Pk∣k−1​为预测的协方差阵， Q k Q_k Qk​为IMU的噪声协方差， Q s Q_s Qs​为稳定滤波器所添加的额外过程噪声协方差阵。

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 Hx​k）为: Y轴、Z轴的雅可比矩阵求法类似，则三轴量测雅可比矩阵为：

假设 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​，则：

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

量测量磁偏角mag_delination 根据状态量计算磁偏角 atan2f(magE, magN) y k y _k yk​ = mag_delination-atan2f(magE,magN)

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

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

机体系到水平面的旋转矩阵为 C b n C_b^n Cbn​，但磁力计测量量旋转时，航向旋转角度应设为0。设俯仰角为θ，横滚为∅，则旋转矩阵为： （2）航向角测量值计算 设mag_declination为磁北和真北的航偏角，则水平面的磁感值为： 航向角为：

状态量如图1.1所示，利用状态量中的四元数计算航向角φ估计值： 求φ对各状态量偏导，具体求导过程参见附录，求偏导结果为： 量测雅可比矩阵为:

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

状态量如图1.1所示，则由状态量可知空速预测值TAS_pred为： 求TAS_pred相对于状态量的偏导，则有： 量测雅可比矩阵为:

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

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

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