PX4 ECL库代码及文档Github下载链接：PX4-ECL
系统状态、量测方程如下：
（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=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为稳定滤波器所添加的额外过程噪声协方差阵。
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轴的雅可比矩阵求法类似，则三轴量测雅可比矩阵为：
假设
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()函数。
