PX4 EKF模块解读含matlab代码

无人机开发

已于 2022-11-29 16:52:23 修改

阅读量842

点赞数 1

分类专栏：无人机导航制导与控制文章标签：程序人生

于 2022-11-29 16:47:18 首次发布

本文链接：https://blog.csdn.net/weixin_41230454/article/details/128101085

版权

无人机导航制导与控制专栏收录该内容

5 篇文章 5 订阅

订阅专栏

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

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

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

$x={{\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系的位置,， $\Delta {{\theta }_{b}}$ 表示陀螺仪角度增量的偏差， $\Delta {{v}_{b}}$ 为加速度计获取的速度增量偏差， ${{m}_{ned}}$ 为地磁向量在ned系的投影， ${{m}_{b}}$ 为磁力计的零偏， ${{v}_{wind}}$ 为水平方向的风速。

2、IMU传播

四元数传播

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

速度状态传播

$\Delta v=\Delta {{v}_{m}}-\Delta {{v}_{b}}+{{\eta }_{a}}$

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

位置状态传播

${{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的测量信息，即 $[\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}}$

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

$\theta =a\sin ({{a}_{mx}}/g)$

$\phi =-a\tan ({{a}_{my}}/{{a}_{mz}})$

2.2 航向角初始化

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

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

通过双天线获取
双天线单位矢量在体系的坐标为
$\text{ant }\!\!\_\!\!\text{ ve}{{\text{c}}_{b}}=[\cos (ant\_yaw);\text{sin(ant }\!\!\_\!\!\text{ yaw);0 }\!\!]\!\!\text{ }$ ，其中 $ant\_yaw$ 为双天线矢量与无人机纵轴之间的夹角，其方向为纵轴绕z轴进行顺时针旋转。
$\text{ant }\!\!\_\!\!\text{ ve}{{\text{c}}_{n}}={{C}_{nb}}\text{ant }\!\!\_\!\!\text{ ve}{{\text{c}}_{b}}$
即
$\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(\Phi ,dYaw)=angle2quat(\Phi )\otimes angle2quat({{C}_{bn}}{{[0,0,dYaw]}^{T}})$

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

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

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

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

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

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

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

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

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