# Heading Estimation for Pedestrian Dead Reckoning Based on Robust Adaptive Kalman Filtering

^{*}

## Abstract

**:**

## 1. Introduction

- A heading estimation approach based on RAKF is proposed for PDR. Compared with the conventional KF-based approach, the proposed one uses an M-estimator-based model to control measurement outliers, and employs a state discrepancy statistic-based adaptive factor to resist the negative impacts of state model disturbances.
- Static tests were conducted, and the results indicate the advantages of our proposed approach over the conventional KF-based approach are faster converging speed, and more accurate estimation. Dynamic tests were carried out, and results of PDR demonstrate that our proposed approach provides more accurate and robust estimates, compared with the conventional KF-based approach.
- It is found that the proposed approach handles the issue of sudden turn in pedestrian location tracking quite well, and alleviates the problem of error accumulation effectively.

## 2. Heading Estimation for PDR Based on Smart Phone-Embedded MEMS Sensors

#### 2.1. Heading Representation and Determination

**q**is a 4-tuple:

**q**

^{−1}is the inverse of the quaternion

**q**:

#### 2.2. Heading Estimation Using Acceleration and Magnetic Field

#### 2.2.1. Magnetometer Calibration

- (1)
- Constructing an ellipsoid model

_{x}, m

_{y}, and m

_{z}denote the raw magnetometer measurements of a device in its body frame, sf

_{x}, sf

_{y}, and sf

_{z}denote the scale factors, $\mathrm{\Delta}{m}_{x0}$, $\mathrm{\Delta}{m}_{y0}$, and $\mathrm{\Delta}{m}_{z0}$ denote the hard iron-caused biases, R denotes the ellipsoid radius.

- (2)
- Estimating the parameters of the model

- (3)
- Correcting the magnetic field measurements

**m**(m

_{x}, m

_{y}, m

_{z}) are first shifted according to a vector Δ

**m**(Δm

_{x}

_{0}, Δm

_{y}

_{0}, Δm

_{z}

_{0}). Then, the measurements are scaled depending on a vector

**s**(1 + sf

_{x}, 1 + sf

_{y}, 1 + sf

_{z}). Finally, we will obtain calibrated measurements $\widehat{m}$ (${\widehat{m}}_{x}$, ${\widehat{m}}_{y}$, ${\widehat{m}}_{z}$).

**C**

_{sf}=

**s**·

**I**

_{3×3}denotes a scale transformation matrix.

#### 2.2.2. Heading Calculation

**q**

_{am}can be directly derived from acceleration vector and calibrated magnetic field vector $\widehat{m}$ by solving the Wahba’s problem [37]. Valenti et al. [13] proposed to decompose

**q**

_{am}into two quaternions,

**q**

_{a}and

**q**

_{m}which are determined by accelerations and magnetic field, respectively:

_{x}, a

_{y}, and a

_{z}denote the accelerometer measurements of a device in its body frame, ${\lambda}_{1}=\sqrt{\frac{{a}_{z}+1}{2}}$, and ${\lambda}_{2}=\sqrt{\frac{1-{a}_{z}}{2}}$. Then the calibrated magnetic field vector is rotated using the quaternion

**q**

_{a}:

**l**is the rotated magnetic field vector. Then the quaternion

**q**

_{m}is futher derived from

**l**:

_{x}and l

_{y}denote X and Y components of

**l**, $\mathrm{\Gamma}={l}_{x}^{2}+{l}_{y}^{2}$.

#### 2.3. Heading Estimation Using Angular Rate

_{x}, ω

_{y}, and ω

_{z}are the X, Y, and Z components of the gyroscope output in a device’s body frame. In order to obtain the results at different time instants, the discrete form of (21) should be used:

## 3. Robust Adaptive Kalman Filtering for Heading Estimation

#### 3.1. State and Measuring Models for Heading Estimation

**Z**

_{k}denote the measurement at time k:

**Z**

_{k}has to be changed by checking the difference between current predicted state ${X}_{k}$ and itself:

**H**is an identity matrix

**I**

_{4×4}, and ${v}_{k}$ denotes the model noise.

#### 3.2. Predicting

- Computing the predicted state ${\widehat{X}}_{k|k-1}$

- Computing the predicted state error variance matrix ${\widehat{P}}_{k|k-1}$:

**Q**

_{k}is the state model noise covariance matrix.

#### 3.3. Updating

- Computing the gain matrix ${K}_{k}$

**R**

_{k}. c is a constant, and it is usually within the range of [1.3, 2.0]. ${r}_{{k}_{i}}^{\prime}$ denotes the standard residual, and it is calculated by:

- Computing the corrected state ${\widehat{X}}_{k}$:

- Updating the state error variance matrix ${\widehat{P}}_{k}$:

## 4. Experimental Evaluation

#### 4.1. Experimental Setup

**Q**= 10

^{−10}*

**I**

_{4×4},

**R**= 10

^{−6}*

**I**

_{4×4}, and for dynamic tests,

**Q**= 10

^{−8}*

**I**

_{4×4},

**R**= 10

^{−6}*

**I**

_{4×4}. Moreover, the sampling frequency of data is 50 Hz.

#### 4.2. Results and Analysis

#### 4.2.1. Performances on Heading Estimation in the Static Tests

#### 4.2.2. Performances on Heading Estimation in the Dynamic Tests

- Results of the tests in the first site

- Results of the tests in the second site

## 5. Conclusions and Future Work

## Author Contributions

## Funding

## Conflicts of Interest

## References

- Qian, J.; Pei, L.; Ma, J.; Ying, R.; Liu, P. Vector graph assisted pedestrian dead reckoning using an unconstrained smartphone. Sensors
**2015**, 15, 5032–5057. [Google Scholar] [CrossRef] [PubMed] - Wang, J.; Hu, A.; Li, X.; Wang, Y. An improved PDR/magnetometer/floor map integration algorithm for ubiquitous positioning using the adaptive unscented Kalman filter. ISPRS Int. J. Geo-Inf.
**2015**, 4, 2638–2659. [Google Scholar] [CrossRef] - Guan, T.; Fang, L.; Dong, W.; Qiao, C. Robust Indoor Localization with Smartphones through Statistical Filtering. In Proceedings of the 2017 International Conference on Computing, Networking and Communications (ICNC), Santa Clara, CA, USA, 26–29 January 2017; IEEE: Piscataway, NJ, USA, 2017. [Google Scholar]
- Deng, Z.; Hu, Y.; Yu, J.; Na, Z. Extended Kalman filter for real time indoor localization by fusing WiFi and smartphone inertial sensors. Micromachines
**2015**, 6, 523–543. [Google Scholar] [CrossRef] - Li, X.; Wang, J.; Liu, C.; Zhang, L.; Li, Z. Integrated WiFi/PDR/smartphone using and adaptive system noise extended Kalman filter algorithm for indoor localization. ISPRS Int. J. Geo-Inf.
**2016**, 5, 8. [Google Scholar] [CrossRef] - Raitoharju, M.; Nurminen, H.; Piche, R. Kalman filter with a linear state model for PDR+WLAN positioning and its application to assisting a particle filter. EURASIP J. Adv. Signal Process.
**2015**, 2015, 33. [Google Scholar] [CrossRef] - Wu, D.; Xia, L. Hybrid Location Estimation by Fusing WLAN Signals and Inertial Data. In Principle and Application Progress in Location-Based Services; Lecture Notes in Geoinformation and Cartography; Springer International Publishing: Basel, Switzerland, 2014; pp. 81–92. [Google Scholar]
- Li, Z.; Feng, L.; Yang, A. Fusion based on visible light positioning and inertial navigation using extended Kalman filter. Sensors
**2017**, 17, 1093. [Google Scholar] [CrossRef] - Deng, Z.; Wang, G.; Hu, Y.; Wu, D. Heading estimation for indoor pedestrian navigation using a smartphone in the pocket. Sensors
**2015**, 15, 21518–21536. [Google Scholar] [CrossRef] [PubMed] - Wu, J.; Zhou, Z.; Chen, J.; Fourati, H.; Li, R. Fast complementary filter for attitude estimation using low-cost MARG sensors. IEEE Sens. J.
**2016**, 18, 6997–7007. [Google Scholar] [CrossRef] - Valenti, R.G.; Dryanovski, I.; Xiao, J. Keeping a good attitude: A quaternion-based orientation filter for IMUs and MARGs. Sensors
**2015**, 15, 19302–19330. [Google Scholar] [CrossRef] [PubMed] - Kottath, R.; Narkhede, P.; Kumar, V.; Karar, V.; Poddar, S. Multiple model adaptive complementary filter fot attitude estimation. Aerosp. Sci. Technol.
**2017**, 69, 574–581. [Google Scholar] [CrossRef] - Valenti, R.; Dryanovski, I.; Xian, J. A linear Kalman filter for MARG orientation estimaton using the algebraic quaternion algorithm. IEEE Trans. Instrum. Meas.
**2016**, 65, 467–481. [Google Scholar] [CrossRef] - Yuan, X.; Yu, S.; Zhang, S.; Wang, G.; Liu, S. Quternion-based unscented Kalman filter for accurate indoor heading estimation using wearable multi-sensor system. Sensors
**2015**, 15, 10872–10890. [Google Scholar] [CrossRef] [PubMed] - Zhang, S.; Yu, S.; Liu, C.; Yuan, X.; Liu, S. A dual-linear Kalman filter for real-time orientation determination system using low-cost MEMS sensors. Sensros
**2016**, 16, 264. [Google Scholar] [CrossRef] [PubMed] - Feng, K.; Li, J.; Zhang, X.; Shen, C.; Bi, Y.; Zheng, T.; Liu, J. A new quaternion-based Kalman filter for real-time attitude estimation using the two-step geometrically-intuitive correction algorithm. Sensors
**2017**, 17, 2146. [Google Scholar] [CrossRef] [PubMed] - Ettlinger, A.; Neuner, H.; Burgess, T. Development of a Kalman filter in the Gauss-Helmert model for reliability analysis in orientation determination with smartphone sensors. Sensors
**2018**, 18, 414. [Google Scholar] [CrossRef] [PubMed] - Ding, W.; Wang, J.; Rizos, C. Improving adaptive Kalman estimation in GPS/INS integration. J. Navig.
**2007**, 60, 517–529. [Google Scholar] [CrossRef] - Hu, C.; Chen, W.; Chen, Y.; Liu, D. Adaptive Kalman filtering for vehicle navigation. J. Glob. Position. Syst.
**2003**, 2, 42–47. [Google Scholar] [CrossRef] - Li, W.; Wang, J. Effective adaptive Kalman filter for MEMS-IMU/magnetometers integrated attitude and heading reference systems. J. Navig.
**2013**, 66, 99–113. [Google Scholar] [CrossRef] - Zheng, B.; Fu, P.; Li, B.; Yuan, X. A robust adaptive unscented Kalman filter for nonlinear estimation with uncertain noise covariance. Sensors
**2018**, 18, 808. [Google Scholar] [CrossRef] [PubMed] - Hide, C.; Michaud, F.; Smith, M. Adaptive Kalman Filtering Algorithms for Integrating GPS and Low Cost INS. In Proceedings of the IEEE Position Location and Navigation Symposium, Monterey, CA, USA, 26–29 April 2004; pp. 227–233. [Google Scholar]
- Yang, Y. Comparison of adaptive factors in Kalman filters on navigation results. J. Navig.
**2005**, 58, 471–478. [Google Scholar] [CrossRef] - Yang, Y.; Gao, W. An optimal adaptive Kalman filter. J. Geodesy
**2006**, 80, 177–183. [Google Scholar] [CrossRef] - Yang, Y.; Xu, T. An adaptive Kalman filter based on sage windowing weights and variance components. J. Navig.
**2003**, 56, 231–240. [Google Scholar] [CrossRef] - Wu, F.; Yang, Y. An extended adaptive Kalman filtering in tight coupled GPS/INS integration. Surv. Rev.
**2010**, 42, 146–154. [Google Scholar] - Lee, D.; Vukovich, G.; Lee, R. Robust adaptive unscented Kalman filter for spacecraft attitude estimation using quaternion measurements. J. Aerosp. Eng.
**2017**, 30, 04017009. [Google Scholar] [CrossRef] - Hajiyev, C.; Soken, H.E. Robust adaptive unscented Kalman filter for attitude estimation of pico satellites. Int. J. Adapt. Control Signal Process.
**2014**, 28, 107–120. [Google Scholar] [CrossRef] - Hide, C.; Moore, T.; Smith, M. Multiple Model Kalman Filtering for GPS and Low-Cost INS Integration. In Proceedings of the ION GNSS 17th International Technical Meeting of the Satellite Division, Long Beach, CA, USA, 21–24 September 2004; pp. 1096–1103. [Google Scholar]
- Mohamed, A.H.; Schwarz, K.P. Adaptive Kalman filtering for INS/GPS. J. Geodesy
**1999**, 73, 193–203. [Google Scholar] [CrossRef] - Wang, J. Stochastic modelling for RTK GPS/GLONASS positioning and navigation. J. Inst. Navig.
**2000**, 46, 297–305. [Google Scholar] [CrossRef] - Yang, Y.; He, H.; Xu, G. Adaptively robust filtering for kinematic geodetic positioning. J. Geodesy
**2001**, 75, 109–116. [Google Scholar] [CrossRef] - Henderson, D.M. Euler Angles, Quaternions, and Transformation Matrices; Technical Reports; NASA: Washington, DC, USA, 1977.
- Diebel, J. Representing attitude: Euler angles, unit quaternions, and rotation vectors. Matrix
**2006**, 58, 1–35. [Google Scholar] - Horn, B. Closed-form solution of absolute orientation using unit quaternion. J. Opt. Soc. Am. A
**1987**, 4, 629–642. [Google Scholar] [CrossRef] - Gebre-Egziabher, D.; Elkaim, G.H.; Powell, J.D.; Parkinson, B.W. Calibration of strapdown magnetometers in magnetic field domain. J. Aerosp. Eng.
**2006**, 19, 87–102. [Google Scholar] [CrossRef] - Wahba, G. A least squares estimate of satellite attitude. SIAM Rev.
**1965**, 7, 409. [Google Scholar] [CrossRef] - Chen, W.; Chen, R.; Chen, Y.; Kuusniemi, H.; Wang, J.; Fu, Z. An Effective Pedestrian Dead Reckoning Algorithm Using a Unified Heading Error Model. In Proceedings of the IEEE/ION Position, Location and Navigation Symposium (PLANS) 2010, Indian Wells, CA, USA, 3–6 May 2010; pp. 340–347. [Google Scholar]

**Figure 2.**3-dimension locus magnetic field measurements, (

**a**) error-free, (

**b**) contaminated by hard iron, and scale factor error.

**Figure 5.**Standard deviations and mean values of absolute heading errors with respect to different values of the adaptive parameter c0.

**Figure 6.**Comparisons of the performances on heading estimation of different algorithms in the static test.

**Figure 7.**Comparisons of the performances on heading estimation of KF and RAKF with respect to three participants, (

**a**) participant 1, (

**b**) participant 2, and (

**c**) participant 3.

**Figure 8.**Comparisons of the performances on location tracking of KF and RAKF with respect to three participants, (

**a**) participant 1, (

**b**) participant 2, and (

**c**) participant 3.

**Figure 9.**Distributions of location errors in location tracking of KF and RAKF with respect to three participants, (

**a**) participant 1, (

**b**) participant 2, and (

**c**) participant 3.

**Figure 10.**Comparisons of the performances on heading estimation of KF and RAKF regarding five participants, (

**a**) participant 1, (

**b**) participant 2, (

**c**) participant 3, (

**d**) participant 4, and (

**e**) participant 5.

**Figure 11.**Comparisons of the performances on location tracking of KF and RAKF regarding five participants, (

**a**) participant 1, (

**b**) participant 2, (

**c**) participant 3, (

**d**) participant 4, and (

**e**) participant 5.

**Figure 12.**Distributions of location errors in location tracking of KF and RAKF regarding five participants, (

**a**) participant 1, (

**b**) participant 2, (

**c**) participant 3, (

**d**) participant 4, and (

**e**) participant 5.

**Figure 13.**Mean values of location errors in location tracking of KF and RAKF regarding five participants (the error bar stands for the STD. = of location errors).

**Figure 14.**Location tracking performances in terms of (

**a**) trajectory, (

**b**) location error distribution of different algorithms regarding the participant 3.

**Table 1.**Detailed information of all participants (K is the step length parameter used in Section 4.2.2).

Participant | Sex | Height (m) | Weight (Kg) | K |
---|---|---|---|---|

1 | Male | 1.66 | 59 | 0.36 |

2 | Male | 1.75 | 75 | 0.43 |

3 | Male | 1.71 | 60 | 0.39 |

4 | Female | 1.61 | 52 | 0.4 |

5 | Male | 1.64 | 65 | 0.37 |

Algorithms | Mean (Rad) | STDEV. (Rad) |
---|---|---|

KF | 0.002232 | 0.003297 |

RAKF (c = 1.5, c0 = 1.5) | 0.002049 | 0.002028 |

RAKF (c = 1.5, c0 = 15) | 0.00184 | 0.002776 |

Participants | Error Metrics | KF | RAKF |
---|---|---|---|

Participant 1 | Mean error (m) | 1.48 | 1.35 |

STD. error (m) | 0.90 | 0.81 | |

Participant 2 | Mean error (m) | 1.41 | 0.85 |

STD. error (m) | 0.94 | 0.44 | |

Participant 3 | Mean error (m) | 2.10 | 1.78 |

STD. error (m) | 1.20 | 0.99 |

Algorithm | Average Time (ms) |
---|---|

KF | 0.036610526 |

RAKF | 0.040133333 |

© 2018 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).

## Share and Cite

**MDPI and ACS Style**

Wu, D.; Xia, L.; Geng, J.
Heading Estimation for Pedestrian Dead Reckoning Based on Robust Adaptive Kalman Filtering. *Sensors* **2018**, *18*, 1970.
https://doi.org/10.3390/s18061970

**AMA Style**

Wu D, Xia L, Geng J.
Heading Estimation for Pedestrian Dead Reckoning Based on Robust Adaptive Kalman Filtering. *Sensors*. 2018; 18(6):1970.
https://doi.org/10.3390/s18061970

**Chicago/Turabian Style**

Wu, Dongjin, Linyuan Xia, and Jijun Geng.
2018. "Heading Estimation for Pedestrian Dead Reckoning Based on Robust Adaptive Kalman Filtering" *Sensors* 18, no. 6: 1970.
https://doi.org/10.3390/s18061970