PX4 EKF模块解读含matlab代码_px4 时间同步-CSDN博客

这里主要介绍px4里面的定位模块，即EKF库。

1、状态向量与协方差的预测

1、Px4的状态向量为24维，其如下所示：

$x=[qvpΔθbΔvbmnedmbvwind]Tx={{\left[ \begin{matrix} q & v & p & \begin{matrix} \Delta {{\theta }_{b}} & \Delta {{v}_{b}} & {{m}_{ned}} & \begin{matrix} {{m}_{b}} & {{v}_{wind}} \\ \end{matrix} \\ \end{matrix} \\ \end{matrix} \right]}^{T}}$
其中 $q$ 表示NED系至体系的旋转四元数， $v$ 表示在ned系的速度， $p$ 表示机体系原点(imu位置）在ned系的位置,， $Δθb\Delta {{\theta }_{b}}$ 表示陀螺仪角度增量的偏差， $Δvb\Delta {{v}_{b}}$ 为加速度计获取的速度增量偏差， ${{m}_{ned}}$ 为地磁向量在ned系的投影， ${{m}_{b}}$ 为磁力计的零偏， ${{v}_{wind}}$ 为水平方向的风速。

2、IMU传播

四元数传播

$Δθ=Δθm-Δθb-Δθearth+η\Delta \theta =\Delta {{\theta }_{m}}\text{-}\Delta {{\theta }_{b}}\text{-}\Delta {{\theta }_{earth}}\text{+}\eta$
上式中 $Δθm\Delta {{\theta }_{m}}$ 为最优传感器输出的结果（通过标定、圆锥补偿、温补）， $Δθearth\Delta {{\theta }_{earth}}$ 为地球自转运动所产生的角度增量，$\eta $为均值为 0 ，方差为 d a V a r 的高斯白噪声。将$ \Delta \theta $转换为四元数$ \Delta {{q}_{k}}$得到，即；
$qk+1=qk⊗Δqk{{q}_{k+1}}={{q}_{k}}\otimes \Delta {{q}_{k}}$

速度状态传播

$Δv=Δvm−Δvb+ηa\Delta v=\Delta {{v}_{m}}-\Delta {{v}_{b}}+{{\eta }_{a}}$

上式中 $Δvm\Delta {{v}_{m}}$ 为最优加速度计传感器输出的结果（通过标定、圆锥补偿、温补）， $ηa{{\eta }_{a}}$ 为均值为0，方差为dvVar的高斯白噪声。故而得到
$vk+1=vk+CnbΔv+[0;0;g]Δt{{v}_{k+1}}={{v}_{k}}+{{C}_{nb}}\Delta v+[0;0;g]\Delta t$

位置状态传播

$pk+1=pk+(vk+1+vk)Δt/2{{p}_{k+1}}={{p}_{k}}+({{v}_{k+1}}+{{v}_{k}})\Delta t/2$

其它状态传播
其它状态全部建立为CV模型，故而
$KaTeX parse error: No such environment: align at position 8: \begin{̲a̲l̲i̲g̲n̲}̲ & \Delta {{\th…$

3、状态方程、协方差方程递推
卡尔曼滤波的状态递推方程如下：
${{x}_{k+1}}=f({{x}_{k}},{{u}_{k}})=F*{{x}_{k}}+G*{{u}_{k}}$
其中 ${{u}_{k}}$ 为imu的测量信息，即 $[Δθm,Δvm][\Delta {{\theta }_{m}},\Delta {{v}_{m}}]$ 。

2、卡尔曼滤波初始化

2.1 俯仰、滚转角初始化

当imu静止时，其加速度计敏感到的力为重力的反作用力，故其

${{a}_{m}}=-{{C}_{bn}}[0;0;g]$
${{C}_{bn}}={{\left[ \begin{matrix} \cos \psi \cos \theta & \cos \psi \sin \theta -\cos \phi \sin \psi & \sin \phi \sin \psi +\cos \phi \cos \psi \sin \theta \\ \cos \theta \sin \psi & \cos \phi \cos \psi +\sin \phi \sin \psi \sin \theta & \cos \phi \sin \psi \sin \theta -\cos \psi \sin \theta \\ -\sin \theta & \cos \theta \sin \phi & \cos \phi \cos \theta \\ \end{matrix} \right]}^{T}}$

即可以得到初始俯仰角和滚转角

$θ=asin⁡(amx/g)\theta =a\sin ({{a}_{mx}}/g)$

$ϕ=−atan⁡(amy/amz)\phi =-a\tan ({{a}_{my}}/{{a}_{mz}})$

2.2 航向角初始化

通过磁力计获取
建立临时体系，其坐标系为ned系旋转初始航向角所形成的坐标系 $b_1$ 系，通过 ${{m}^{b1}}={{C}_{b1b}}{{m}^{b}}$ 即获取初始磁向量在 $b_1$ 系的投影，即磁向量与 $b_1$ 系x轴之间的夹角为 $φ1=−atan⁡2(myb1,mxb1){{\varphi }_{1}}=-a\tan 2(m_{y}^{b1},m_{x}^{b1})$ 即真实航向角为 $φ=φ1+decl\varphi ={{\varphi }_{1}}+decl$ ，其中 $d e c l$ 为磁偏角，可通过查表获取。
通过视觉获取
由于通过视觉传感器信息可以获取导航系至体系的转换矩阵，其形式为 ${{C}_{bn}}$ ，故得到

$φ=atan⁡2(Cnb(2,1),Cnb(1,1))\varphi =a\tan 2({{C}_{nb}}(2,1),{{C}_{nb}}(1,1))$

通过双天线获取
双天线单位矢量在体系的坐标为
$⁣_ ⁣ ⁣ vecb=[cos⁡(ant_yaw);sin(ant ⁣ ⁣_ ⁣ ⁣ yaw);0 ⁣ ⁣] ⁣ ⁣ \text{ant }\!\!\_\!\!\text{ ve}{{\text{c}}_{b}}=[\cos (ant\_yaw);\text{sin(ant }\!\!\_\!\!\text{ yaw);0 }\!\!]\!\!\text{ }$ ，其中 $ant\_yaw$ 为双天线矢量与无人机纵轴之间的夹角，其方向为纵轴绕z轴进行顺时针旋转。
$⁣_ ⁣ ⁣ vecn=Cnbant ⁣ ⁣_ ⁣ ⁣ vecb\text{ant }\!\!\_\!\!\text{ ve}{{\text{c}}_{n}}={{C}_{nb}}\text{ant }\!\!\_\!\!\text{ ve}{{\text{c}}_{b}}$
即
$⁣_ ⁣ ⁣ vecn(2),ant ⁣ ⁣_ ⁣ ⁣ vecn(1))−ant_yaw\varphi =a\tan 2(\text{ant }\!\!\_\!\!\text{ ve}{{\text{c}}_{n}}(2),\text{ant }\!\!\_\!\!\text{ ve}{{\text{c}}_{n}}(1))-ant\_yaw$

2.3 高度初始化

利用气压计或者外部视觉高度进行初始化，由_params.vdist_sensor_type进行控制。

2.4 Ned系地磁向量初始化

由磁力计测得的磁矢量为 ${{m}_{m}}$ （已扣除先前估计的磁零偏值）， ${{m}_{ned}}={{C}_{nb}}{{m}_{m}}$
，其中 ${{C}_{nb}}$ 由初始化所得到的姿态角获取。

2.5 其余状态初始化

其余状态全部初始化为0。

2.6 协方差初始化

已知姿态角初始方差 ${{R}_{0}}$ 为sq(_params.initial_tilt_err)

$q=f(Φ,dYaw)=angle2quat(Φ)⊗angle2quat(Cbn[0,0,dYaw]T)q=f(\Phi ,dYaw)=angle2quat(\Phi )\otimes angle2quat({{C}_{bn}}{{[0,0,dYaw]}^{T}})$

其中$\Phi $为姿态角, $d Y a w$ 为对准后的航向误差，其服从均值为0，方差为
${{R}_{dYaw}}$
（由对应航向传感器参数决定）。即 $q=H∗Φ+G∗dYawq=H*\Phi +G*dYaw$ ，其中 $H=∂f∂Φ∣Φ=Φ0H=\frac{\partial f}{\partial \Phi }\left| \Phi ={{\Phi }_{0}} \right.$ , $Φ0{{\Phi }_{0}}$ 为初始化姿态角， $G=∂f∂dYaw∣dYaw=0G=\frac{\partial f}{\partial dYaw}\left| dYaw=0 \right.$ 。

即 $q$ 对应的协方差为 ${{R}_{q}}=H*{{R}_{0}}*{{H}^{T}}+G{{R}_{dYaw}}{{G}^{T}}$

水平速度项噪声为 $⁣_ ⁣ ⁣ params.gps ⁣ ⁣_ ⁣ ⁣ vel ⁣ ⁣_ ⁣ ⁣ noise, 0.01f)){{R}_{vh}}=\text{sq(fmaxf( }\!\!\_\!\!\text{ params}\text{.gps }\!\!\_\!\!\text{ vel }\!\!\_\!\!\text{ noise, 0}\text{.01f))}$

垂向速度项噪声为 ${{R}_{vv}}={{1.5}^{2}}{{R}_{vh}}$

水平位置项噪声为 $⁣_ ⁣ ⁣ params.gps ⁣ ⁣_ ⁣ ⁣ pos ⁣ ⁣_ ⁣ ⁣ noise, 0.01f)){{R}_{ph}}=\text{sq(fmaxf( }\!\!\_\!\!\text{ params}\text{.gps }\!\!\_\!\!\text{ pos }\!\!\_\!\!\text{ noise, 0}\text{.01f))}$

垂向位置项噪声 $R_{pv}$ 则由不同高度源对应的标准差参数确定。

陀螺零偏的初始方差为
$⁣_ ⁣ ⁣ params.switch ⁣ ⁣_ ⁣ ⁣ on ⁣ ⁣_ ⁣ ⁣ gyro ⁣ ⁣_ ⁣ ⁣ bias, 0.01f)){{R}_{\Delta {{\theta }_{b}}}}=\text{sq(fmaxf( }\!\!\_\!\!\text{ params}\text{.switch }\!\!\_\!\!\text{ on }\!\!\_\!\!\text{ gyro }\!\!\_\!\!\text{ bias, 0}\text{.01f))}$

加速度计零偏的初始方差为

$⁣_ ⁣ ⁣ params.switch ⁣ ⁣_ ⁣ ⁣ on ⁣ ⁣_ ⁣ ⁣ acc ⁣ ⁣_ ⁣ ⁣ bias, 0.01f)){{R}_{\Delta {{v}_{b}}}}=\text{sq(fmaxf( }\!\!\_\!\!\text{ params}\text{.switch }\!\!\_\!\!\text{ on }\!\!\_\!\!\text{ acc }\!\!\_\!\!\text{ bias, 0}\text{.01f))}$

其imu最优传感器切换时，即复位于上述值。

$m_{ned}$ 、 $m_b$ 的初始方差为 $⁣_ ⁣ ⁣ params.mag ⁣ ⁣_ ⁣ ⁣ noise){{R}_{m}}=sq(\text{ }\!\!\_\!\!\text{ params}\text{.mag }\!\!\_\!\!\text{ noise})$

$v_{wind}$ 的初始方差为 $⁣_ ⁣ ⁣ params.initial ⁣ ⁣_ ⁣ ⁣ wind ⁣ ⁣_ ⁣ ⁣ uncertainty){{R}_{{{v}_{wind}}}}=sq(\text{ }\!\!\_\!\!\text{ params}\text{.initial }\!\!\_\!\!\text{ wind }\!\!\_\!\!\text{ uncertainty})$