本文介绍一篇使用 IMU/GPS 数据融合的行人定位论文，这里重点是理解本文提出的 Stop Detection 和 GPS Correction。

论文地址为：https://www.researchgate.net/publication/261452498_IMUGPS_based_pedestrian_localization

---

1. Introduction

低成本的 IMU 和 GPS 结合时可以提供行人准确的位置信息。本文提出了三种计算位置的策略：IMU数据二次积分、零速更新（Zero-velocity Update：ZUPT）、IMU和GPS融合的扩展卡尔曼滤波（EKF）。

在使用GPS进行定位时，容易受到高建筑物、山脉信号遮挡的影响，同时 GPS 的定位误差也比较大（1~10m左右）。而 IMU 则不会受到环境的影响，通过对加速度和角速度进行积分便可以得到位置、速度和方向信息。然而，IMU 积分导致的累计误差会随着时间不断增加，漂移现象往往非常严重。

本文研究目的是追踪室外行人，如上图1所示，脚上安装有Xsens 的IMU，可以测量行人的加速度和角速度， GPS模块用来获取 GPS 位置信息。

下面介绍行人定位中使用到的两个坐标，如下图2所示：惯性传感器坐标$B$ (有时也称为载体坐标)，世界坐标$W$（有时也称为导航坐标）。在世界坐标中，$X$ 轴指向地球表面 East 方向，$Y$ 轴指向地球表面 North 方向，Z 轴根据右手法则向上（其实这里使用的是ENU坐标）。$R_B^W$表示为从载体坐标到世界坐标的旋转变换（例如将载体坐标下的加速度和角速度转换为世界坐标下的加速度和角速度）。

---

2. Basic Position Calculation

下面介绍计算位置的最基础方法，由于使用的是Xsens 的IMU，传感器可以直接输出方向信息，这里用单位四元数$\mathbf{Q}=(q_w,q_1,q_2,q_3)$来表示$R_B^W$。

状态向量定义为：
$$\begin{array}{rl}{X}_k& ={\left({\mathbf{P}}_k{\mathbf{V}}_k{\mathbf{O}}_k\right)}^T\\ & ={\left(\begin{array}{lllllcccc}{x}_k& y_k& z_k& {\dot{x}}_k& {\dot{y}}_k& {\dot{z}}_k& {\varphi }_k& {\theta }_k& {\psi }_k\end{array}\right)}^T\end{array}$$

其中${\mathbf{P}}_k=(x_k,y_k,z_k{)}^T$是行人在世界坐标的位置，${\mathbf{V}}_k=({\dot{x}}_k,{\dot{y}}_k,{\dot{z}}_k{)}^T$是行人脚步运动速度，${\mathbf{O}}_k=({\varphi }_k,{\theta }_k,{\psi }_k{)}^T$是方向欧拉角。

载体加速度$a_k^B$到世界坐标加速度$a_k^W$的转换关系为：
$$a_k^W={\mathbf{Q}}_k\ast a_k^B\ast {\mathbf{Q}}_k^{\prime }-G$$

其中，$\ast$表示为四元数相乘，$\mathbf{G}=(0,0,g{)}^T$为地球重力向量。由于载体加速度包含重力加速度和运动加速度，因此这里转换到世界坐标系后要减去重力加速度。

四元数和欧拉角的转换关系：
$$\begin{array}{rl}{\mathbf{O}}_k& =\left(\begin{array}{c}{\varphi }_k\\ {\theta }_k\\ {\psi }_k\end{array}\right)=\left(\begin{array}{c}\mathrm{atan}2\left(2\left(q_w{q}_1+q_2{q}_3\right),1-2\left(q_1^2+q_2^2\right)\right)\\ \arcsin \left(2\left(q_w{q}_2-q_3{q}_1\right)\right)\\ \mathrm{atan}2\left(2\left(q_w{q}_3+q_1{q}_2\right),1-2\left(q_2^2+q_3^2\right)\right)\end{array}\right)\end{array}$$

根据二次积分，可以获得位置和速度信息，计算方程如下：
$$\begin{array}{rl}{X}_k(1:6)=\left(\begin{array}{l}{\mathbf{P}}_k\\ {\mathbf{V}}_k\end{array}\right)& =\left(\begin{array}{cc}\mathbf{I}& \mathbf{I}\ast T_S\\ 0& \mathbf{I}\end{array}\right)X_{k-1}(1:6)+\left(\begin{array}{c}{T}_S^2/2\ast \mathbf{I}\\ T_S\ast \mathbf{I}\end{array}\right)a_{k-1}^W\end{array}$$

其中，$T_s$是IMU采样时间间隔，$\mathbf{I}$是 $3\times 3$的单位阵，根据上述方程，就能得到行人在不同时刻的位置。然而，由于IMU本身固有的漂移现象，并不能得到可靠的位置信息。图3是数据采集时行人走过的真实运动轨迹，图4是计算轨迹，可以看到计算后的轨迹漂移现象非常严重。

---

3. Error Correction By Stop Detection

为了解决漂移现象，本文提出了 Stop Detection 方法。行走时，脚部其实是经历了一个静态阶段和运动阶段相互变换的过程，静态阶段速度应为0，因此当检测到静态阶段时应将速度设为0，这样就可以部分修正漂移现象，这种修正方法也被称为 zero-velocity updates (ZUPTs)。

作者使用了一个多条件的静态检测算法，具体如下：

其中，$\Vert {\mathbf{a}}_k\Vert ={\left[a_k^b(1{)}^2+a_k^b(2{)}^2+a_k^b(3{)}^2\right]}^{0.5}$是加速度模，${\sigma }_{a_k^b}^2=\frac{1}{2s+1}{\sum }_{j=k-s}^{k+s}\left(a_k^b-{\overline{a_k^b}}^2\right)$是加速度局部方差，$\Vert w_k\Vert ={\left[w_k^b(1{)}^2+w_k^b(2{)}^2+w_k^b(3{)}^2\right]}^{0.5}$是角速度模。

当同时满足以上条件时，就判定为静止，即 $C_{\text{stop }}=C1\&C2\&C3$。此时，位置和速度更新方程如下 （这里需要提醒的时，当判定为静止时，$C_{\text{stop}}$应为0才能保证速度初始化为0，这里与原文处理有一点不同）。
$$\begin{array}{rl}{X}_k(1:6)=\left(\begin{array}{l}{\mathbf{P}}_k\\ {\mathbf{V}}_k\end{array}\right)& =\left(\begin{array}{cc}\mathbf{I}& \mathbf{I}\ast T_S\\ 0& \mathbf{I}\ast C_{\text{stop}}\end{array}\right)X_{k-1}(1:6)+\left(\begin{array}{c}{T}_S^2/2\ast \mathbf{I}\\ T_S\ast \mathbf{I}\end{array}\right)a_{k-1}^W\end{array}$$

下面是 stop detection 结果和计算后的轨迹，漂移现象得到了很大的改善。

---

4. Error Correction By GPS

IMU导航的主要缺陷是其误差会随着时间不断累加。相反，GPS不会出现很大漂移现象。因此常见导航系统为GPS/IMU组合导航，本文作者提出使用EKF来修正IMU定位误差。

已知系统状态转移模型为：
$${\hat{\mathbf{X}}}_k=\left(\begin{array}{c}{\hat{\mathbf{P}}}_k\\ {\hat{\mathbf{V}}}_k\\ {\hat{\mathbf{O}}}_k\end{array}\right)=\underset{\mathbf{A}}{\underbrace{\left(\begin{array}{ccc}\mathbf{I}& \mathbf{I}\ast Ts& 0\\ 0& \mathbf{I}\ast C_{stop}& 0\\ 0& 0& 0\end{array}\right)}}{\mathbf{X}}_{k-1}+\underset{B}{\underbrace{\left(\begin{array}{c}{T}_s^2/2\ast \mathbf{I}\\ T_s\ast \mathbf{I}\\ 0\end{array}\right)}}{\mathbf{a}}_k^w+\left(\begin{array}{c}0\\ 0\\ Q2\mathrm{Euler}\left({\mathbf{Q}}_k\right)\end{array}\right)+{\mathbf{w}}_k$$

其中$\mathbf{A}$是$9\times 9$的状态转移矩阵，$\mathbf{B}$是控制矩阵，${\mathbf{w}}_k$是过程噪声。

测量模型为：
$${\mathbf{z}}_k=\mathbf{H}{\hat{\mathbf{X}}}_k+{\mathbf{n}}_k$$

其中${\mathbf{z}}_k$是预测测量，${\mathbf{H}}_k$是测量矩阵，${\mathbf{n}}_k$是测量噪声。

已知GPS输出为：经度、纬度、高度、航向和速度信息。一般只使用经度、纬度和航向信息，因为GPS的高度和速度信息噪声很大。这里需要对位置信息进行坐标转换处理，转换到世界坐标，坐标转换方程为：

$$x_{gps}=\cos (\varphi )\sqrt{\frac{1}{{\left(\frac{\sin (\varphi )}{a}\right)}^2+{\left(\frac{\cos (\varphi )}{c}\right)}^2}}\left[(\mathrm{lon}1-\mathrm{lon}0)\ast \frac{\pi }{180}\right]$$

$$y_{gps}=\sqrt{\frac{1}{{\left(\frac{\sin (\varphi )}{a}\right)}^2+{\left(\frac{\cos (\varphi )}{c}\right)}^2}}\left[(\mathrm{lat}1-\mathrm{lat}0)\ast \frac{\pi }{180}\right]$$

其中$\varphi =\frac{\pi }{2}-\frac{\mathrm{lat}0+\mathrm{lat}1}{2}\times \frac{\pi }{180}$，$(\mathrm{lon}0,\mathrm{lat}0)$是参考点位置信息，$(\mathrm{lon}1,\mathrm{lat}1)$是测量点坐标，$(a,c)$是地球长半轴和短半轴半径。

这里实际测量值为：
$${\mathbf{m}}_k=(x_{gps},y_{gps},{\psi }_{gps}{)}^T$$

根据测量值，可以得到测量矩阵为：
$$\mathbf{H}=\left(\begin{array}{ccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right)$$

卡尔曼更新方程为：
$${\mathbf{X}}_k={\hat{\mathbf{X}}}_k+{\mathbf{K}}_k\ast \left({\mathbf{m}}_k-\mathbf{H}{\hat{\mathbf{X}}}_k\right)$$

其中，卡尔曼增益${\mathbf{K}}_k$计算方程为：
$${\mathbf{K}}_k={\hat{\mathbf{\Sigma }}}_k{\mathbf{H}}^T{\left(\mathbf{H}{\hat{\mathbf{\Sigma }}}_k{\mathbf{H}}^T+{\mathbf{R}}_k\right)}^{-1}$$

其中${\hat{\mathbf{\Sigma }}}_k$是状态协方差矩阵，预测时协方差矩阵计算方程为：
$${\hat{\mathbf{\Sigma }}}_k=\mathbf{A}{\hat{\mathbf{\Sigma }}}_k{\mathbf{A}}^T+{\mathbf{N}}_k$$

GPS更新时协方差矩阵计算方程为：
$${\mathbf{\Sigma }}_k=({\mathbf{I}}_{9\times 9}-{\mathbf{K}}_k\mathbf{H}){\hat{\mathbf{\Sigma }}}_k({\mathbf{I}}_{9\times 9}-{\mathbf{K}}_k\mathbf{H}{)}^T+{\mathbf{R}}_k$$

这里介绍下方向修正，因为我们使用四元数表示方向，四元数修正方程为：
$${\mathbf{Q}}_k=\Delta {\mathbf{Q}}_{k-1}{\mathbf{Q}}_k$$

其中$\Delta {\mathbf{Q}}_{k-1}=\mathrm{Euler}2Q\left(0,0,{\mathbf{K}}_k\ast \left({\mathbf{m}}_k-\mathbf{H}{\hat{\mathbf{X}}}_k\right)(9)\right)$，将欧拉角转换为四元数。

整个算法流程如下：

最后是本文提出的定位算法在两个测试场景下的效果：