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

Pedestrian dead reckoning (PDR) using smart phone-embedded micro-electro-mechanical system (MEMS) sensors plays a key role in ubiquitous localization indoors and outdoors. However, as a relative localization method, it suffers from the problem of error accumulation which prevents it from long term independent running. Heading estimation error is one of the main location error sources, and therefore, in order to improve the location tracking performance of the PDR method in complex environments, an approach based on robust adaptive Kalman filtering (RAKF) for estimating accurate headings is proposed. In our approach, outputs from gyroscope, accelerometer, and magnetometer sensors are fused using the solution of Kalman filtering (KF) that the heading measurements derived from accelerations and magnetic field data are used to correct the states integrated from angular rates. In order to identify and control measurement outliers, a maximum likelihood-type estimator (M-estimator)-based model is used. Moreover, an adaptive factor is applied to resist the negative effects of state model disturbances. Extensive experiments under static and dynamic conditions were conducted in indoor environments. The experimental results demonstrate the proposed approach provides more accurate heading estimates and supports more robust and dynamic adaptive location tracking, compared with methods based on conventional KF.


Introduction
The expansion of location-based services (LBS) and applications has led to extensive interest in ubiquitous localization which may rely on widely used smart phones. The rich sensors embedded in smart phones support vary types of localization techniques, such as cellular localization, WiFi localization, vision-based location tracking, micro-electro-mechanical system (MEMS) sensors-based pedestrian dead reckoning (PDR), etc. However, a stand-alone technique cannot satisfy all positioning demands of LBS. For example, Global Navigation Satellite Systems (GNSS) cannot work in blocked regions, and WiFi localization systems (WLS) are limited by the coverage of WiFi signals, etc. Among these techniques, PDR is of great importance that it can flexibly link up different absolute positioning systems (such as GNSS, WLS, etc.) to achieve ubiquitous location provision for LBS. However, as a kind of relative localization method, it suffers from the problem of location error accumulation, and therefore cannot hold its performance continuously. Thus, it is essential to improve the tracking performance of the PDR method. Although external techniques, such as indoor graph matching [1][2][3], WiFi localization [3][4][5][6][7], visible light positioning [8] etc. have been considered to assist PDR, it is crucial to enhance its own performance in several aspects, such as speed estimation, heading determination and position calculation, the errors of which can propagate and can result in Actually, besides the above presented solutions, location accuracy of conventional filters can also be improved by using adaptive methods. For example, Ding et al. [18] proposed a process noise scaling algorithm for autonomously tuning the process noise covariance to the optimal magnitude. Hu et al. [19] investigated two adaptive algorithms which were based on fading memory and variance component estimation respectively, and found that both algorithms perform better than conventional KF, and the variance component estimation filter achieves the best positioning accuracy. Li et al. [20] proposed an effective adaptive Kalman filter for attitude and heading estimation. When filtering, the noise variance matrix R is tuned by a three-segment function that is constructed depending on the level of acceleration. Zheng et al. [21] proposed a robust adaptive UKF with a two-step adaptive scheme. First, an innovation-based statistic is used to identify model errors, and then if model errors exist, two adaptive factors are applied to control the noise covariance matrices Q and R by balancing the last noise covariance matrices and the estimated ones. In summary, there are three kinds of adaptive filtering algorithms [18,22], such as the covariance scaling-based adaptive filter [19,21,[23][24][25][26][27][28], the multi-model adaptive estimation-based filter [29], and adaptive stochastic modelling-based filter [18,20,30,31]. Moreover, in order to control the influence of measurement outliers, Yang et al. [32] combined robust estimation and adaptive filtering and proposed the theory of adaptively robust Kalman filtering for kinematic navigation and positioning. Yang also summarized the models and the judging statistics for constructing adaptive factors systematically, and explained the relations between their proposed algorithm and other filters in detail. Although these adaptive filtering algorithms have been widely applied in various fields, the application in PDR has been rarely investigated.
Considering pedestrians' complex walking patterns, it is challenging to track or position them with their smart devices (smart phone, smart watch, etc.). As a result, in order to improve the tracking performances of PDR, a method based on robust adaptive Kalman filtering (RAKF) is proposed for heading estimation. Outputs from gyroscope, accelerometer and magnetometer sensors are used. To resist the negative impacts from measurement outliers and state model disturbances, a maximum likelihood-type estimator (M-estimator)-based model is used in combination with an adaptive factor. Generally, the contributions of our work can be summarized as follows: • 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.
In the rest of this paper, we first present the procedure for heading estimation by using smart phone-embedded MEMS sensors in Section 2. After that, we explain the proposed RAKF algorithm in detail in Section 3. In Section 4, we show the experiments and results. At last, we draw conclusions and future work in Section 5.

Heading Estimation for PDR Based on Smart Phone-Embedded MEMS Sensors
Low cost MEMS sensors embedded in smart phones, such as accelerometers, magnetometers, and gyroscopes provide raw data for pedestrian speed estimation and heading estimation. In this paper, we assume that the heading of a pedestrian and that of his/her smart phone coincide. Thus, only the heading of the smart phone needs to be determined. As Figure 1 presents, raw data from the three sensors are used in two ways, the acceleration and magnetic field data are combined to calculate absolute headings, and the angular rate is used to integrate relative headings. The two kinds of headings are then fused using a filtering algorithm to obtain optimal values which may be used iteratively in the angular rate integration. headings are then fused using a filtering algorithm to obtain optimal values which may be used iteratively in the angular rate integration.

Heading Representation and Determination
Attitude and heading for a rigid body are always handled together. To represent the attitude heading, we define an orthogonal body frame (X-Y-Z) B in which Y and Z axes link up with the forward and up directions respectively, and X axis points to the right. Commonly, the attitude is determined by the rotation matrix with respect to the ENU (East (X)-North (Y)-Up (Z)) frame (also named navigation frame N). Define vector n X in frame N, and the corresponding vector b X in frame B, the mapping between the two vectors can be expressed as: where, b n C represents the rotation matrix from the frame N to the frame B. To be specific, suppose the frame N first rotate around Z axis with an angle ψ, and then rotate around X axis about an angle θ, and finally rotate around Y axis with an angle φ, the rotation matrix b n C will be calculated as: and it can be further written as: According to the definition of body frame, ψ, θ, and φ are called heading, pitch, and roll angles respectively. It is noticed that b n C will be different with respect to other rotational orders [33]. Since Euler angles have the problems of singularity and lower computation efficiency [14], quaternion is designed to replace it for attitude representation. A quaternion q is a 4-tuple: where 0 q is the scalar part, and   1 2 3 T q q q  e denotes the vector part. In this paper, unit quaternion that is with the constraint of unity norm: is used. Likewise, a unit quaternion can also be used to represent the attitude of a rigid body. Consider the vectors defined above, the mapping can be expressed as [34,35]:

Heading Representation and Determination
Attitude and heading for a rigid body are always handled together. To represent the attitude heading, we define an orthogonal body frame (X-Y-Z) B in which Y and Z axes link up with the forward and up directions respectively, and X axis points to the right. Commonly, the attitude is determined by the rotation matrix with respect to the ENU (East (X)-North (Y)-Up (Z)) frame (also named navigation frame N). Define vector X n in frame N, and the corresponding vector X b in frame B, the mapping between the two vectors can be expressed as: where, C b n represents the rotation matrix from the frame N to the frame B. To be specific, suppose the frame N first rotate around Z axis with an angle ψ, and then rotate around X axis about an angle θ, and finally rotate around Y axis with an angle φ, the rotation matrix C b n will be calculated as: and it can be further written as: cos ϕ cos ψ − sin ψ sin θ sin ϕ cos ϕ sin ψ + cos ψ sin θ sin ϕ − cos θ sin ϕ − sin ψ cos θ cos θ cos ψ sin θ sin ϕ cos ψ + sin ψ sin θ cos ϕ sin ϕ sin ψ − cos ψ sin θ cos ϕ cos θ cos ϕ According to the definition of body frame, ψ, θ, and φ are called heading, pitch, and roll angles respectively. It is noticed that C b n will be different with respect to other rotational orders [33]. Since Euler angles have the problems of singularity and lower computation efficiency [14], quaternion is designed to replace it for attitude representation. A quaternion q is a 4-tuple: where q 0 is the scalar part, and e = q 1 q 2 q 3 T denotes the vector part. In this paper, unit quaternion that is with the constraint of unity norm: is used. Likewise, a unit quaternion can also be used to represent the attitude of a rigid body. Consider the vectors defined above, the mapping can be expressed as [34,35]: where ⊗ indicates the quaternion multiplication, q −1 is the inverse of the quaternion q: According to the matrix form of quaternion multiplication [34], (6) can be expanded as: where M(q) is the quaternion matrix function [34], and M(q) is its conjugate form. At last, we can get: A similar equation as (1) can be derived from (9): where C b n (q) is the rotation matrix formed by using quaternion: Inspection of (3) and (11) yields the calculation of the attitude heading:

Heading Estimation Using Acceleration and Magnetic Field
Having defined how to represent headings, accelerations and magnetic fields can be used to estimate meaningful headings for PDR.

Magnetometer Calibration
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. These errors can bias the magnetometer outputs, or be superimposed on the outputs. Methods for removing the negative impacts caused by these errors are needed. In this paper, we assume that the outputs of the magnetometer in a smart device are mainly corrupted by the hard iron and scale factor errors. Thus, a method that recovers the locus of error-free magnetic field measurements as Figure 2a presents from an altered locus as Figure 2b presents is applied. In general, the procedure can be summarized as follows: (1) Constructing an ellipsoid model We can see from Figure 2b that the magnetic field measurements at a given geographical location without calibration approximates an ellipsoid, thus an ellipsoid model that can adjust the bias and non-uniform scale is constructed: where mx, my, and mz denote the raw magnetometer measurements of a device in its body frame, sfx, sfy, and sfz denote the scale factors, (2) Estimating the parameters of the model To fit the best ellipsoid and to estimate the six parameters accurately, enough measurements that span the entire Euler angle space at a given location should be collected. With the collected data, a least square (LS) estimation algorithm can be used to approximate the model. Detailed implementation of the LS algorithm refers to [36].

Heading Calculation
Once the magnetometer is calibrated, absolute headings with better accuracy can be obtained. Quaternion heading qam can be directly derived from acceleration vector and calibrated magnetic field vector m by solving the Wahba's problem [37]. Valenti et al. [13] proposed to decompose qam into two quaternions, qa and qm which are determined by accelerations and magnetic field, respectively: In general, the procedure can be summarized as follows: (1) Constructing an ellipsoid model We can see from Figure 2b that the magnetic field measurements at a given geographical location without calibration approximates an ellipsoid, thus an ellipsoid model that can adjust the bias and non-uniform scale is constructed: where m 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, ∆m x0 , ∆m y0 , and ∆m z0 denote the hard iron-caused biases, R denotes the ellipsoid radius.
(2) Estimating the parameters of the model To fit the best ellipsoid and to estimate the six parameters accurately, enough measurements that span the entire Euler angle space at a given location should be collected. With the collected data, a least square (LS) estimation algorithm can be used to approximate the model. Detailed implementation of the LS algorithm refers to [36].
(3) Correcting the magnetic field measurements With the estimated two tuples of parameters, magnetometer outputs can be calibrated. Raw measurements m (m x , m y , m z ) are first shifted according to a vector ∆m (∆m x0 , ∆m y0 , ∆m z0 ). Then, the measurements are scaled depending on a vector s (1 + sf x , 1 + sf y , 1 + sf z ). Finally, we will obtain calibrated measurementsm (m x ,m y ,m z ).
where C sf = s · I 3×3 denotes a scale transformation matrix.

Heading Calculation
Once the magnetometer is calibrated, absolute headings with better accuracy can be obtained. Quaternion heading q am can be directly derived from acceleration vector and calibrated magnetic field vectorm 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: where a x , a y , and a z denote the accelerometer measurements of a device in its body frame, λ 1 = a z +1 2 , and λ 2 = 1−a z 2 . Then the calibrated magnetic field vector is rotated using the quaternion q a : where l is the rotated magnetic field vector. Then the quaternion q m is futher derived from l: where l x and l y denote X and Y components of l, Γ = l 2 x + l 2 y .

Heading Estimation Using Angular Rate
The angular rates output by the gyroscope can also be used to estimate quaternion attitude headings which represent the changed value relative to the initial quaternion, and the estimation is based on a differential equation: where s ω is constructed with the gyroscope output: where ω 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: Using the quaternion matrix function, (23) can be further expanded as: where M(s ω,t ) is the quaternion matrix function of s ω,t . The above two methods for heading estimation can be combined to produce more robust and accurate results, and a frame of KF is applied in this paper. In the following, the process of RAKF is explained in detail.

State and Measuring Models for Heading Estimation
According to the above equations for calculating quaternion headings, the state and measuring models are designed as follows: Let X k represent the state at time k, and F k = I 4×4 + ∆t 2 M(s ω,k ), the state model can be formed as: where w k denotes the model noise.
Let Z k denote the measurement at time k: where q am,k is calculated using (17). To avoid the discontinuities of headings caused by (18) and (20), Z k has to be changed by checking the difference between current predicted state X k and itself: where d k = Z k · X k is the dot product of two quaternions. A linear function is enough to construct the measuring model: where H is an identity matrix I 4×4 , and v k denotes the model noise.
Since a unit quaternion is used in our designed algorithm, the initial state value X 0 = q ω,0 and all the measurements Z 1:k = {Z i , i = 1, · · · , k} should be normalized before they are inputted into the process of filtering. Given the models for heading estimation are designed above, a RAKF algorithm is used for obtaining optimal results. The algorithm consists of two procedures, predicting and updating which are presented in the subsequent sections.

•
Computing the predicted stateX k|k−1 According to the state model in (25), the predicted state can be calculated as: • Computing the predicted state error variance matrixP k|k−1 : where Q k is the state model noise covariance matrix.

Updating
• Computing the gain matrix K k In the RAKF, the computation of K k is different from conventional implementation in the KF. To control the outliers in the measurements, an M-estimator-based robust estimation of the equivalent weight matrix P k of the measurements is used. Among several formatting methods, we choose Huber's approach [32]. Then, the diagonal p k ii and non-diagonal p k ij elements of P k are determined as follows: where σ ii and σ ij are diagonal and non-diagonal elements of the measurement noise covariance matrix R k . c is a constant, and it is usually within the range of [1.3, 2.0]. r k i denotes the standard residual, and it is calculated by: where r k i is the residual of the measurement z k i , and σ r k i is the mean deviation of r k i : whereẐ k|k−1 is the predicted measurement which is calculated depending on the measuring model in (28): In order to control the influence of dynamic model error, an adaptive factor is applied for correcting the predicted state error variance matrixP k|k−1 . Before calculating the adaptive factor, a kind of state discrepancy statistic for judging the state model errors [26,32] is chosen as: where tr(·) stands for the trace of a matrix. X k is a least-square estimator of the state: where P k = R −1 k denotes the weight matrix. To avoid measurement outliers, equivalent weight matrix P k can be applied in (37).
With the chosen statistic ∆ X k , a two-segment function is applied for calculating the adaptive factor: where c0 is a constant which can be tuned depending on practical implementation. Having weaken the negative impacts from measurement outliers and state model errors, a proper gain matrix can be obtained: • Computing the corrected stateX k :X The state needs to be normalized further: • Updating the state error variance matrixP k : Using the equations from (29) to (42), headings can be estimated iteratively in the frame of RAKF.

Experimental Setup
To evaluate the proposed heading estimation approach, we conducted extensive tests in two situations, static and dynamic. In the static tests, a Xiaomi 5 smart phone was put still on a table in an office, collect data covering more than ten minutes. All the heading results calculated from accelerations and magnetic field were averaged to obtain the reference heading value. Additionally, dynamic tests were performed in the corridors on the fifth and the seventh floors in the research building for the School of Geography and Planning at Sun Yat-sen University. The floor plans are presented in Figure 3a,b, respectively. We employed five persons to participate in collecting data. The participants had different heights, different weights, and different walking postures. Table 1 lists the detailed information of each participant. They were all asked to hold the Xiaomi 5 smart phone on their chest to collect data along labeled traces which are marked by black lines in Figure 3a,b respectively. For the tests in the first site on the fifth floor, three participants were involved in collecting data where they walked back and forth twice, and finally returned back to the start point after three sharp turns. The length of the location traces that they walked is as long as 150.4 m each. Whereas for the tests in the second site on the seventh floor, all of the five participants walked from the start point to the end point once. The length of each traces is about 68 m. A Xiaoyi 4kplus sports camera was used to record their walking, and relative accurate positions were derived from the videos to construct the reference traces. Conventional KF is used as the baseline for comparisons, and the noise covariance matrix of both the measurements and the states are set differently in the static and dynamic tests. The matrices are empirically determined depending on the standard deviation of each measurement outputted by the smartphone. For static tests, 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.  Conventional KF is used as the baseline for comparisons, and the noise covariance matrix of both the measurements and the states are set differently in the static and dynamic tests. The matrices are empirically determined depending on the standard deviation of each measurement outputted by the smartphone. For static tests, Q = 10 −10 * I4×4, R = 10 −6 * I4×4, and for dynamic tests, Q = 10 −8 * I4×4, R = 10 −6 * I4×4. Moreover, the sampling frequency of data is 50 Hz.

Performances on Heading Estimation in the Static Tests
Robust estimation can control the outputs of KF by using a parameter for determining which part of the measurements may cause negative influences. In this paper, the weights of the "negative measurements" are reduced to alleviate their impacts. However, if initial values for KF have not given properly, robust estimation can result slower convergence. Figure 4 presents heading errors of KF and its variants (Robust KF, RKF) with different values of the robust parameter. We find that RKFs produce significant heading errors at the beginning of filtering, but converge to relatively smooth results after a time period. Morever, the smaller the parameter is, the smoother the results are during the subsequent time period. Thus, the value in subsequent experiments is set to 1.5 which means the measurements with absolute errors over 1.5σ (mean squared error) will be handled.

Performances on Heading Estimation in the Static Tests
Robust estimation can control the outputs of KF by using a parameter for determining which part of the measurements may cause negative influences. In this paper, the weights of the "negative measurements" are reduced to alleviate their impacts. However, if initial values for KF have not given properly, robust estimation can result slower convergence. Figure 4 presents heading errors of KF and its variants (Robust KF, RKF) with different values of the robust parameter. We find that RKFs produce significant heading errors at the beginning of filtering, but converge to relatively smooth results after a time period. Morever, the smaller the parameter is, the smoother the results are during the subsequent time period. Thus, the value in subsequent experiments is set to 1.5 which means the measurements with absolute errors over 1.5σ (mean squared error) will be handled. The state discrepancy statistic-based adaptive method determines the adaptive factor depending on the difference between the predicted state and the result estimated using the acceleration and magnetic field data. The adaptive factor can directly change the accuracy and precision of the filtering results. Figure 5 presents mean values and standard deviations of absolute heading errors with respect to different values of adaptive parameters. The chosen of the adaptive parameter should balance the two aspects. Figure 6 presents results produced by KF and RAKFs with two different adaptive parameters. We can see that the results produced by the RAKF with an adaptive parameter of 1.5 converge faster but fluctuate with a larger amplitude, whereas the outputs of the RAKF with an adaptive parameter of 15 are smooth but converge slower. We adopt the average value of all the heading estimations from accelerometer and magnetometer as the true value to obtain the statistical results of estimation errors of KF and RAKF. Table 2 presents the results, and we can see that RAKFs estimate more accurate and steady headings than KF, the mean errors decrease about 8.2% and 17.6% respectively, and the standard deviations decrease about 38.5% and 15.8% respectively. Additionally, the results indicate special characteristics of RAKFs with different adaptive parameters.  The state discrepancy statistic-based adaptive method determines the adaptive factor depending on the difference between the predicted state and the result estimated using the acceleration and magnetic field data. The adaptive factor can directly change the accuracy and precision of the filtering results. Figure 5 presents mean values and standard deviations of absolute heading errors with respect to different values of adaptive parameters. The chosen of the adaptive parameter should balance the two aspects. Figure 6 presents results produced by KF and RAKFs with two different adaptive parameters. We can see that the results produced by the RAKF with an adaptive parameter of 1.5 converge faster but fluctuate with a larger amplitude, whereas the outputs of the RAKF with an adaptive parameter of 15 are smooth but converge slower. We adopt the average value of all the heading estimations from accelerometer and magnetometer as the true value to obtain the statistical results of estimation errors of KF and RAKF. Table 2 presents the results, and we can see that RAKFs estimate more accurate and steady headings than KF, the mean errors decrease about 8.2% and 17.6% respectively, and the standard deviations decrease about 38.5% and 15.8% respectively. Additionally, the results indicate special characteristics of RAKFs with different adaptive parameters.

Performances on Heading Estimation in the Dynamic Tests
To further verify the superiority of the proposed RAKF in heading estimation, dynamic tests were conducted at two test sites. Since reference headings in dynamic tests are hard to obtain, estimation errors cannot be presented straightforwardly. Fortunately, location tracking performance of PDR can reflect heading estimation performance to some extent. Therefore, in order to examine heading errors, headings estimated by KF and RAKF are applied respectively in PDR location tracking which is based on conventional EKF.
An EKF-based PDR always consists of three parts, heading estimation, speed estimation, and location tracking. Except heading estimation, the other two parts are explained simply in the following. Detailed implementation refers to [38]. Speed estimation contains two steps, stride detection and step length estimation. For stride detection, peaks of measured total acceleration are counted. For step length estimation, a one-parameter nonlinear model [7,38] is employed:

Performances on Heading Estimation in the Dynamic Tests
To further verify the superiority of the proposed RAKF in heading estimation, dynamic tests were conducted at two test sites. Since reference headings in dynamic tests are hard to obtain, estimation errors cannot be presented straightforwardly. Fortunately, location tracking performance of PDR can reflect heading estimation performance to some extent. Therefore, in order to examine heading errors, headings estimated by KF and RAKF are applied respectively in PDR location tracking which is based on conventional EKF.
An EKF-based PDR always consists of three parts, heading estimation, speed estimation, and location tracking. Except heading estimation, the other two parts are explained simply in the following. Detailed implementation refers to [38]. Speed estimation contains two steps, stride detection and step length estimation. For stride detection, peaks of measured total acceleration are counted. For step length estimation, a one-parameter nonlinear model [7,38] is employed:

Performances on Heading Estimation in the Dynamic Tests
To further verify the superiority of the proposed RAKF in heading estimation, dynamic tests were conducted at two test sites. Since reference headings in dynamic tests are hard to obtain, estimation errors cannot be presented straightforwardly. Fortunately, location tracking performance of PDR can reflect heading estimation performance to some extent. Therefore, in order to examine heading errors, headings estimated by KF and RAKF are applied respectively in PDR location tracking which is based on conventional EKF.
An EKF-based PDR always consists of three parts, heading estimation, speed estimation, and location tracking. Except heading estimation, the other two parts are explained simply in the following. Detailed implementation refers to [38]. Speed estimation contains two steps, stride detection and step length estimation. For stride detection, peaks of measured total acceleration are counted. For step length estimation, a one-parameter nonlinear model [7,38] is employed: where A max (or A min ) is the maximum (or minimum) vertical acceleration in a single step and K is a constant. An assumption is that the leg is a lever of fixed length while the foot is on the ground. Location tracking is based on the primary theory of dead reckoning [7,38], and it is implemented in the frame of EKF, as presented in [38]. Depending on different walking patterns of participants, K in (43) is set separately. The value of K for each participant is presented in Table 1. Other settings for the filtering, such as model noise variances, robust parameter and adaptive parameter are with the same values in which the robust and the adaptive parameters are set as 1.5 and 3, respectively.

•
Results of the tests in the first site The heading estimation results of KF and RAKF are presented in Figure 7. We can see that, the results of RAKF are smoother than those of KF. Noticeably, the most important improvement that is marked by black circles in Figure 7a,b of RAKF for heading estimation is the ability of controlling outliers, as well as the adaptation to sudden heading changes, compared with KF. However, similar improvements cannot be found in Figure 7c which means sudden turns did not cause significant heading errors during participant 3's walking. The results also indicate that different walking characteristics of the three participants bring about great challenges for heading estimation based on RAKF with constant robust and adaptive parameters. where max A (or m in A ) is the maximum (or minimum) vertical acceleration in a single step and K is a constant. An assumption is that the leg is a lever of fixed length while the foot is on the ground. Location tracking is based on the primary theory of dead reckoning [7,38], and it is implemented in the frame of EKF, as presented in [38].
Depending on different walking patterns of participants, K in (43) is set separately. The value of K for each participant is presented in Table 1. Other settings for the filtering, such as model noise variances, robust parameter and adaptive parameter are with the same values in which the robust and the adaptive parameters are set as 1.5 and 3, respectively.


Results of the tests in the first site The heading estimation results of KF and RAKF are presented in Figure 7. We can see that, the results of RAKF are smoother than those of KF. Noticeably, the most important improvement that is marked by black circles in Figure 7a,b of RAKF for heading estimation is the ability of controlling outliers, as well as the adaptation to sudden heading changes, compared with KF. However, similar improvements cannot be found in Figure 7c which means sudden turns did not cause significant heading errors during participant 3's walking. The results also indicate that different walking characteristics of the three participants bring about great challenges for heading estimation based on RAKF with constant robust and adaptive parameters. More intuitive improvements can be observed in location tracking, performances of which are presented in Figure 8. For all three participants, the RAKF results approximate the reference trace better, compared with the results of KF. Location errors in tracking are further shown in Figure 9. All three figures indicate that KF and RAKF both provide low location errors at the beginning of tracking, but KF performs as worse as the walking distance becomes longer. The performances demonstrate that PDR suffers from location error accumulation, but RAKF is able to handle the problem to some extent. Finally, Table 3 gives the statistical results of location errors. Compared with KF, the outputs of RAKF are with higher accuracy and precision. The mean errors of RAKF' outputs decrease 8.8%, More intuitive improvements can be observed in location tracking, performances of which are presented in Figure 8. For all three participants, the RAKF results approximate the reference trace better, compared with the results of KF. Location errors in tracking are further shown in Figure 9. All three figures indicate that KF and RAKF both provide low location errors at the beginning of tracking, but KF performs as worse as the walking distance becomes longer. The performances demonstrate that PDR suffers from location error accumulation, but RAKF is able to handle the problem to some extent. Finally, Table 3 gives the statistical results of location errors. Compared with KF, the outputs of RAKF are with higher accuracy and precision. The mean errors of RAKF' outputs decrease 8.8%, 39.7%, 15.2% respectively, and the standard deviations of location errors decrease 10%, 53.2%,17.5%, respectively, compared with that of KF.     Similar heading estimation performances were obtained at the second site. The heading estimation results of KF and RAKF are presented in Figure 10. We still can see that the results of RAKF are smoother than those of KF. The adaptation of RAKF to sudden turns which are marked by black circles in Figure 10a-e is proven again. More intuitive performances can be observed in location tracking, results presented in Figure 11. For all five participants, the RAKF results approximate the reference trace better, compared with the results of KF. The changing of location errors in tracking are further drawn in Figure 12. The performances demonstrate that PDR suffers from location error accumulation, but RAKF is able to handle the problem to some extent. Finally, Figure 13 gives the mean and STD. errors of location tracking for five participants. Compared with KF, the outputs of RAKF are with higher accuracy and precision. The mean errors of RAKF' outputs decrease 14%, 18%, 22%, 26%, and 29%, respectively, and the standard deviations of location errors decrease 9%, 15%, 23%, 7%, and 7.8%, respectively, compared with that of KF.  • Results of the tests in the second site Similar heading estimation performances were obtained at the second site. The heading estimation results of KF and RAKF are presented in Figure 10. We still can see that the results of RAKF are smoother than those of KF. The adaptation of RAKF to sudden turns which are marked by black circles in Figure 10a-e is proven again. More intuitive performances can be observed in location tracking, results presented in Figure 11. For all five participants, the RAKF results approximate the reference trace better, compared with the results of KF. The changing of location errors in tracking are further drawn in Figure 12. The performances demonstrate that PDR suffers from location error accumulation, but RAKF is able to handle the problem to some extent. Finally, Figure 13 gives the mean and STD. errors of location tracking for five participants. Compared with KF, the outputs of RAKF are with higher accuracy and precision. The mean errors of RAKF' outputs decrease 14%, 18%, 22%, 26%, and 29%, respectively, and the standard deviations of location errors decrease 9%, 15%, 23%, 7%, and 7.8%, respectively, compared with that of KF.  We can find from the location tracking results of both test sites that the accuracy levels are different with the same setting of noise covariance matrices. Precisely, the noise covariance matrices used in Kalman filtering should be set specially for each pedestrian. However, this is impractical for wide implementation. The RAKF can alleviate the negative influence of imprecise setting of noise covariance matrices to some extent, but the best location accuracy performance is hard to obtain with constant robust and adaptive parameters. Taking location tracking of participant 3 in the first test site as an example, although the location accuracy provided by the proposed RAKF with the parameters c = 1.5, and c0 = 3 is higher than that of KF, RAKF with other setting of parameters can achieve even better performances. Figure 14 presents comparions on location tracking trajectory and location error distribution using different algorithms. We can find that RAKF with the parameters c = 1.5, and c0 = 8 performs much better than it with the parameters c = 1.5, and c0 = 3. Therefore, in our opinion, an  We can find from the location tracking results of both test sites that the accuracy levels are different with the same setting of noise covariance matrices. Precisely, the noise covariance matrices used in Kalman filtering should be set specially for each pedestrian. However, this is impractical for wide implementation. The RAKF can alleviate the negative influence of imprecise setting of noise covariance matrices to some extent, but the best location accuracy performance is hard to obtain with constant robust and adaptive parameters. Taking location tracking of participant 3 in the first test site as an example, although the location accuracy provided by the proposed RAKF with the parameters c = 1.5, and c0 = 3 is higher than that of KF, RAKF with other setting of parameters can achieve even better performances. Figure 14 presents comparions on location tracking trajectory and location error distribution using different algorithms. We can find that RAKF with the parameters c = 1.5, and c0 = 8 performs much better than it with the parameters c = 1.5, and c0 = 3. Therefore, in our opinion, an We can find from the location tracking results of both test sites that the accuracy levels are different with the same setting of noise covariance matrices. Precisely, the noise covariance matrices used in Kalman filtering should be set specially for each pedestrian. However, this is impractical for wide implementation. The RAKF can alleviate the negative influence of imprecise setting of noise covariance matrices to some extent, but the best location accuracy performance is hard to obtain with constant robust and adaptive parameters. Taking location tracking of participant 3 in the first test site as an example, although the location accuracy provided by the proposed RAKF with the parameters c = 1.5, and c0 = 3 is higher than that of KF, RAKF with other setting of parameters can achieve even better performances. Figure 14 presents comparions on location tracking trajectory and location error distribution using different algorithms. We can find that RAKF with the parameters c = 1.5, and c0 = 8 performs much better than it with the parameters c = 1.5, and c0 = 3. Therefore, in our opinion, an automatic solution for determining the most suitable adaptive and further robust parameters are needed. Generally, the above results demonstrate that KF can provide optimal heading estimations with proper models and the corresponding noise properties. However, for PDR, the statistical properties of pedestrians' movements are changing dynamically and constant noise levels can result in divergent performances both in heading estimation and location tracking. Moreover, measurement outliers also corrupt the performance of PDR. The proposed RAKF can adapt to dynamic conditions, such as sudden turns during pedestrian's walking, and it is robust in that constant parameters are effective for different persons. Moreover, the results also indicate that heading errors are some of the main error sources for location estimation using the PDR approach. It is necessary to obtain accurate headings, whether the PDR method is assisted by external techniques or not.
Finally, we analyzed the computational time of the proposed approach. The KF and RAKF algorithms are implemented using C#, and the corresponding software runs on an 2.7 Hz Intel Core i5 processor. The average runtimes of one iteration of the filtering algorithms are listed in Table 4. The results demonstrate that our proposed RAKF is slightly slower than the KF. Nonetheless, the proposed RAKF improves the accuracy of heading estimation effectively. Generally, the above results demonstrate that KF can provide optimal heading estimations with proper models and the corresponding noise properties. However, for PDR, the statistical properties of pedestrians' movements are changing dynamically and constant noise levels can result in divergent performances both in heading estimation and location tracking. Moreover, measurement outliers also corrupt the performance of PDR. The proposed RAKF can adapt to dynamic conditions, such as sudden turns during pedestrian's walking, and it is robust in that constant parameters are effective for different persons. Moreover, the results also indicate that heading errors are some of the main error sources for location estimation using the PDR approach. It is necessary to obtain accurate headings, whether the PDR method is assisted by external techniques or not.
Finally, we analyzed the computational time of the proposed approach. The KF and RAKF algorithms are implemented using C#, and the corresponding software runs on an 2.7 Hz Intel Core i5 processor. The average runtimes of one iteration of the filtering algorithms are listed in Table 4. The results demonstrate that our proposed RAKF is slightly slower than the KF. Nonetheless, the proposed RAKF improves the accuracy of heading estimation effectively.

Conclusions and Future Work
In this paper, the outputs of smart phone-embedded MEMS sensors, such as accelerometers, magnetometers, and gyroscopes are fused using RAKF for pedestrian heading estimation. To alleviate the negative influence of measurement outliers, an M-estimator-based model is applied for identifying and controlling them. Moreover, a state discrepancy statistic-based adaptive factor is used to reduce the effect of state model disturbances. Experiments under static and dynamic conditions are conducted to verify the superiority of the application of RAKF over KF. In the static tests, RAKF provides faster convergence speed and better accuracy, compared with KF. In the dynamic tests, the headings produced by RAKF and KF are input into a PDR method, respectively. Location tracking performances reveal that the headings estimated by RAKF lead to more accurate location estimations, especially in the situation of sudden turns during pedestrians' walking. The results also tell us that it is necessary to estimate headings accurately, although there are other data or techniques, such as indoor graphs, WiFi positioning for enhancing the performance of PDR.
For PDR, each pedestrian may have special walking characteristics, which can result in a dedicated noise covariance matrix. Thus, the determination of adaptive parameters seems a cumbersome task which needs an automatic process. In the future, we will focus on studying an adaptive solution with the ability to automatically determine parameters. Moreover, tests about different carrying modes of the smart phone will be carried out, and the associating modifications to the filtering will be made.