PX4 EKF模块解读含matlab代码（下）

本章节继续上节介绍的Px4 中定位模块EKF内容进行介绍，其卡尔曼状态向量与协方差的预测以及卡尔曼滤波的初始化方法见上一章节，可关注微信公众号进行查看。

1、测量方程

1.1 磁力计

磁力计更新分三种情况，第一种方法是用三轴磁力计的数据作为三维观测，第二种方法是将磁力计数据转换为航向角，作为航向角的观测信息，第三种方法是利用磁偏角作为观测信息。

1、三轴磁力计

$$m_m=C_{bn}\ast m_{ned}+m_b$$
故其观测方程如下：
$m_m=H\ast x+{\eta }_m$，其中$H=\frac{\partial m_m}{\partial x}|x={\hat{x}}_k$，其中${\hat{x}}_k$为状态变量的第k步预测值，服从均值为0，标准差为_params.mag_noise的高斯分布。

2、磁航向

$$m_m=\text{atan2(}{C}_{nb}\text{(2,1),}{C}_{nb}\text{(1,1))}$$
其中由磁力计数据$m_m$转换为磁航向数据的方法见2.2节航向角初始化中的磁力计获取部分(见上篇），$C_{nb}$由$q$转换得到。

3、磁偏角

$$m_m=\text{atan2(}{m}_{ned}\text{(2),}{m}_{ned}\text{(1))}$$
$m_m$由查表或者参数得到。
故其观测方程如下：
$m_m=H\ast x+{\eta }_m$，其中$H=\frac{\partial m_m}{\partial x}|x={\hat{x}}_k$，其中${\hat{x}}_k$为状态变量的第k步预测值，服从均值为0，标准差为$r_m$的高斯分布。

1.2 光流

光流传感器的更新主要是速度的更新，其可以观测视线角速率。视线角速率是自主导航中一个非常重要的参数，其在光流中的计算公式如下：

$${\omega }_m={[{\omega }_x,{\omega }_y]}^T={[\frac{v_{by}}{range},-\frac{v_{bx}}{range}]}^T$$

记光流传感器视场中心为点p，imu中心为点i，ned坐标系原点记为o点。其中

$$v_b={{\dot{r}}_{op}}^b=[v_{bx};v_{by};v_{bz}]$$

即是op向量的变化率在体系的投影。

其计算公式分别如下：

$${r_{op}}^n={r_{oi}}^n+{r_{ip}}^n$$

$${r_{op}}^n={r_{oi}}^n+C_{nb}{r_{ip}}^b$$

$${{\dot{r}}_{op}}^n={{\dot{r}}_{oi}}^n+{\dot{C}}_{nb}{r_{ip}}^b=v+C_{nb}{{\omega }_{nb}}^{b\wedge }{r_{ip}}^b$$

$$v_b=C_{bn}{{\dot{r}}_{op}}^n=[v_{bx},v_{by},v_{bz}]$$

${{\omega }_{nb}}^{b\wedge }$是机体角速率的李代数。

$range$是沿光流传感器视场中心到地面的距离，记为常值，其计算公式如下:

首先计算imu中心至地面的高度$h_{oi}=p_{td}-p_d$ ，其中$p_{td}$为地面在ned系的高度，$p_d$为imu中心在ned系的高度，$h_{op}$为光流中心至地面的高度，$h_{ip}$为ip向量在高度方向的投影。

$$h_{op}=h_{oi}+h_{ip}=h_{oi}-C_{nb}{r_{ip}}^b(3)$$

$$range=\frac{h_{op}}{C_{bn}(3,3)}$$

故光流传感器的观测方程为：
$${\omega }_m=H\ast x+{\eta }_p$$

其中$H=\frac{\partial {\omega }_m}{\partial x}|x={\hat{x}}_k$，其中${\hat{x}}_k$为状态变量$x$的第k步预测值，${\eta }_p$是光流传感器的测量噪声，服从均值为0，标准差为$r_p$的高斯分布。

1.3 GPS

普通GPS的更新拆分为GPS速度、水平位置和高度两部分（统一在高度源部分进行介绍），当有双天线时，则多份航向角观测信息。

1、速度与水平位置

$$v_m={\dot{r}}_{oi}^n$$

$$p_m=r_{oi}^n$$

$$GP{S}_m=[v_m;p_m]$$

由GPS速度、水平位置信息经过空间同步至imu中心点的等效速度、水平位置。
其观测方程如下：

$$GP{S}_m=[v;p]=H\ast x+diag([{\eta }_{gv};{\eta }_{gp}])$$

${\eta }_{gv}$是gps速度的测量噪声，服从均值为0，标准差为$r_{gv}$的高斯分布；${\eta }_{gp}$是gps位置的测量噪声，服从均值为0，标准差为$r_{gp}$的高斯分布,H通过偏导可知为eye(5)。

2、双天线航向角

双天线单位矢量在ned系的坐标为$\text{ant }\_\text{ ve}{\text{c}}_n$，即双天线测量的方位角为

$${\phi }_m=a\tan 2(\text{ant }\_\text{ ve}{\text{c}}_n(2),\text{ant }\_\text{ ve}{\text{c}}_n(1))$$

故gps双天线的航向角测量方程如下：
${\phi }_m=H\ast x+{\eta }_{\phi }$，其中$H=\frac{\partial {\phi }_m}{\partial x}|x={\hat{x}}_k$，其中${\hat{x}}_k$为状态变量$x$的第k步预测值，${\eta }_{\phi }$为双天线航向角的测量噪声，服从均值为0，标准差为$r_{\phi }$的高斯分布。

1.4 空速计

空速计的测量值计算如下：

$${v_m}^{at}={\sqrt{\Vert v-[v_{wind};0]\Vert }}_2$$
故空速计测量方程如下：
${v_m}^{at}=H\ast x+{\eta }_{at}$，其中$H=\frac{\partial {v_m}^{at}}{\partial x}|x={\hat{x}}_k$，其中${\hat{x}}_k$为状态变量$x$的第k步预测值，${\eta }_{at}$真空速的测量噪声，服从均值为0，方差为$R_{at}$的高斯分布。由于空速计获得的是EAS(认为EAS==IAS)，而ekf中所用的测量信息为TAS，需要交EAS的测量噪声转换至TAS。

$$TAS=EAS\ast EAS2TAS$$

其中已知EAS噪声方差为sq(_params.eas_noise)。即可得TAS的方差.

$$R_{at}=R_{ae}\ast EAS2TA{S}^2$$

1.5 侧滑角

当为固定翼模式时，由于是BTT控制模式，故可以虚构侧滑角测量信息。

$$v_{bw}=C_{bn}(v-[v_{wind};0])$$

$${\beta }_m=a\tan 2(v_{bw}(2),v_{bw}(1))=0$$

侧滑角测量方程如下：
${\beta }_m=H\ast x+{\eta }_{\beta }$
其中$H=\frac{\partial {\beta }_m}{\partial x}|x={\hat{x}}_k$，其中${\hat{x}}_k$为状态变量$x$的第k步预测值，${\eta }_{\beta }$侧滑角的测量噪声，服从均值为0，标准差为$r_{\beta }$的高斯分布。

1.6 气动力

加速度计测量的阻力与侧向力的表达式如下：

$$acc{X}_m=-C_x\ast v_{bw}\text{(1)}\ast v_{bw}\text{(1)*}\rho /2\ast sign(v_{bw}\text{(1)})$$
$$acc{Y}_m=-C_y\ast v_{bw}\text{(2)}\ast v_{bw}\text{(2)*}\rho /2\ast sign(v_{bw}\text{(2)})$$

其中单位质量的阻力系数$C_x$、侧向力系数$C_y$为参数常值。

1.7 高度类传感器

可获取高度源的传感器有气压计、激光、gps、视觉。其测量方程一样。测量值由各类传感器转换至ned系高度。即量测方程如下：

$$P_{dm}=p(3)+{\eta }_h$$

${\eta }_h$高度的测量噪声，服从均值为0，标准差为$r_h$（由高度源决定）的高斯分布。

1.8 视觉

视觉传感器信息含有：世界系至当前机体系的旋转四元数，视觉传感器位置、速度在世界系的坐标投影，其中世界坐标系与ned系存在转换矩阵$C_{nw}$。 该坐标系转换参考坐标系的定义与转换章节（所有文章可见公众号）。

• 速度

$$v_m={\dot{r}}_{oi}^n$$
由视觉传感器获取的速度信息经坐标转换至ned系，再经过空间同步至imu中心点的等效速度。
其观测方程如下：

$$v_m=v=H\ast x+{\eta }_{ev}$$

$H=\frac{\partial v_m}{\partial x}|x={\hat{x}}_k$，其中${\hat{x}}_k$为状态变量$x$的第k步预测值，
${\eta }_{ev}$是视觉速度的测量噪声，服从均值为0，标准差为$r_{ev}$的高斯分布。

• 绝对位置

$$p_m=r_{oi}^n$$
由视觉传感器获取的位置信息经坐标转换至ned系，再经过空间同步至imu中心点的等效位置。
其观测方程如下：$p_m=p=H\ast x+{\eta }_{ep}$

• 里程计位置
  当gps测量的水平位置作为观测信息时，视觉位置则利用前后之差得到里程计信息，进行滤波，其观测方程同上，只是观测值$p_m$有所改变，变成里程计信息。

1.9 附加水平速度

地面传感器获得机体的水平速度信息，利用其构造的测量方程如下：

$$v_{hm}=v(1:2)=H\ast x+{\eta }_{av}$$

2 传感器的时空同步

由于不同传感器安装位置不同，其融合的时间戳不一致，会导致时间、空间不同步问题。
1、空间同步
ekf中机体系中心为imu中心，故需要将传感器获取的位置、速度信息转换至imu中心点在ned系的位置、速度投影。

记传感器为点s，imu中心为点i，ned坐标系原点记为o点。

其中$v={{\dot{r}}_{oi}}^n$ ，$p={r_{oi}}^n$
空间同步计算公式分别如下：

$${r_{oi}}^n={r_{os}}^n+{r_{si}}^n$$

$${r_{oi}}^n={r_{os}}^n+C_{nb}{r_{si}}^b$$

$${{\dot{r}}_{oi}}^n={{\dot{r}}_{os}}^n+{\dot{C}}_{nb}{r_{si}}^b={{\dot{r}}_{os}}^n+C_{nb}{{\omega }_{nb}}^{b\wedge }{r_{si}}^b$$

${{\omega }_{nb}}^{b\wedge }$是机体角速率的李代数。

2、时间同步
将所有传感器存储至回环里，且imu buffer存满至少一个回环后才启动卡尔曼滤波更新方程。并利用imu buffer进行滤波周期的控制，更新时则从传感器buffer中寻求先前与delay imu最相邻的信息进行融合。则每个传感器时间戳与当前delay imu时间戳不超过一帧。
为选择与当前传感器时间戳最近的imu进行融合，故对传感器时间戳扣除一半delay imu时间，使其同步时间戳降低至0.5个滤波周期。

3 卡尔曼滤波输出

通过降采样将IMU数据压入栈中，并利用栈中的数据进行卡尔曼滤波，有效解决了传感器的时间同步问题，但是引入了新的问题，即所估计的imu bias数据存在一定的滞后，故需要引入超前校正量IMU corrections对其进行修正。

Imu周期为$\Delta t_{imu}$ ，ekf周期为$\Delta t_{ekf}$，当前imu角度增量数据为$\Delta a$，体系速度增量数据为$\Delta v$，捷联惯导解算的上一周期姿态、位置、速度，z向位置、z向速度分别为$q_{bn}^{k-1}$、$v_n^{k-1}$、$p_n^{k-1}$、$v_{nz}^{k-1}$、$p_{nz}^{k-1}$， IMU corrections中对应的姿态误差项$\_delta\_angle\_corr$、位置对准误差项$pos\_correction$、速度对准误差项$vel\_correction$、z项速度对准误差项$vel\_d\_correction$。

当前ekf输出的四元数为$q_{b^{\prime }n}^{k-m}$（EKF中体系以下标$b^{\prime }$与惯导解算的体系$b$进行区分），同一时间轴线上output栈中四元数为$q_{bn}^{k-m}$ ，故其$b^{\prime }$系至$b$系的误差四元数：

$$q_{b{b}^{\prime }}^{k-m}=q_{n{b}^{\prime }}^{k-m}\otimes q_{bn}^{k-m}=q_{b{b}^{\prime }}^{k-m}\otimes q_{nb}^{k-m}\otimes q_{bn}^{k-m}$$

由$q_{b{b}^{\prime }}^{k-m}$求得$b$系至$b^{\prime }$系误差矢量角为$delta\_angle\_err$（转换方法见附录）。$q_{b{b}^{\prime }}^{k-m}$与当前时刻时延为$\tau$，即其传递函数为$e^{-\tau s}\approx \frac{A}{A+s}$，式中$A=1/\tau$。对$delta\_angle\_err$进行一阶滤波，其增益为$0.5\ast A$，即可获得$\_delta\_angle\_corr$。
其开环传递函数为：$G(s)=\frac{A}{A+s}\ast \frac{0.5\ast A}{s}$，故可得闭环特征方程为$s^2+As+0.5A^2=0$，故当前系统阻尼为$\frac{\sqrt{2}}{2}$。

当前拍角度变化修正量为：$\Delta a_c=\Delta a-\Delta {\theta }_b\ast \frac{\Delta t_{imu}}{\Delta t_{ekf}}+\_delta\_angle\_corr$， 由旋转矢量法得到$dq$，即 $q_{bn}^k=q_{bn}^{k-1}\otimes dq$。

当前拍速度变化修正量为：

$$\Delta v_c=\Delta v-\Delta v_b\ast \frac{\Delta t_{imu}}{\Delta t_{ekf}}$$
故速度:

$$v_n^k=v_n^{k-1}+inv(q_{bn}^k)\otimes \Delta v_c+[0;0;g]\ast \Delta t_{imu}+vel\_correction$$

$$p_n^k=p_n^{k-1}+(v_n^k+v_n^{k-1})/2\ast \Delta t_{imu}+pos\_correction$$

z向速度:

$$v_{nz}^{k-1}=v_{nz}^{k-1}+(inv(q_{bn}^k)\otimes \Delta v_c+[0;0;g]\ast \Delta t_{imu})[3]+vel\_d\_correction$$
z向位置:

$$p_{nz}^{k-1}=p_{nz}^{k-1}+((v_n^k+v_n^{k-1})/2\ast \Delta t_{imu})[3]+pos\_d\_correction$$

上式中对准误差$vel\_correction$采用PI对误差进行处理，处理后对buffer进行对准修正。其开环传递函数为：

$$G(s)=(\frac{1}{{\tau }_v}+\frac{0.1}{{\tau }_v^{2}s})\ast \frac{1}{s}$$

对准误差$pos\_correction$采用相同方法进行处理。其开环传递函数为：

$$G(s)=(\frac{1}{{\tau }_p}+\frac{0.1}{{\tau }_p^{2}s})\ast \frac{1}{s}$$

其中${\tau }_v$、${\tau }_p$为参数_params.vel_Tau，_params.pos_Tau。

z项速度对准误差项$vel\_d\_correction$采用PD对误差进行处理，处理后对buffer进行对准修正。其开环传递函数为：

$$G(s)=(\frac{1}{{\tau }_p}+\frac{1.1s}{{\tau }_p})\ast \frac{1}{s}\ast \frac{1}{s}$$

关注公众号，获取imu与磁力计融合定位代码

❤️ 扫一扫，添加我的公众号或者搜索【无人机开发】