Adaptive Kalman filtering-based pedestrian navigation algorithm for smartphones

Abstract

Pedestrian navigation with daily smart devices has become a vital issue over the past few years and the accurate heading estimation plays an essential role in it. Compared to the pedestrian dead reckoning (PDR) based solutions, this article constructs a scalable error model based on the inertial navigation system and proposes an adaptive heading estimation algorithm with a novel method of relative static magnetic field detection. To mitigate the impact of magnetic fluctuation, the proposed algorithm applies a two-way Kalman filter process. Firstly, it achieves the historical states with the optimal smoothing algorithm. Secondly, it adjusts the noise parameters adaptively to reestimate current attitudes. Different from the pedestrian dead reckoning-based solution, the error model system in this article contains more state information, which means it is more sensitive and scalable. Moreover, several experiments were conducted, and the experimental results demonstrate that the proposed heading estimation algorithm obtains better performance than previous approaches and our system outperforms the PDR system in terms of flexibility and accuracy.

Introduction

Location-based service has been increasingly popular owing to the information, convenience, and security it provides. Mobile phones and smart devices are widely spread in recent years, and the various sensors embedded could be used to achieve location information. Global Positioning System (GPS) could be applied to outdoor scenarios but susceptible to the signal blocking and refraction caused by buildings. Wi-Fi, ¹ ultrasound, ² RFID and magnetic field ³ -based location technologies require preinstallation of localization beacons, which are not suitable for dangerous or emergent environments. Microelectromechanical systems (MEMS) technology has a more ubiquitous application scenario, which mainly utilizes inertial measurement unit (IMU), consisting of the accelerometer, gyroscope, and magnetometer. Unfortunately, the error would propagate and increase without the constraint and contribute to the worse result as the time goes. External techniques, such as indoor graph matching, ⁴ Wi-Fi localization, visible light positioning, ^5,6 and magnetic positioning, ⁷ effectively enhance the performance of IMU-based methods by fusing position results. ⁸

There are two typical IMU-based pedestrian navigation algorithms. The one is pedestrian dead reckoning (PDR)-based solution and the other one is inertial navigation system (INS). PDR-based solution mainly contains three tasks: step detection, stride length estimation, and heading estimation. Various algorithms based on the physiological characteristics of the pedestrian movement have been proposed to resolve these tasks. Because of the cyclical characteristics of acceleration waveform, peak detection, ⁹ zero-crossing, ¹⁰ and flat zone detection ¹¹ could be applied to detect steps. Moreover, the step length models ^{12 –14} also estimate step length using accelerate features. Heading direction estimation is the critical point of PDR. It is a usual way that integrating data collected by the gyroscope to achieve the attitude. Roll and pitch error could be compensated by accelerates as well as magnetic data compensate yaw error. Attitude heading reference system (AHRS) ¹⁵ is a classic way of fusing them. The second approach is based on INS in which the raw data of triaccelerometer and trigyroscope are obtained at a high rate for tracking position, velocity, and attitude (PVA) of the device employing integration. Although INS would hold more information, which makes the system states more dynamic and detailed, the error of INS increases rapidly over time due to the low quality of the IMU in mobile devices and double integration of the error during the calculation. Zero-velocity update (ZUPT) is an effective solution to rectify the velocity error during walking if the device is strapped on foot. ¹⁶ There would be an observable instant static interval between two steps. The correction of heading error is similar to the PDR-based solution, which mainly relies on the accelerate and magnetic data. Besides, technology like zero-angle rate update ¹⁷ could also be employed. This article will focus on the INS-based solution aided by the PDR results.

Extended Kalman filter (EKF) would be an ideal choice for INS processing input data and handle the observation events. In this article, an error model, which considers position error, velocity error, and attitude error, is constructed to track system error state and feedback to PVA. To avoid the singularity problem and reduce computation cost, device attitude is represented in the form of quaternion ¹⁸ instead of Euler angle. The error states in model propagate in the manner of kinematic equations. Meanwhile, several observation equations would work if judgment conditions get satisfied. Step length and step speed calculated by the PDR solutions are applied to rectify the state error as the gait model has better performance than the integration value of raw sensor data. Magnetic data that provide absolute heading direction ¹⁹ play a vital role in heading observation. Unfortunately, the existence of ferromagnetic material interferes with the earth’s magnetic field, which means the angle is unstable and unreliable. ²⁰ To resolve this problem, this article proposed a new way. The system holds a sliding window, which records the angle between the real-time raw magnetic vector and the magnetic vector rotated from the first vector in the window by the gyroscope. The variance of angles in the window is used to judge the availability of local magnetic data. If available, attitude quaternion at the beginning of the window would be deduced by the optimal smoothing algorithm. After that, Sage–Husa adaptive filtering algorithm would estimate the variance parameters of each measurement and compensate the current attitude. The main contributions of this article is summarized as follows:

To obtain more accurate and robust pedestrian’s location, we construct an error model system based on scalable EKF. Position, velocity, and attitude errors are predicted from the kinematic equations and updated by the static state, gravity, gait model, and magnetic vectors. Errors would be feedbacked to rectify system states. It is convenient to extend the system by other modules at corresponding scenarios, such as GPS, Wi-Fi fingerprints, and so on.

We propose a magnetic-based adaptive heading correction algorithm, including a relative static magnetic field judgment and a two-way Kalman Filter (KF) calibration procedure. The similarity between the magnetic vectors achieved by magnetometer and gyroscope is used to judge the availability of local magnetic data. All magnetic data in the window would be involved during the filtering. Because the interference may still exist, adaptive noise estimation is applied to deal with the fluctuation. Thus, we deduced the attitude quaternion backward using optimal smoothing algorithm and calculate current state forward using Sage–Husa algorithm.

Extensive and elaborate experiments are conducted to evaluate the performance of the system. The scenarios are mainly the outdoor playground, the street road, and indoor office area and shopping mall. We also compare the error heading angle with other different heading estimation algorithms, including the principal component analysis (PCA) based method and AHRS, on the street road with the partially interfered magnetic field. The performance of the position tracking is evaluated by the error. The experimental results demonstrate that the proposed algorithm has high precision and better stability. Besides, the system could track pedestrian states in the indoor localization experiment with better sensitivity and robustness.

The rest of the article is organized as follows. The second section describes the related works. The third section presents the proposed algorithm, including architecture, several experiments, explanations, and so on. The fourth section evaluates the availability of the proposed algorithm under different environments. Some common algorithms and the PDR system have been compared to prove the performance. We conclude this article in the fifth section.

Related works

Various schemes have already been attempted to optimize the results of the device localization. There would be an instant during the stance phase of a gait cycle between adjacent two steps that accelerations and velocities are zero. That is to say, any nonzero accelerations and velocities exist during this phase are considered accumulated noise and thus should be reset to zero. ZUPT is the ideal solution to handle this situation ^21,22 and restrict the state error timely by each step. However, ZUPT requires the relevant sensors to be mounted on the foot, which are not suitable and infeasible for smartphone users.

Considering the poor quality of sensors built-in smart devices, several algorithms ^23,24 attempt to reduce the impact of the noise. Low pass filter, bandpass filter, or Butterworth filter are employed in various scenarios handling raw data.

The existing indoor localization systems, especially based on IMU sensor data, still face challenges, such as accumulated errors from sensors and external magnetic field effects. Some works assume that the related information is already known. The research ^25,26 utilize the indoor map or floor plan to derive the constraints and paths from so that estimated trajectory and heading direction could be rectified. Virtual landmarks are another kind of information that may be achieved before the localization. ^27,28 Virtual landmarks are identified by observing distinct signatures or patterns in the data extracted from magnetic data and Wi-Fi signals collected at specific locations. Once detecting the repetitive landmark during walking, states would converge to its location. ²⁹ It is a kind of impractical for these aforementioned approaches to improve the accuracy of localization just with the assistance of other external conditions sometimes.

To track pedestrian’s location, the most practical solution is the PDR algorithm. Many studies have paid efforts to improve its performance in step detection, stride length estimation, and heading estimation modules. Khedr and Nasser ³⁰ proposed a novel step counter, which does not require a classifier, adaptively tunes the filters, and thresholds used without the need for presets while accomplishing the task in a real-time operation manner. Wang et al. ¹³ combine smartphone mode recognition and stride length estimation to provide an accurate walking distance estimation. The research models stride length using the regression model of machine learning by constructing higher-order features. Last but not least, heading estimation is always the hardest part to optimize. Magnetometers are essential for estimating absolute orientation. However, they often lack calibration, so that the outputs are easily contaminated by hard iron, soft iron, and scale factor errors. Wu et al. ²² construct an ellipsoid model to correct the magnetic field measurements. Heading calculation would be conducted after that. In the literature, ³¹ a detector is proposed that identifies the magnetic field measurements that are used for estimating heading with adequate accuracy. The expected errors in the heading estimates are also output based on the test statistics, which allow the proposed detector to be utilized for sensor fusion and estimation of errors associated with gyroscopes. Abadi et al. ³² use data fusion to average the heading estimates from multiple people walking in the same direction. The rationale of this component is based on the observation that magnetic perturbations are often highly localized in space and if multiple people are walking in the same direction, then only some of their magnetometers are likely to be perturbed.

Without a doubt, Bayesian filters like KF and complementary filter (CF) are also employed in many algorithms. ^33,34 CF is relatively easy to implement and has a lower computational cost, whereas the heading accuracy produced by CF is slightly worse than that by KF and even worse in highly dynamic environments. ²² Therefore, KF has a wider range of applications, especially in terms of direction estimation. Valenti et al. ³⁵ utilize a linear KF for magnetic angular rate and gravity sensors that process angular rate, acceleration, and magnetic field data to obtain an estimation of the orientation in quaternion representation. EKF solution ³⁶ and unscented KF solution ³⁷ were applied for the better estimate of the orientation. Consequently, the KF is practical and easy to extend.

Many machine learning technologies have also been employed in pedestrian localization algorithms. Zhang et al. ³⁸ predict the position of pedestrian regardless of holding postures by taking the movement habits of pedestrian into the process of extreme learning. Besides, KF fuses the predictions of neural network by taking it as observation values because neural networks map input–output relationships without a priori knowledge. ³⁹ Many subproblems of pedestrian localization could find a new way to explore like step counter, holding style classifier, and so on.

Compared with the methods mentioned above, our system is easily applied to smartphones without restriction. It is practical in different scenarios without relying on a particular environment. Moreover, the external information is easily employed in the system due to its high scalability. As for machine learning solutions, our system has advantages of no need for training and low calculation overhead.

Methods

A system state model is built to track the status of the device, which describes its position, velocity, and attitude. To rectify the drift, an EKF is applied in several common scenarios. At last, we utilize an adaptive filter while at the relative static magnetic area. This section will explore the contributions to the project in detail.

System state model

The overview of the system is shown in Figure 1. Obviously, the position is our final target, which means all states related to it should be maintained. According to the kinematic equation, velocity and attitude are included in the state. Compared to the Euler angle and rotation matrix, the quaternion is a better choice for us to represent attitude, which could bring us cheaper computation overhead and avoid the problem of gimbal lock. The position and the velocity could be simply saved in two triaxial vectors.

Figure 1.

Structure of the error model system.

The prediction occurs in response to some external input sensor data received. As for our system, inputs consist of three-dimensional acceleration, angular velocity, and magnetic vector in the device coordinate system. Firstly, the attitude quaternion will be updated with equation (1)

$\left\{\begin{array}{l}{Q}_{b\left(m\right)}^i=Q_{b\left(m-1\right)}^i\otimes Q_{b\left(m-1\right)}^{b\left(m\right)}\\ Q_{b\left(m-1\right)}^{b\left(m\right)}=\left[\begin{array}{c}\text{cos}\left(\text{Δ}{\theta }_m/2\right)\\ \frac{\text{Δ}{\theta }_m}{\text{Δ}{\theta }_m}\text{sin}\left(\text{Δ}{\theta }_m/2\right)\end{array}\right]\\ \Delta {\theta }_m=\left|\text{Δ}{\theta }_m\right|\\ \text{Δ}{\theta }_m={{\displaystyle \int }}_{t_{m-1}}^{t_m}{\omega }_{ib}^b\text{d}t\end{array}\right.$

1

where $Q_{b\left(m\right)}^i$ represents quaternion transforming from the device coordinate system to the navigation coordinate system at t_m . $Q_{b\left(m-1\right)}^{b\left(m\right)}$ represents change quaternion between t_m and $t_{m-1}$ , which is calculated by the gyroscope data. Quaternion multiplication ⊗ represents attitude quaternion rotated by another quaternion calculated by angular velocity. In a word, the differential equation is applied to realize the prediction of quaternion update with real-time angular velocity.

Then, the position and velocity will be updated using the kinematic equation as follows

$\left\{\begin{array}{l}{\text{p}}_m^i={\text{p}}_{m-1}^i+{\text{v}}_m^i\Delta t\\ {\text{v}}_m^i={\text{v}}_{m-1}^i+\left[{\text{f}}_m^i-{\text{g}}^i\right]\Delta t\\ \left[\begin{array}{cc}0& {\text{f}}_m^i\end{array}\right]=Q_{b\left(m\right)}^i\left[\begin{array}{cc}0& {\text{f}}_m^b\end{array}\right](Q_{b\left(m\right)}^i{}^{-1})\end{array}\right.$

2

where ${\text{f}}_m^i$ , ${\text{v}}_m^i$ , and ${\text{p}}_m^i$ are the acceleration, velocity, and position of device at t_m in the navigation coordinate system. $Q_{b\left(m\right)}^i$ is the attitude quaternion rotating from the device coordinate system to the navigation coordinate system at t_m . ${\text{g}}^i$ is the Earth gravity vector.

The procedure of state transition is therefore in order. That is to say, the attitude should be updated first and used to calculate acceleration in the navigation coordinate system to update velocity, and finally, the position is updated. Algorithm 1 describes the once iteration of the whole system.

Algorithm 1.

The framework of error model system.

Error model

It is widely known that the low-grade MEMS IMU, which usually introduces large acceleration and gyroscope biases, will cause rapid accumulation of the state error. A common but effective measurement is building an EKF error model to compensate for the drift because the system integrates the raw data of triaccelerometer and trigyroscope.

The system estimates the error between the current state and real state and represents it as equation (3). Besides, the bias of sensors, including gyroscope and accelerator, should be measured and represented as equation (4). With error at last moment and bias achieved, the system could update the error vector at this moment by the kinematic equation, as in equation (5)

$\delta s={\left[\begin{array}{ccc}\delta {\text{p}}^i& \delta {\text{v}}^i& \delta {\text{a}}^i\end{array}\right]}^T$

3

${\text{b}}_s=\left[\begin{array}{cc}{\text{b}}_g& {\text{b}}_f\end{array}\right]$

4

$\begin{array}{l}\delta s_m=\left[\begin{array}{ccc}{O}_3& {\text{I}}_3\Delta t& {{\rm O}}_{3,3}\\ {{\rm O}}_{3,3}& O_3& ({\text{f}}_m^i\times )\Delta t\\ {{\rm O}}_{3,3}& {{\rm O}}_{3,3}& O_3\end{array}\right]\delta s_{m-1}\\ +\left[\begin{array}{cc}{{\rm O}}_{3,3}& {{\rm O}}_{3,3}\\ {{\rm O}}_{3,3}& {\text{C}}_{b\left(m\right)}^i\\ -{\text{C}}_{b\left(m\right)}^i& {{\rm O}}_{3,3}\end{array}\right]{\text{b}}_s\Delta t\end{array}$

5

where $\delta {\text{p}}^i$ $\delta {\text{v}}^i$ $\delta {\text{a}}^i$ are position error vector, velocity error vector, and attitude error rotation vector separately. ${\text{f}}_m^i\times$ is the skew-symmetric matrix form of acceleration vector at t_m in the navigation system.

A KF works in a two-step process. In the prediction step, the KF produces estimates of the current state variables along with their uncertainties. The transition of attributes in the system mainly relies on the kinematic equations. The design matrix of this procedure is given in equation (5). Once the outcome of the next measurement is observed, these estimates are updated using a weighted average, with more weight being given to estimate with higher certainty. Next, detailed observation scenarios and update strategies will be defined.

ZUPT is an inexpensive and effective method that could be employed if the device at the relatively static state. The velocity is assumed to be zero when ZUPT detected. The device is held in hand so that the stance phase would not be detected during the walking. It is still practical to calibrate the velocity and convergence the error although there are not a lot of chances to keep the device static while walking. There are serval existing ways to judge the relatively static state. Here, acceleration and angular velocity among the sliding window are considered as the judgment conditions. The maximum and variance are calculated and compared with the threshold. The observation equation is given as follows

$\delta \text{v}={\text{v}}^i-O_{3,1}=\delta {\text{v}}^i+n_v$

6

$H_{\text{zupt}}=\left[\begin{array}{ccc}{O}_{3\times 3}& I_{3\times 3}& O_{3\times 3}\end{array}\right]$

7

where ${\text{v}}^i$ is the velocity state, ${\text{δv}}^i$ is the error velocity state, which should be rectified, and n_v is the measurement noise. $H_{\text{zupt}}$ is the observation matrix.

Compared to ZUPT, it is much easier to detect low dynamic state, which means external acceleration does not exist. A typical scenario is at the time when walking at a constant speed or ZUPT detected. The difference between the normal value of Earth’s gravity vector and accelerator observation vector could be practical to judge this state. In this situation, the acceleration collected by the device could be reckoned as gravity after rotating by the attitude quaternion. The bias between the two vectors is the observation value used to calibrate attitude error. The observation equation is given as follows

$\begin{array}{l}\delta \text{f}=\left({\text{I}}_3-\delta \text{a}\times \right){\text{C}}_b^i\left({\text{f}}^b+n_f\right)-{\text{f}}^i\\ =\left[\left({\text{C}}_b^i{\text{f}}^b\right)\times \right]\delta \text{a}+{\text{C}}_b^i{n}_f\end{array}$

8

$H_{\text{gravity}}=\left[\begin{array}{ccc}{O}_{3\times 3}& O_{3\times 3}& \left[\left({\text{C}}_b^i{\text{f}}^b\right)\times \right]\end{array}\right]$

9

where $\delta \text{a}$ is the rotation vector representing the error attitude and $\delta \text{a}\times$ is the skew-symmetric matrix form of it. ${\text{f}}^b$ represents the acceleration vector, on the other hand, ${\text{f}}^i$ the local gravity vector.

PDR algorithms estimate the user’s position by the effective pedometer algorithm, heading estimation, and step length estimation algorithm, whose external inputs are only the raw sensor data. Under these conditions, we utilize approximate relative position variation to rectify the position error. Here, we assume that pedestrians walk along the direction they are facing, meanwhile, they hold the device forward. Therefore, the vector representing position variation is constructed as $\left[\begin{array}{ccc}0& {\text{L}}_{\text{step}}& 0\end{array}\right]$ at the device coordinate system. The position calculated with PDR-based solution is readily achieved by the current attitude. Step detection algorithm in this article is realized by maintaining a model to track the walking state of pedestrian. In addition, the system employed a classic step length algorithm ⁴⁰ in equation (10) for the sake of simplicity

${\text{L}}_{\text{step}}\hspace{0.17em}\text{=}\hspace{0.17em}\text{K}·\sqrt[4]{{\text{f}}_{z\left(\text{max}\right)}^i-{\text{f}}_{z\left(\text{min}\right)}^i}$

10

where ${\text{L}}_{\text{step}}$ is the step length, ${\text{f}}_{z\left(\text{max}\right)}^i$ and ${\text{f}}_{z\left(\text{min}\right)}^i$ are the maximum value and the minimum value of z-axis acceleration in the navigation coordinate system. The parameter K in equation (10) requires training for each pedestrian in advance. The update with the observed position is represented as follows

$\delta \text{p}={\text{p}}_k-\left({\text{p}}_{k-1}+C_{b(k-1)}^i{\text{L}}_{step}\right)=\delta {\text{p}}^i+n_p$

11

where ${\text{p}}_k$ represents the position of k’th step detected and $\delta {\text{p}}^i$ is the error position vector. $C_{b\left(k-1\right)}^i$ is the rotation matrix at the time k−1, namely, the time the last step occurred. The observation matrix $H_{\text{step\_length}}$ is shown in equation (12)

$H_{\text{step\_length}}=\left[\begin{array}{ccc}{I}_{3\times 3}& O_{3\times 3}& O_{3\times 3}\end{array}\right]$

12

Once step length achieved, dividing by the period between two steps could get step speed along the walking direction, which also is the speed at the device coordinate system. The system could rectify the error model with all the above conditions prepared. More specifically, the observed velocity error vector equals the step velocity minus the device’s velocity state, the same as the position error vector. The observation equations of velocity state are given as follows

$\begin{array}{l}\delta \text{v}={\text{v}}^l-{\text{v}}^i+n_v\\ ={\text{C}}_i^b\left(\text{I}+\delta \text{a}\times \right)\left({\text{v}}^i+\delta {\text{v}}^i\right)-{\text{v}}^i+n_v\\ ={\text{C}}_i^b\delta {\text{v}}^i-{\text{C}}_i^b\left({\text{v}}^i\times \right)\delta \text{a}+n_v\end{array}$

13

$H_{\text{step\_velocity}}=\left[\begin{array}{ccc}{O}_{3\times 3}& {\text{C}}_i^b& -{\text{C}}_i^b\left({\text{v}}^i\times \right)\end{array}\right]$

14

where ${\text{v}}^l$ is the step velocity calculated by step length, ${\text{v}}^i$ is the velocity vector, and $\delta {\text{v}}^i$ is the error velocity error vector.

Methods mentioned above are incapable of correcting the drift of the heading angle. ZUPT and step length or step speed are mainly used to restrict the drift of velocity and position. The update with gravity could be used to measure the attitude error, whereas the heading angle would not have an impact on the vertical component. When acceleration rotated to gravity, it has no matter with the heading that it points to. Nevertheless, calibration with the magnetic field vector is a practical solution to correct the error attitude under this situation.

Adaptive magnetic vector update

The magnetometer is the sensor utilized for measuring the Earth’s magnetic field. By resolving these three-axis measurements in horizontal and vertical fields, one estimates the absolute heading, which is the pointing direction of the sensor block with respect to the magnetic north. The raw data collected by the magnetometer are reliable and precise in the open outdoor environment, where free from magnetic perturbation. We conducted several experiments in the playground to prove magnetometer on mobile devices has significant availability in this situation. Figure 2 shows the experimental result. The walking route is irregular, and the direction is random. Only the holding style got ensured. The gyroscope and magnetometer keep recording data during the walking to plot the figure.

Figure 2.

The raw magnetic vector and the vector calculated by the angular velocity while walking in the playground randomly.

As we can see from the figure, two kinds of magnetic vectors are plotted by the time. The calculated one is the vector rotated from the first epoch magnetic vector by the attitude quaternion. The attitude quaternion is calculated by recursively multiplying the differential quaternion rotated using angular velocity. The blue one represents the raw magnetometer vector collected by the magnetometer. The fluctuations in them are quite similar. Even small jitter of the variation looks similar. Accordingly, we could conduct that the variation of the angle between calculated magnetic data and raw sensor data during a window could be utilized to judge whether Earth’s magnetic field gets interfered in the area that pedestrian passes through. To verify this conclusion, another experiment was performed in the playground. The difference is an iron box placed in the middle of the road trying to simulate the disturbance at the indoor scenario. The route is a straight line and plotted in Figure 6. The details would be presented in the fourth section. The experimental results are shown in Figure 3.

Figure 3.

The raw magnetic vector and the vector calculated by the angular velocity while walking in the playground with an iron box placed on the road.

Even though it is desirable to utilize the magnetic field sensors for heading estimation outdoor, the presence of manmade infrastructure in all kinds of urban environments makes it impossible to sense only the Earth’s magnetic field at all times. These manmade magnetic anomalies caused by ferromagnetic materials, electronic devices, mechanical and electrical infrastructures among others are the main culprits contaminating the magnetic field information. Therefore, methods for removing the negative impacts caused by these disturbances are needed.

In this article, we proposed a new algorithm to take advantage of the indoor magnetic field adaptively, minimizing the negative effects of interference. A novel judgment method is applied to evaluate the availability of data in the window. Before inputting data into the KF, the noise parameters should be given to lessen the impact of magnetic fluctuation. Adaptive parameter estimation is realized by applying a two-way KF on the historical data in the window. The core idea here is to filter the possible noise with a series of observation values. The details will be introduced in the following parts.

First of all, the judgment of the availability of the local magnetic field is conducted before applying the algorithm. Compared to the magnetometer, the gyroscope in the device is more robust and not influenced by the environment. Moreover, they both could be used to estimate the heading, so the similarity of their outcome is suitable for judging the stability of the local magnetic field. Instead of making use of the heading direction directly, a novel method of detecting the stability of magnetic data is employed in this article. The sliding window is kept to record historical sensor data. A sequence of angles between two vectors would be calculated by inverse trigonometric function. One vector is constructed by raw magnetic data, and the other one is the magnetic vector rotated from the first magnetic vector in the window by the gyroscope. It would be available to judge the similarity between the variation of gyroscope and magnetometer by calculating the variance of angles consequently. If the variance is small enough, in other words, the collected magnetic data could be considered relatively reliable, and the system model would adaptively calibrate the heading angle. The bias and error still exist despite that the magnetic data in the window could be considered stable. So, adjusting the noise parameter in the window dynamically to update system states at this time would be more practical.

Thus, the optimal smoothing algorithm is used to calculate the optimal smoothing state at the beginning of the window. More specifically, the fixed interval smoothing algorithm implemented based on forwarding Kalman filtering is used to apply backward filtering. The state is estimated by taking full advantage of all measured values in the measurement interval, which has higher estimation accuracy than unidirectional filtering. Hence, the system would traverse the data among the window in reverse order of them got appended. Equation (15) describes the procedure of backward filtering, where ${\widehat{X}}_{b,k}$ and $P_{b,k}$ are prediction values and variance of state at time k

$\left\{\begin{array}{l}{\widehat{X}}_{b,k/k+1}={\varphi }_{k/k+1}^{*}{\widehat{X}}_{b,k+1}\\ P_{b,k/k+1}={\varphi }_{k/k+1}^{*}{P}_{b,k+1}{\left({\varphi }_{k/k+1}^{*}\right)}^{\text{T}}+{\Gamma }_k^{*}{Q}_k{\left({\Gamma }_k^{*}\right)}^{\text{T}}\\ K_{b,k}=P_{b,k/k+1}{H}_k^T{\left(H_k{P}_{b,k/k+1}{H}_k{}^{\text{T}}+R_k\right)}^{-1}\\ {\widehat{X}}_{b,k}={\widehat{X}}_{b,k/k+1}+K_{b,k}\left(Z_k-H_k{\widehat{X}}_{b,k/k+1}\right)\\ P_{b,k}=\left(\text{I}-K_{b,k}{H}_k\right)P_{b,k/k+1}\end{array}\right.$

15

where ${\varphi }_{k/k+1}^{*}$ represents the transition matrix and H_k represents the observation matrix. ${\varphi }_{k/k+1}^{*}$ equals to ${\varphi }_{k+1/k}^T$ . The procedure is rather similar to the forward KF except for the direction of iteration. Equation (16) fuses the results of forward and backward filtering to achieve the optimal smoothing value and its variation estimation

$\left\{\begin{array}{l}{\widehat{X}}_{s,k}={\left(P_{f,k}+P_{b,k/k+1}\right)}^{-1}\left(P_{b,k/k+1}{\widehat{X}}_{f,k}+P_{f,k}{\widehat{X}}_{b,k/k+1}\right)\\ P_{s,k}={\left(P_{f,k}^{-1}+P_{b,k/k+1}^{-1}\right)}^{-1}\end{array}\right.$

16

In the above equation, the subscripts f and b mean the forward and backward direction, respectively. The lower right subscript s indicates smoothing results. ${\widehat{X}}_{s,k}$ is the optimal smoothing value of state at time k.

Eventually, Sage–Husa adaptive filtering would be applied to rectify the heading error vector after the above conditions prepared. It estimates the noise parameters of the system in real time through the measurement output while performing state estimation. The measurement in the indoor magnetic field is the typical application suitable for this algorithm owing to its instability and unreliability. Because it is almost impossible to find the proper noise parameters for each of the observed magnetic vector collected.

Some information about the system model is implied in the measurement output, so the noise parameters should be reestimated according to the measurement output when they are not accurate enough. The prediction error is given by

${\stackrel{\sim }{Z}}_{k/k-1}=Z_k-{\widehat{Z}}_{k/k-1}=H_k{\stackrel{\sim }{X}}_{k/k-1}+{\text{v}}_k$

17

The ${\stackrel{\sim }{Z}}_{k/k-1}$ represents the error between the prediction value and observation value. ${\text{v}}_k$ is the observation noise. We calculate the variance on both sides of equation (17) and shift the item, equation (18) is obtained, where R_k is the variance of the error between the prediction value and observation value

$R_k=\text{E}\left[{\stackrel{\sim }{Z}}_{k/k-1}{\stackrel{\sim }{Z}}_{k/k-1}^{\text{T}}\right]-H_k{P}_{k/k-1}{H}_k{}^{\text{T}}$

18

The exponential elimination memory weighted average method is beneficial to reduce the impact of old measurement noise. ${\widehat{R}}_k$ is constructed as follows

${\widehat{R}}_k=\left(1-{\beta }_k\right){\widehat{R}}_{k-1}+{\beta }_k\left({\stackrel{\sim }{Z}}_{k/k-1}{\stackrel{\sim }{Z}}_{k/k-1}^{\text{T}}-H_k{P}_{k/k-1}{H}_k{}^{\text{T}}\right)$

19

where parameter ${\beta }_k$ is described in equation (20), b is the fading factor, and usually assigned as 0.9–0.999

${\beta }_k=\frac{{\beta }_{k-1}}{{\beta }_{k-1}+b}$

20

System parameters have been set up so that the error model could update its attitude error vector with the collected magnetic vector. Equation (21) explains the way it works and equation (22) is the observation matrix for the magnetic vector update

$\begin{array}{l}\delta \text{m}=\left({\text{I}}_3-\delta \text{a}\times \right){\text{C}}_b^i\left({\text{m}}^b+n_m\right)-{\text{m}}^i\\ =\left[\left({\text{C}}_b^i{\text{m}}^b\right)\times \right]\delta \text{a}+{\text{C}}_b^i{n}_m\end{array}$

21

$H_{\text{mag}}=\left[\begin{array}{ccc}{O}_{3\times 3}& O_{3\times 3}& \left[\left({\text{C}}_b^i{\text{m}}^b\right)\times \right]\end{array}\right]$

22

where $\delta \text{a}$ is the rotation vector representing the error attitude and $\delta \text{a}\times$ is the skew-symmetric matrix form of it. ${\text{m}}^b$ and ${\text{m}}^i$ represent the magnetic value in the device and navigation coordinate system, respectively.

So far, the complete system has been introduced. It makes use of sensor data to update position, velocity, and attitude states, and constructs an error model using Kalman filtering. Calibration errors are measured by relative static state, gravity, and step length, and fed back into the system states. In addition, the system employs interval smoothing and adaptive filtering to cope with indoor magnetic field measurements that the variance of angles in the window ensures the availability. Thus, the system guarantees the accuracy of each state.

Experimental evaluation

In this section, the experiments had been conducted to prove the availability of the proposed algorithm and the performance of the system. Considering the instability and unpredictability of indoor magnetic interference, the experiment of the adaptive magnetic vector update was conducted in the outdoor environment. The other experiments were performed indoor and combined with the map to display the results intuitively. The devices used in the experiments were Huawei Mate 20 and Samsung Galaxy S9, embedding with the accelerator, gyroscope, and magnetometer. The devices collected data at the rate of 100 Hz. Other details about experimental scenarios were described in the following parts.

Heading availability for adaptive magnetic vector update method

Three scenarios were picked up to evaluate the algorithm, including the middle of the playground, the road with a metal box placed in the half, and the road with two buildings aligned. First of all, accurate low magnetic interference detection matters a lot before applying the algorithm. As mentioned before, the walking route where several turns occurred is irregular. Figure 4 shows the variance of angles in the window and the time that attitude rectification occurred during the walking. The green dashed in the figure represents the rectified time.

Figure 4.

The variance of angles between two vectors while walking in the playground randomly.

Observing Figure 4, the rectified points represent the magnetic data collected represent relatively stable from several seconds to before to this time. The window length means the duration of detecting time. To reduce computational overhead, intervals are set to avoid successive algorithm call in our experiments. That is the reason why there are blanks between green lines, and its size could be adjusted. Observing the figure, there still exists many times the variance not as small as we thought. The experimental data were analyzed, and it was found that the time when the variance became larger coincided with the time when the device rotation ended. The guess is due to the inconsistent data convergence rate between the magnetometer and the gyroscope.

After achieving a sequence of available sensor data, the current attitude would be rotated by a rotation vector, which maintains the error provided by the algorithm. Figure 5 shows the triaxial Euler angles of the device during the walking. The one is provided by Android’s API, ⁴¹ which calculated using raw magnetic data and accelerator data. The other one is transformed from the attitude quaternion that kept as system states.

Figure 5.

The variation of the Euler angles while walking in the playground randomly.

The angle calculated by the proposed algorithm changes more smoothly due to that the gyroscope is mainly used to perform the rotation while magnetic data are unstable. The initial attitude is achieved after detecting the relatively static local magnetic field and applying the two-way KF. Before that, the system would take the attitude calculated by raw sensor data as its attitude state. Observing the figure, the Euler angle variation is similar to the angle rotated by the gyroscope although the correction occurred. The reason for that is the experiments were performed in the playground in which little magnetic disturbance exists, that is to say, the attitude achieved by the algorithm is accessible for correction in this situation.

To control the disturbance of the magnetic field, an iron box was placed in the middle of the playground at the Beijing University of Post and Telecommunication. As a result, we could know the different areas whether we could conduct our algorithm or not and tell them apart easily. Figure 6 shows the experimental scenario.

Figure 6.

The playground where experiment conducted.

The green dashed represents the route we walked, and the white square is the iron box. The magnetic vectors accessed by the magnetometer and calculated by the gyroscope are shown in Figure 3. It is obvious that the box did disturb the raw magnetic vector and we could judge where it begins and where it ends easily. The variance of angles between two vectors in the window is shown in Figure 7.

Figure 7.

The variance of angles between two vectors while walking in the playground with an iron box placed on the road.

The area interfered by the box has a much greater variance value than other areas, which means the time we conduct the algorithm is reasonable. Figure 8 displays the variation of the Euler angle. It could be concluded from the figure that the adaptive magnetic vector update method is reliable when interference exists.

Figure 8.

The variation of the Euler angles while walking in the playground with an iron box placed on the road.

Performance comparison of heading estimation

In this section, we compare our algorithm with several existing algorithms to evaluate its heading estimation performance. We choose a path, which passes two buildings by its side as the experimental scenario. There is an open space between two buildings. So, some parts of the path get the impact of magnetic interference slightly and some parts seriously. The length of the road is about 120 m, and it costs about 100 s to finish walking. The direction of the road has already known, and the error could be achieved by that. This road is chosen as our experimental scenario because of the magnetic field on it in accordance with some pattern. The parts close to the buildings would be interfered by the ferromagnetic materials, whereas the other parts would not.

Three methods are chosen to conduct the comparison experiment in this article. One is the raw method, one is based on PCA, and another one is based on AHRS. ¹⁵ The raw method calculates the azimuth using the accelerator and magnetometer data. The gravity vector is estimated by applying a low-pass filter on the raw acceleration, which simply considers the noise is the high-frequency part. The vertical component of the magnetic vector could be obtained by operating the inner product between the unit gravity vector and raw magnetic vector, after that, multiplying the unit vertical vector. The horizontal component is obtained by subtracting the vertical one. The angle between the horizontal global magnetic vector and the calculated vector is our result, namely the heading angle.

The PCA method is based on the fact that acceleration values fluctuate the most along to the direction of the movement. It exploits the acceleration signal patterns in the horizontal plane to conduct heading estimation. The 180° ambiguity problem usually exists while applying PCA methods because it could not identify similar components at the forward and backward direction. Many studies have proposed various ways to solve this problem. Here, we think the direction more closer to the device’s y-axis orientation is the heading direction and make sure about it while experimenting.

The AHRS is a classical method for heading estimation and is widely used in many scenarios. The magnetometer and the gyroscope are mainly applied to achieve the results. The core of the AHRS algorithm is the way to fuse the results of attitudes. Different from the KF, AHRS is based on a Newton optimization using an analytic formulation of the gradient that is derived from a quaternion representation of motion. It aims to ameliorate the computational load and parameter tuning burdens associated with conventional Kalman-based approaches. There are two kinds of implementation, one relies on the Earth’s magnetic field and the other one does not. Here, to prove the performance of heading estimation using magnetic vectors, experiments chose the first one. The β in the experiment is set to 0.1.

The parameters of noise play a vital role when referring to the KF. Noise analysis is typically a matter of special investigations. That is to say, covariance matrices Q and R requires adjusting to proper values. For simplicity, they are just diagonal matrix in our experiments. R corresponds to the measurement noise covariance matrices. It could be adjusted adaptively in our algorithm. Therefore, we set 0.12 when applying the magnetic vectors update and 0.01 for ZUPT and step update. Gravity update takes 0.8 as the value. Q is a covariance matrix associated with the noise in states. It is mainly deduced by the results of the experiments. Its values of position, velocity, and attitude error correspond to 0.1, 1.2, and 0.01, respectively.

Figure 9 describes the angle variation of four methods during walking. The raw method gets influenced greatly when the interference exists. The PCA method performs better but not stable in comparison with our proposed algorithm. AHRS-based method could produce acceptable heading results. The serious magnetic disturbance in the environment still has an impact on its performance. Our proposed algorithm is the preferable solution here, whose result is most close to the real heading direction.

Figure 9.

Comparisons of the performances on heading estimation of raw, PCA, and proposed solutions. PCA: principal component analysis.

Table 1 summarizes the obtained error metrics for our proposed algorithm and each algorithm mentioned below, including the mean, median, standard deviation, and maximum value. Once again, the results also indicate that the proposed algorithm has an improvement on other methods when the magnetic field in the environment is complicated.

Table 1.

Comparison of heading error with four different methods.

Methods	Absolute error (°)
	Mean	Median	Standard deviation	Maximum
Raw	11.18	9.12	8.94	52.84
PCA	9.45	7.65	7.57	29.93
AHRS	5.30	3.73	4.63	20.98
Proposed	2.71	1.50	2.17	5.51

PCA: principal component analysis; AHRS: attitude heading reference system.

Figure 10 displays the cumulative heading angle error distribution for three considered algorithms. The error of our proposed algorithm is smaller and more stable.

Figure 10.

Cumulative heading angle error distribution for four considered algorithms.

Comparison of position track

We evaluate the performance of the whole error model system and compare it with the traditional PDR solution in this part. The experimental scenarios are the seventh floor of the Institute of Computing Technology (ICT), Chinese Academy of Sciences, and the first floor of Maples International Shopping Center in Beijing. Figure 11 shows the map of the seventh floor of the ICT. The black lines are areas that could not pass, such as desks and walls. The scale of the map is known that 12 pixels represent about a meter. The square corridors are chosen as the reference walking route represented by the black line in the figure.

Figure 11.

Trajectories of reference, PDR, and proposed solutions on the seventh floor of ICT. PDR: pedestrian dead reckoning; ICT: Institute of Computing Technology.

Figure 12 shows the map of the first floor of Maples International Shopping Center in Beijing. The scale of this map is known that 9.6 pixels represent about a meter. Similarly, the experiments record the data by walking through the corridors and holding the devices in hands.

Figure 12.

Trajectories of PDR, and proposed solutions on the first floor of mall. PDR: pedestrian dead reckoning.

The trajectories of our proposed algorithm and the traditional PDR solution are plotted with different line styles on the map. Our error model system retains more details because of the INS mechanism. Observing the picture, we could figure out that the fluctuations caused by the swaying of the pedestrian are more clear. The line plotted is not as straight as the PDR solution. Furthermore, the trajectory of our system nearby the turn is more smooth and more close to the pattern pedestrian walks. The radian of it is greater than the trajectory of the PDR solution. We could achieve the conclusion that our error system is more sensitive and contains more details.

Figure 13 displays the cumulative position error distribution for two considered algorithms. The transition of attributes in the PDR solution is triggered by detecting a new step, whereas the transformation in our system is triggered by the IMU input. It could be considered that our system has better performance in localization than the PDR way owing to two facts. The one is the better sensitivity. The interval between the two steps is about 0.5–0.6 s, and the system states would not transit during this moment. The other one is better robustness. No matter how good the pedometer algorithm is, there will still be errors, which could introduce evident bias. In the comparison of the PDR solution, the position calculated by the velocity would get less affected.

Figure 13.

Cumulative position error distribution for two considered algorithms.

The heading estimation performance of our system slightly outperforms than the PDR solution, but the performance of position prediction is similar due to the same gait model. In another aspect, the Kalman-based system generates more computational overhead to track the states at a high frequency. The proposed heading estimation algorithm also applies historical data to rectify the current system states. With the rapid development of technology, smart devices would carry processor and IMU with better performance, which means the extra calculation could be ignored and our system could take advantage of more precise inertial sensor data better. It could be concluded that our error model system would be more suitable for current and future devices.

Conclusion

This article proposes a novel heading estimation algorithm utilizing the magnetic vectors. An EKF-based error model system is constructed to realize the algorithm and track the pedestrian’s location. The variance of the angles between collected magnetic vectors and calculated vectors among the sliding window is applied to judge whether the algorithm should be conducted at this time. The detection ensures the raw data input available, considering it is vulnerable to the ferromagnetic materials in the environment. The heading estimation algorithm consists of two steps. Firstly, the system states at the historical moments would be deduced backward using optimal smoothing algorithm. Secondly, Sage–Husa algorithm would be applied to reestimate the states by adaptively adjust the noise covariance of collected magnetic vectors. Furthermore, the error model system built in this article has the advantage of high scalability. Many existing mature methods are employed into the system like ZUPT and gravity vector update. The velocity is updated by the step speed achieved from the gait model.

The experimental results show the availability and the performance of the proposed heading estimation algorithm compared with some existing methods. The performance of the whole system also gets evaluated. The proposed algorithm and the error model show improvement over the classic methods. The algorithm considers the complicated environment and fully utilizes magnetic vectors to cooperate with the angular velocity collected by the gyroscope. The system contains more detail and has a better sense of robustness.

In future work, we intend to remove restrictions of the holding style of the device. The pedestrian would not always keep their devices align with the direction they walk. The state of the device that placed in the hand swinging would be another focus requires to resolve.