An Improved Heuristic Drift Elimination Method for Indoor Pedestrian Positioning

Abstract

Indoor positioning is currently a research hotspot. In recent years, Pedestrian Dead Reckoning (PDR) has been widely used in indoor positioning. However, the positioning error caused by heading drifts will accumulate as the walking distance increases, so some methods need to be used to correct the heading angle. Heuristic Drift Elimination (HDE) is an effective heading correction algorithm, which only uses the information of a building’s dominant directions to reduce the heading error, but it does not apply to the non-dominant direction condition. In this paper, we propose a heading drift suppressing method for the limitation of HDE. Firstly, the method constructs membership functions to judge the pedestrian’s motion according to the result of comprehensive evaluation. Then, it further determines by a threshold condition whether the pedestrian walks along the dominant directions, and a heading error measurement is introduced for heading correction. Finally, we verify by experiments that the proposed method can correct heading angles properly for different conditions, which indicates an adaptability to the environment.

1. Introduction

As technology advances, people’s lives are becoming more and more intelligent, and the demand for location services is gradually increasing. Therefore, the development of indoor positioning technology has drawn wide attention from people. It has broad application prospects in target monitoring, emergency rescue, shopping guides, and so on.

Currently, indoor positioning relies mainly on WiFi, Bluetooth, UWB (Ultra Wide Band), and other wireless location technologies, as well as optical communications, machine vision, and so forth. Using the above positioning technologies can achieve good results. For instance, when we use UWB, the positioning accuracy can even reach centimeter level. However, due to the layout of reference beacons in advance and a high cost, its application is limited. Thus, some researchers apply inertial navigation technology to indoor positioning. With the data collected by MEMS (Micro-Electro-Mechanical Systems)-integrated IMUs (Inertial Measurement Units), the pedestrian’s position can be calculated. This method can effectively resist environmental disturbance and has strong autonomy, but cumulative errors caused by MEMS devices cannot be ignored. Foxlin [1] first proposed the IEZ framework, that is, Inertial Navigation System (INS), Extended Kalman Filter (EKF), and Zero Velocity Update (ZUPT) to estimate the motion track. Some researchers carried out their work based on this method [2,3]. The framework restrains the horizontal attitude errors well, but it cannot make the heading correction. Hence, many scholars at home and abroad have conducted in-depth studies. Yang Hong et al. [4,5] adopted a geomagnetic correction algorithm to correct the heading angle by taking the difference of courses calculated by the magnetometer and attitude matrix as a measurement. Afzal [6] estimated the heading error by capturing changes in the magnetic field measured by magnetometers in the pedestrian’s stationary state. Since the magnetometer suffers more perturbations in indoor environments, the reliability of its calculated heading angle cannot be guaranteed. Therefore, some people believe that the magnetometer is not suitable for indoor use [7]. To improve the accuracy of heading calculation, Widyawan et al. [8] fused INS and visual image information, but this method was greatly affected by the light intensity. Similarly, some researchers used the particle filter method to estimate the heading angle by getting the interior plan of the building in advance [9], but most of the time it was hard to do that. Borenstein [10] proposed the Heuristic Drift Elimination (HDE) algorithm in 2010. The algorithm is based on the fact that most walls and corridors inside buildings are made up of straight lines and right angles and use only four or eight dominant directions as a reference without prior information about the environment, which can effectively reduce the heading drift.

However, as Jiménez mentioned in Reference [11], the HDE algorithm is suitable for pedestrians walking along the dominant directions, but sometimes the building does not all consist of paths along the dominant directions. Special structures, such as circular arcs, may exist, and the heading angle will be wrongly corrected if the HDE algorithm is used in this case. Thus, the algorithm needs to be improved in some ways.

In this paper, we conducted a study of the heading correction method based on the IEZ framework for the problems above. We first describe the basic IEZ framework for getting the pedestrian’s motion information, propose the improved HDE (IHDE) method, and then verify the validity of proposed method by experiments.

2. Pedestrian Dead Reckoning (PDR) Implementation with Kalman Filter

PDR is implemented based on a Kalman filter. The method is to obtain the motion information by calculating the inertial sensor data. When the footfall of the pedestrian is detected, the EKF measurements will be updated by ZUPT and the estimated errors are used to correct the motion information. The 15-dimensional error state vector of EKF is $\delta {\mathit{x}}_k={\left[\begin{array}{cccc}\begin{array}{cc}\delta {\mathit{r}}_k& \delta {\mathit{v}}_k\end{array}& \delta {\mathit{\phi }}_k& \delta {\mathit{a}}_k^b& \delta {\mathit{\omega }}_k^b\end{array}\right]}^T$. The vector contains the errors in position, velocity, and attitude, as well as the estimated biases for accelerometers and gyroscopes, each of which consists of its 3-axis components. k indicates the moment of data sampling. Figure 1 shows the PDR implementation process.

An accelerometer and gyroscope are mounted on the human body to collect data related to the motion information. The Micro-Inertial-Navigation System (MINS) block then calculates the data. The Zero Velocity Detector (ZVD) block is used to detect zero speed moments to trigger ZUPT and IHDE blocks, and these two blocks provide the EKF block with measurements to estimate errors.

2.1. Micro-Inertial-Navigation System

The MINS block calculates the data collected by the IMU to get the position, velocity, and attitude information. The process is divided into 4 steps:

• Data Preprocessing: Due to the drift characteristics of inertial sensors, we compensate the raw data ${\mathit{a}}_k$ and ${\mathit{\omega }}_k$ by the estimated errors $\delta {\mathit{a}}_k^b$ and $\delta {\mathit{\omega }}_k^b$ before calculating them.
• Attitude Update: We update the quaternion to get the pedestrian’s attitude information. The quaternion updating method is as follows:

  $${\mathit{Q}}_k=\left(\cos \frac{\Delta }{2}\cdot {\mathit{I}}_{4\times 4}+\frac{2}{\Delta }\sin \frac{\Delta }{2}\cdot \Delta \mathit{\theta }\right){\mathit{Q}}_{k-1}.$$

  (1)

  ${\mathit{Q}}_k={\left[\begin{array}{cccc}{q}_0& q_1& q_2& q_3\end{array}\right]}^T$ represents the quaternion, and ${\mathit{I}}_{4\times 4}$ is a 4 by 4 unit matrix. Since the sampling period $\Delta t$ is very short,

  $$\Delta \mathit{\theta }=\frac{1}{2}\left[\begin{array}{cccc}0& -\Delta {\theta }_x& -\Delta {\theta }_y& -\Delta {\theta }_z\\ \Delta {\theta }_x& 0& \Delta {\theta }_z& -\Delta {\theta }_y\\ \Delta {\theta }_y& -\Delta {\theta }_z& 0& \Delta {\theta }_x\\ \Delta {\theta }_z& \Delta {\theta }_y& -\Delta {\theta }_x& 0\end{array}\right].$$

  (2)

  $\Delta {\theta }_i$($i=x,y,z$) indicates the angle increment of gyroscope, and $\Delta =\sqrt{{\left(\Delta {\theta }_x\right)}^2+{\left(\Delta {\theta }_y\right)}^2+{\left(\Delta {\theta }_z\right)}^2}$ is the modulus of angle increments. The attitude angles can then be calculated by the quaternion as:

  $$\begin{array}{l}\gamma =\arctan \left(2\left(q_2{q}_3-q_0{q}_1\right)/\left(q_3{}^2-q_2{}^2-q_1{}^2+q_0{}^2\right)\right)\\ \theta =\arcsin \left(2\left(q_0{q}_2-q_1{q}_3\right)\right)\\ \psi =\arctan \left(2\left(q_1{q}_2+q_0{q}_3\right)/\left(q_1{}^2+q_0{}^2-q_2{}^2-q_3{}^2\right)\right)\end{array}.$$

  (3)

  $\gamma$, $\theta$ and $\psi$ are the roll, pitch, and yaw angle, respectively.
• Position and Velocity Update: We first transform the acceleration in the sensor body frame to the navigation frame with the attitude matrix, and then remove the gravitational acceleration:

  $${\overline{\mathit{a}}}_k=C_b^t{\mathit{a}}_k+{[\begin{array}{ccc}0& 0& g\end{array}]}^T.$$

  (4)

  ${\overline{\mathit{a}}}_k$ represents the free acceleration, and ${[\begin{array}{ccc}0& 0& g\end{array}]}^T$ the gravitational acceleration with g’s value chosen as 9.80665 m/s². $C_b^t$ is the attitude matrix, where t refers to the navigation frame (defined to be North-East-Down on the ground) and b is the body frame. The pedestrian’s position and velocity are updated by the integral of ${\overline{\mathit{a}}}_k$.
• Motion Information Correction: If a zero speed moment is detected, because theoretically the pedestrian’s velocity should be zero, we take the difference between zero and ${\mathit{v}}_k$, that is,

  $$\Delta {\mathit{v}}_k={[\begin{array}{ccc}0& 0& 0\end{array}]}^T-{\mathit{v}}_k$$

  (5)

  as the measurements, and estimate the error state by EKF to correct the motion information. The position and velocity can be directly corrected by $\delta {\mathit{r}}_k$ and $\delta {\mathit{v}}_k$. The attitude correction is equivalent to the correction of attitude matrix:

  $${\overline{\mathit{C}}}_b^t=\left({\mathit{I}}_{3\times 3}-{\mathsf{\Delta }}_t\right){\mathit{C}}_b^t$$

  (6)

  where ${\mathsf{\Delta }}_t$ is the skew symmetric matrix for $\delta {\mathit{\phi }}_k$. According to the conversion relationship between the attitude matrix and quaternion, the quaternion can then be corrected.

2.2. Extended Kalman Filter

● State Equation

$$\delta {\mathit{x}}_{k/k-1}={\mathit{F}}_{k/k-1}\delta {\mathit{x}}_{k-1}+{\mathit{G}}_{k/k-1}{\mathit{w}}_{k-1}$$

(7)

$\delta {\mathit{x}}_{k/k-1}$ is the predicted error state, and $\delta {\mathit{x}}_{k-1}$ is the last filtered error state. ${\mathit{w}}_{k-1}$ is the process noise with covariance matrix ${\mathit{Q}}_{k-1}=E\left({\mathit{w}}_{k-1}{\mathit{w}}_{k-1}{}^T\right)$. The state transition matrix ${\mathit{F}}_{k/k-1}$ and noise driven matrix ${\mathit{G}}_{k/k-1}$ corresponding to the error state are derived from the error model of INS as follows [12]:

$$\begin{array}{l}{\mathit{F}}_{k/k-1}=\left[\begin{array}{ccccc}{\mathit{I}}_{3\times 3}& {\mathit{I}}_{3\times 3}\cdot \Delta t& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}\\ {\mathit{O}}_{3\times 3}& {\mathit{I}}_{3\times 3}& \mathit{\Omega }\cdot \Delta t& {\mathit{C}}_b^t\cdot \Delta t& {\mathit{O}}_{3\times 3}\\ {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{I}}_{3\times 3}& {\mathit{O}}_{3\times 3}& -{\mathit{C}}_b^t\cdot \Delta t\\ {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{I}}_{3\times 3}& {\mathit{O}}_{3\times 3}\\ {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{I}}_{3\times 3}\end{array}\right]\\ {\mathit{G}}_{k/k-1}=\left[\begin{array}{cccc}{\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}\\ {\mathit{C}}_b^t\cdot \Delta t& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}\\ {\mathit{O}}_{3\times 3}& -{\mathit{C}}_b^t\cdot \Delta t& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}\\ {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{I}}_{3\times 3}\cdot \Delta t& {\mathit{O}}_{3\times 3}\\ {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{I}}_{3\times 3}\cdot \Delta t\end{array}\right]\end{array}.$$

(8)

$\mathit{\Omega }$ is the skew symmetric matrix for the acceleration transformed to the navigation frame. ${\mathit{O}}_{3\times 3}$ refers to a 3 by 3 zero matrix.

● Measurement Equation

$${\mathit{z}}_k={\mathit{H}}_k\delta {\mathit{x}}_{k/k-1}+{\mathit{\eta }}_k$$

(9)

where ${\mathit{z}}_k$ represents the measurements, and ${\mathit{\eta }}_k$ is the measurement noise with covariance matrix ${\mathit{R}}_k$ equal to $E\left({\mathit{\eta }}_k{\mathit{\eta }}_k{}^T\right)$. Since the measurements are velocity errors of three axes, the measurement matrix is

$${\mathit{H}}_k=\left[\begin{array}{ccccc}{\mathit{O}}_{3\times 3}& {\mathit{I}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}\end{array}\right].$$

(10)

We divide the role of EKF into two cases: if the zero speed moment is not detected, then EKF will only be updated in time, that is, only the last filtered error covariance matrix ${\mathit{P}}_{k-1}$ is updated when ${\mathit{z}}_k$ is obtained [13].

$${\mathit{P}}_{k/k-1}={\mathit{F}}_{k/k-1}{\mathit{P}}_{k-1}{\mathit{F}}_{k/k-1}{}^T+{\mathit{G}}_{k/k-1}{\mathit{Q}}_{k-1}{\mathit{G}}_{k/k-1}{}^T$$

(11)

${\mathit{P}}_{k/k-1}$ is the predicted error covariance matrix, and ${\mathit{P}}_k$ is equal to ${\mathit{P}}_{k/k-1}$ at this moment. In the other case, EKF is updated in state with ${\mathit{z}}_k$ as below:

$$\delta {\mathit{x}}_k=\delta {\mathit{x}}_{k/k-1}+{\mathit{K}}_k\left({\mathit{z}}_k-{\mathit{H}}_k\delta {\mathit{x}}_{k/k-1}\right).$$

(12)

${\mathit{K}}_k$ is the filter gain.

$${\mathit{K}}_k={\mathit{P}}_{k/k-1}{\mathit{H}}^T{\left(\mathit{H}{\mathit{P}}_{k/k-1}{\mathit{H}}^T+{\mathit{R}}_k\right)}^{-1}$$

(13)

2.3. Zero Velocity Detector

The accuracy of zero speed moment detection is closely related to the calculation result. The pedestrian’s movement can be divided into two cases: station and motion, denoted as $C_1$ and $C_2$, respectively. Specifically, the foot swings forward when moving and clings to the ground in station case [14]. We determine whether n is zero speed moment based on the generalized likelihood ratio (GLRT) method [15]:

$$L_G\left({\mathit{m}}_n\right)=\frac{p\left({\mathit{m}}_n;C_1\right)}{p\left({\mathit{m}}_n;C_2\right)}.$$

(14)

$L_G\left({\mathit{m}}_n\right)$ indicates GLRT, and ${\mathit{m}}_n$ refers to the sampling sequence.

$${\mathit{m}}_n=\left\{\left[\begin{array}{c}{\mathit{a}}_k\\ {\mathit{\omega }}_k\end{array}\right]\right\}\hspace{1em}k=n,n+1,\dots ,n+N-1$$

(15)

where N is the length of selected sliding window, and $p\left({\mathit{m}}_n;C_1\right)$ and $p\left({\mathit{m}}_n;C_2\right)$ are the probability density functions for $C_1$ and $C_2$. According to the multiplication principle in probability statistics and considering the independence of two-dimensional random variables,

$$p\left({\mathit{m}}_n;C_j\right)={\displaystyle \prod _{k=n}^{n+N-1}p\left({\mathit{a}}_k;C_j\right)p\left({\mathit{\omega }}_k;C_j\right)\hspace{1em}j=1,2}.$$

(16)

From Equations (14) and (16) we know, to obtain $L_G\left({\mathit{m}}_n\right)$, it is necessary to get the probability distributions of ${\mathit{a}}_k$ and ${\mathit{\omega }}_k$ in advance, but in practice this information is unavailable. So, we represent the sample at each moment as:

$$\left[\begin{array}{c}{\mathit{a}}_k\\ {\mathit{\omega }}_k\end{array}\right]=\left[\begin{array}{c}{\mathit{a}}_k^r\\ {\mathit{\omega }}_k^r\end{array}\right]+{\mathit{n}}_k.$$

(17)

The ideal sampling acceleration ${\mathit{a}}_k^r$ and angular velocity ${\mathit{\omega }}_k^r$ meet the following conditions: in motion case, ${\mathit{a}}_k^r$ is not equal to the gravitational acceleration g, and ${\mathit{\omega }}_k^r$ is not equal to 0; comparatively, they are both equal to the ideal values in the other case. ${\mathit{n}}_k$ is the sampling noise. If we assume it to be zero-mean white noise with the noise variances of the accelerometer and gyroscope ${\sigma }_a{}^2$ and ${\sigma }_{\omega }{}^2$, respectively, then ${\mathit{a}}_k$ and ${\mathit{\omega }}_k$ follow the Gaussian distribution. Hence, based on the above equations we obtain the probability density functions as below:

$$p\left({\mathit{m}}_n;C_j\right)=\frac{1}{{\left(\sqrt{2\pi }{\sigma }_a\right)}^N}\cdot \frac{1}{{\left(\sqrt{2\pi }{\sigma }_{\omega }\right)}^N}\cdot \exp \left(-{\displaystyle \sum _{k=n}^{n+N-1}\left(\frac{1}{2{\sigma }_a{}^2}{\Vert {\mathit{a}}_k-{\mathit{a}}_k^r\Vert }^2+\frac{1}{2{\sigma }_{\omega }{}^2}{\Vert {\mathit{\omega }}_k-{\mathit{\omega }}_k^r\Vert }^2\right)}\right).$$

(18)

According to the basic principle, the GLRT corresponding to moment n can be calculated by maximizing Equation (18) with a proper selection of ${\mathit{a}}_k^r$ and ${\mathit{\omega }}_k^r$. We substitute the maximum values for Equation (14) and reorganize the result:

$$L_G{}^{\prime }\left({\mathit{m}}_n\right)={\displaystyle \sum _{k=n}^{n+N-1}\left(\frac{1}{{\sigma }_a{}^2}{\Vert {\mathit{a}}_k-g\frac{{\overline{\mathit{a}}}_n}{\Vert {\overline{\mathit{a}}}_n\Vert }\Vert }^2+\frac{1}{{\sigma }_{\omega }{}^2}{\Vert {\mathit{\omega }}_k\Vert }^2\right)}<t{h}_G.$$

(19)

${\overline{\mathit{a}}}_n$ represents the average value of acceleration samples in the sliding window. Through the above equation we can determine the pedestrian’s movement at moment n. If $L_G{}^{\prime }\left({\mathit{m}}_n\right)$ is less than the threshold $t{h}_G$, then $C_1$ is true, as shown in red in Figure 2, or $C_2$ is true otherwise.

2.4. The Improved HDE Method

● Limitations of original methods

Although using the IEZ framework has a significant effect upon correcting the horizontal attitude angles, it influences little on heading correction. The reason is that, according to the observability analysis of INS error equations, the error state $\delta {\psi }_k$ is unobservable [16]. Thus, the estimated trajectory will gradually deviate from the expected as the moving distance increases.

In indoor environments, the HDE algorithm can effectively correct the heading angle by taking the dominant directions of the building as a reference. However, not all motion paths in indoor environments are along the dominant directions. If the HDE algorithm is still used in a non-dominant direction condition, as can be seen in Figure 3, the heading angle will be overcorrected, and a new heading error is then generated, which will lead to the wrong estimation of the motion track. Therefore, this problem needs to be considered in practice.

● Details of the proposed method

Based on the above reasons, we first determined whether the pedestrian walked in a straight line. A fixed number of successive steps are taken as the unit of calculation, and all the calculated heading angles of steps are put into a dataset. The standard deviation of the dataset is then used to determine whether the current motion path is a straight line. Considering the complexity of calculation and accuracy of results, we take W (W = 5) steps as the unit of calculation [17], and the heading angle of step s is calculated by MINS. The dataset of heading angles is represented as $\mathsf{\Psi }=\{{\psi }_l|l=s,s-1,\dots ,s-W+1\}$, whose standard deviation is obtained by the following equations:

$$\{\begin{array}{l}std=\sqrt{\frac{1}{W-1}{\displaystyle \sum _{l=s-W+1}^s{\left({\psi }_l-\overline{\psi }\right)}^2}}\\ \overline{\psi }=mean\left(\mathsf{\Psi }\right)\end{array}.$$

(20)

$\overline{\psi }$ is the average of heading angles. As the pedestrian cannot walk strictly along the actual path, by learning from fuzzy mathematics theory, we take std as variable and use the Z-type membership function to describe the motion path.

$${\mu }_1\left(std\right)=\frac{1}{1+\exp \left(k_1\cdot std-a\right)}\hspace{1em}{k}_1>0,a>0$$

(21)

${\mu }_1\left(std\right)$ ranges from 0 to 1. The membership degree of motion path to the straight line decreases as std becomes larger. Figure 4 reflects the trend of the function.

Second, as the normal walking process of the pedestrian is periodic, the time interval between two adjacent steps, denoted as $S{T}_s$, is relatively uniform. If $S{T}_s$ is out of the normal range, the pedestrian may be in abnormal activities, and to prevent the heading angle from being wrongly corrected in this condition, the proposed method should be avoided.

Hence, we take $S{T}_s$ as variable and adopt the commonly used bell-shaped membership function to characterize the pedestrian’s motion state.

$${\mu }_2\left(S{T}_s\right)=\frac{1}{1+b{\left(S{T}_s-{\gamma }_T\right)}^{2c}}\hspace{1em}b>0,c>0$$

(22)

where ${\gamma }_T$ is associated with the motion period, usually set to 1.0~1.2 s. Combined with the motion path and motion state, we can make a comprehensive evaluation of the pedestrian’s motion condition and the evaluation vector M is ${[\begin{array}{cc}{\mu }_1(std)& {\mu }_2(S{T}_s)\end{array}]}^T$. Though the conditions of triggering the proposed method mainly depend on whether the motion path is a straight line, the role of the method needs to be limited in abnormal motion state. Therefore, we adopt $M(.,+)$ model, and with the weight vector $w=[\begin{array}{cc}{w}_1& w_2\end{array}]$, the comprehensive evaluation is

$$E=w\cdot M=w_1{\mu }_1(std)+w_2{\mu }_2(S{T}_s).$$

(23)

Then we give a binary controller as below:

$$En=\{\begin{array}{cc}1& E>{\gamma }_2\\ 0& else\end{array}.$$

(24)

If E is greater than the threshold ${\gamma }_2$, then En is set to 1 and the proposed method will be triggered, or the method has no effect otherwise. Figure 5 shows the analysis process above.

Finally, we get the error measurement $\delta \psi$. If En is 0, $\delta \psi$ will be set to 0 as well, or $\delta \psi$ is obtained by determining whether the pedestrian walks along the dominant directions. We define 4 dominant directions denoted as ${\psi }_d$ (d = 1, 2, 3, 4) to be 0°, 90°, 180°, and 270° respectively, and select 5° and 355° as the thresholds.

$$\left|{\psi }_d-\overline{\psi }\right|<5^{\circ }\Vert \left|{\psi }_d-\overline{\psi }\right|>{355}^{\circ }$$

(25)

If the above equation holds for a certain ${\psi }_d$, then it will be taken as the closest dominant direction ${\psi }_{dominant}$ corresponding to the current heading angle. By subtracting the current heading angle from ${\psi }_{dominant}$ we obtain

$$\delta \psi ={\psi }_{dominant}-{\psi }_s.$$

(26)

If, instead, Equation (26) holds for no ${\psi }_d$, then ${\psi }_{dominant}$ will be substituted for $\overline{\psi }$ as the reference direction to calculate $\delta \psi$ [18]. After that, this error, together with the velocity errors of three axes, is taken as the filter measurements to estimate and correct the heading error, as well as other inertial sensor errors. The measurement matrix in this condition is

$$\mathit{H}=\left[\begin{array}{ccccc}{\mathit{O}}_{3\times 3}& {\mathit{I}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}\\ {\mathit{O}}_{1\times 3}& {\mathit{O}}_{1\times 3}& \left[\begin{array}{ccc}0& 0& 1\end{array}\right]& {\mathit{O}}_{1\times 3}& {\mathit{O}}_{1\times 3}\end{array}\right].$$

(27)

3. Tests

3.1. IMU Description

We used the IMU MTi-G-710 produced by Xsens Technologies B.V in Enschede, The Netherlands, which integrates 3-axis MEMS sensors. The IMU was easily installed on the human body, and had a large measuring range and a small size, and we fixed it to the left foot of a person to collect data.

Table 1. Technical parameters of MTI-G-710.

	Accelerometers	Gyroscopes
Full Scale (FS)	±15 g	±1000°/s
Non-linearity	0.03% FS	0.01% FS
Bias stability	40 μg	10°/h
Bandwidth	375 Hz	415 Hz

lists the relevant technical parameters.

3.2. Results and Analyses

Several tests have been performed in different environments to verify the adaptability of the proposed method. For each test, we used IEZ, HDE, and IHDE methods, respectively, to estimate a person’s walking trajectories and compare results. The following index was adopted to compare the performance of different methods.

$$P_{err}=\frac{D_{err}}{TTD}\times 100\%$$

(28)

$D_{err}$ indicates the deviation distance from the expected ending point, TTD means the total travelled distance, and $P_{err}$ is the positioning error of estimated trajectory.

Test 1 was performed on the first floor of the IT laboratory building in the Beijing Institute of Technology. The person walked from the starting point and followed the directions indicated by the arrows in Figure 6a. The 202-m long path, divided into 8 segments (identified by numbers), is made up of straight lines and right angles. Segments 1 and 7 are 27 m long, while the length of Segments 3 and 5 is 36 m, and the rest of the segments are all 19 m. Before starting to walk, the person was still for 5 s. The reference path and estimated trajectories are shown in Figure 6.

The results in Figure 6b show that, due to the lack of heading correction, the drift of the IEZ trajectory gradually accumulated in the straight lines, which led to an increasing deviation between the estimated trajectory and reference path, with the positioning error reaching about 9.0%. As each segment of the path coincided with the dominant directions, both the HDE and IHDE methods could make full use of the information to correct the heading angle. The final positioning error of the HDE trajectory was less than 0.6%, and for the IHDE trajectory, the smallest error was only 0.2% during the test.

In order to compare with the real environment, we chose the east playground for Test 2 and Test 3. Different from Test 1, the paths in Test 2 and Test 3 contained both the dominant and non-dominant directions.

In Test 2, the person first walked in straight lines along the dominant directions, then walked along the curved track for a distance, and finally walked in a straight line along a non-dominant direction and returned to the starting point. The actual path was about 90 m long, shown in a black dotted line in Figure 7, through which we illustrate the heading correction based on the proposed method. Figure 7a shows the result of evaluation E during the walk. The proposed method was triggered when E exceeded a certain threshold, corresponding to the red part, which indicated straight lines. Then, as explained in Section 2.4, we could further determine if the path was in a dominant direction and introduce error measurement $\delta \psi$. It is worth noticing that between sampling point 1600 and 2475, as the person walked along a circular path, E close to 0 indicated that the method can identify abnormal situations, and thus avoid overcorrection of heading. For other parts in black, due to the sudden change of heading (corresponding to the “edges” in the above subfigure), corrections were not made within a short time.

In contrast, dominant directions were always taken as a reference in the original HDE method, so the heading was still corrected to approach its nearest dominant direction for the last two segments of path, as can be seen in the above subfigure of Figure 7a, making the trajectory wrongly estimated. The positioning error of the HDE trajectory was about 1% larger than that of the IHDE, according to the test results. Figure 7b shows the estimated trajectories using different methods.

The actual path in Test 3 consisted of two straight lines along dominant directions and two arcs, as shown in Figure 8. The heading angle of the HDE trajectory was continuously wrongly corrected based on the dominant directions as a person stepped onto the curved track, which eventually led to the trajectory being useless for positioning. Instead, no heading correction was made on the curved track by using the proposed method, and the estimated trajectory maintained a small deviation from the actual path with $D_{err}$ only about 2 m. The values in

Table 2. The results of positioning errors in each test.

Method	Positioning Errors
	Test 1	Test 2	Test 3
IEZ	8.7~9.6%	2.3~2.8%	2.6~3.2%
HDE	0.3~0.6%	1.3~1.7%	>20%
IHDE	0.2~0.4%	0.4~0.7%	0.3~0.5%

show the typical range of positioning errors for several tests in different environments like those presented in Figure 6, Figure 7 and Figure 8.

4. Conclusions

To solve the problem that the IEZ frame cannot suppress the heading drift, and the HDE method does not apply to the non-dominant direction’s condition, we have proposed the IHDE method and have tested its performance in different environments. The results show that, if the person walks in the non-straight line, using the proposed method can judge this condition by comprehension evaluation compared to the HDE method, and then the heading angle will not be overcorrected, ensuring the correct estimation of trajectories. The positioning errors of estimated IHDE trajectories in tests indicate the adaptability of the proposed method to different environments.