文章目录
本文首先列出了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)更新:
其中状态转移矩阵和量测矩阵定义为如下雅可比矩阵:
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()函数。