# Low-Cost MEMS-Based Pedestrian Navigation  Technique for GPS-Denied Areas

Authors: Abdelrahman Ali; Naser El-Sheimy
Journal: Journal of Sensors
 (2013)

---

## Abstract

The progress in the micro electro mechanical system (MEMS) sensors technology in size, cost, weight, and power consumption allows for new research opportunities in the navigation field. Today, most of smartphones, tablets, and other handheld devices are fully packed with the required sensors for any navigation system such as GPS, gyroscope, accelerometer, magnetometer, and pressure sensors. For seamless navigation, the sensors’ signal quality and the sensors availability are major challenges. Heading estimation is a fundamental challenge in the GPS-denied environments; therefore, targeting accurate attitude estimation is considered significant contribution to the overall navigation error. For that end, this research targets an improved pedestrian navigation by developing sensors fusion technique to exploit the gyroscope, magnetometer, and accelerometer data for device attitude estimation in the different environments based on quaternion mechanization. Results indicate that the improvement in the traveled distance and the heading estimations is capable of reducing the overall position error to be less than 15 m in the harsh environments.


---

## Body

## 1. Introduction

Personal navigation requires technologies that are immune to signal obstructions and fading. One of the major challenges is obtaining a good heading solution in different environments and for different user positions without external absolute reference signals. Part of this challenge arises from the complexity and freedom of movement of a typical handheld user where the heading observability considerably degrades in low-speed walking, making this problem even more challenging. However, for short periods, the relative attitude and heading information is quite reliable. Self-contained systems requiring minimal infrastructure, for example, inertial measurement units (IMUs), stand as a viable option, since pedestrian navigation is not only focused on outdoor navigation but also on indoor navigation.Nowadays, most of the smartphones are programmable and equipped with self-contained, low cost, small size, and power-efficient sensors, such as magnetometers, gyroscopes, and accelerometers. Hence, integrating IMUs navigation solution with a magnetometer-based heading can play an important role in pedestrian navigation in all environments. In the current state of the art in MEMS technology, the accuracy of gyroscopes is not good enough for deriving an absolute heading or relative heading over longer durations of time. However, for short periods, the relative attitude information is quite reliable. Magnetometers, on the other hand, provide absolute heading information once calibrated. However, they can easily be disturbed by ferrous objects nearby, making them unreliable for brief intervals. This calls for the investigation of possible sources of heading error in complementary sensors such as a gyroscope and a magnetometer and improving the accuracy of the result based on an improved Kalman filter design.Much research towards the heading estimation for personal positioning applications has been conducted in the recent years. Some approaches use magnetometers exclusively for heading estimation [1] while others integrate it tightly with an IMU [2, 3]. One commercially available personal locator system based on this principle is the Dead Reckoning Module DRM-4000 made by Honeywell [4]. A quaternion-based method to integrate IMU with magnetometer is presented by [5]. Three body angular rates and four quaternion elements were used to express attitude and were selected as the states of the Kalman filter. The method needs to model the angular motion of the body. In [6], a linear system error model based on the Euler angles errors expressing the local frame errors is developed, and the corresponding system observation model is derived. The proposed method does not need to model the system angular motion and also avoids the nonlinear problem which is inherent in the customarily used methods. A similar technique is proposed by [7] where the angular rates were modeled to be a constant. A nonlinear derivative equation for the Euler angle integration kinematics is investigated in [8]. Work in [9, 10] presented an Euler angle error based method to integrate IMU with magnetometer data where three Euler angle errors and three gyroscope biases were used as states for the Kalman filter. The estimated states were used to correct the Euler angles and to compensate gyroscope drifts, respectively. The work at [11] presented a mathematical model for compass deviation by creating an a priori look-up table for heading corrections. A Kalman filtering approach was investigated by [12] to estimate the angular rotation from the input of a magnetometer compass and three gyroscopes. References [13, 14] presented a least squares technique with improvement which is used for the estimation of the compass deviation model. In addition, much research has been conducted to use the 3D magnetometer-based heading for personal navigation applications in the recent years [15].The magnetometer cannot be used as standalone source for heading information in the harsh environments, especially indoor [16]. In addition, it is required to have knowledge about the preexisted magnetic anomalies resulted from some of the man-made infrastructure [17]. Using magnetic field measurements in heading estimation for indoor navigation also has some limitations as the magnetic field signal needs to be strong enough. Also, the mobile navigation device should be away from any source of disturbances to avoid any perturbation effect [18]. Besides that, the magnetic field during the indoor environment is not completely constant due to the presence of the electronic and electrical devices everywhere. To avoid the problem of magnetometer anomaly, arising out of ferrous materials in the vicinity of the magnetometers, a perturbation detection technique is required. In such scenario, the filter works only in the propagation mode without any update for the attitude. Also the gyroscope bias drifts with time and temperature can be compensated by magnetometers. In this paper, a method is presented to obtain seamless attitude information by integrating the heading outputs based on magnetometer, accelerometer, and gyroscope measurements using the Kalman filter (KF).


## 2. Pedestrian Dead Reckoning (PDR)

Pedestrian dead reckoning is a relative means of positioning where the initial position and heading of the user are supposed to be known. The basic concept and components of the proposed PDR algorithm are shown in Figure1. Generally, steps of the user are detected based on the accelerometer data. To get the travelled distance, the total number of steps is multiplied by the step length. With known heading and reference point, the user can be tracked by successive steps count.Figure 1
The main concept of the PDR algorithm.Step detection is a basic step for any PDR algorithm. Usually, the accelerometer signal is used to detect the steps of the person. Once the step is detected, the total number of steps for a pedestrian can be counted. As a result, the total travelled distance can be estimated by multiplying the step length with the total number of steps. Using travelled distance and heading, user can be located during a typical trip. Step detection algorithm can be performed based on different kinds of sensors, that is, not only accelerometers but even gyroscopes and magnetometers. However, our step event detection scheme is based on using the accelerometer signal. The norm of the three accelerometers is used as in (1), where it is possible to clearly identify the steps by observing, for example, the signal over time:
(1)accelnorm=(Fx2+Fy2+Fz2).
Steps are detected as peaks in the resulting norm, where the step is the highest local maximum in the norm acceleration between the current peak and the previous step peak.


## 3. Sensors Performance

The used device in the test, Samsung Galaxy Nexus smartphone, is shown in Figure2 with axes definition.Figure 2
Device axis definition.Besides other sensors, the device is equipped with triad magnetometer (M), triad gyroscope (G), and triad accelerometer (A). The manufactures and the ranges of the main used sensors are listed in Table1.Table 1
Sensors manufacture and ranges.
Sensor
Manufacture
Range
Gyroscope
mpu-3050
±250 to ±2000°/sec
Accelerometer
BMA220
±2, ±4, ±8, ±16 g
Magnetometer
YAS530
±800μT


### 3.1. Gyroscope Drift

Gyroscopes are mainly used to determine device attitude. The output of such sensor is rotational rate, and performing a single integration on the gyroscopes outputs is necessary to obtain a relative change in angle. Due to the integration process which is very sensible to the systematic errors of the gyroscopes, the bias introduces a quadratic error in the velocity and a cubic error in the position [19]. Gyroscopes measurements can generally be described using
(2)Iω=ω+bω+Sω+Nω+ε(ω),
where Iω is the measured angular rate, ω  is the true angular rate, bω is the gyroscope bias, S is the linear scale factor matrix, N is the nonorthogonality matrix, and ε(ω) is the sensor noise. With integration, the gyroscope bias will introduce an angle error in pitch or roll proportional to time; that is, δθ=∫bωdt=bωt; this small angle will cause misalignment of the IMU. Therefore, when projecting the acceleration from for example the gravity vector g, from the body frame to the local-level frame, the acceleration vector will be incorrectly projected due to this misalignment error. This will introduce an error in one of the horizontal acceleration; that is, δa=gsin(δθ)≈gδθ≈gbωt. Consequently, this leads to an error in velocity δv=∫bωgtdt=(1/2)bωgt2  and in position δp=∫δvdt=∫(1/2)bωgt2dt=(1/6)bωgt3. To overcome the problem of error drift, a bias compensation of gyroscopes is required. When the device is in stationary mode, the deterministic bias of the gyroscope can be estimated by calculating the average gyroscope output during that time.


### 3.2. Magnetometer Perturbation

During operation, especially inside buildings, a magnetometer is subject to many external disturbances such as large metal objects [20]. Other objects like steel structures and electromagnetic power lines can affect the solution of the magnetometer. These kinds of disturbance lead to unpredictable performance of the magnetometer which is a major drawback of using magnetic sensors. In the meanwhile, the magnetic field parameters such as strength, horizontal and vertical magnetic field, and change in the inclination angle can be checked to detect the perturbation in magnetometer measurement by comparing to the reference values which can be found at [21]. Figure 3 shows an example for harsh environment as the magnetometer is totally disturbed due to the steel constructions. The figure shows the total magnetic field in a perturbed area during a walking test compared to the reference value which is 570 mGauss for Calgary [21].Magnetic field perturbation.
(a)
The structure at the CCIT 2nd floor
(b)
Total magnetic field


## 3.1. Gyroscope Drift

Gyroscopes are mainly used to determine device attitude. The output of such sensor is rotational rate, and performing a single integration on the gyroscopes outputs is necessary to obtain a relative change in angle. Due to the integration process which is very sensible to the systematic errors of the gyroscopes, the bias introduces a quadratic error in the velocity and a cubic error in the position [19]. Gyroscopes measurements can generally be described using
(2)Iω=ω+bω+Sω+Nω+ε(ω),
where Iω is the measured angular rate, ω  is the true angular rate, bω is the gyroscope bias, S is the linear scale factor matrix, N is the nonorthogonality matrix, and ε(ω) is the sensor noise. With integration, the gyroscope bias will introduce an angle error in pitch or roll proportional to time; that is, δθ=∫bωdt=bωt; this small angle will cause misalignment of the IMU. Therefore, when projecting the acceleration from for example the gravity vector g, from the body frame to the local-level frame, the acceleration vector will be incorrectly projected due to this misalignment error. This will introduce an error in one of the horizontal acceleration; that is, δa=gsin(δθ)≈gδθ≈gbωt. Consequently, this leads to an error in velocity δv=∫bωgtdt=(1/2)bωgt2  and in position δp=∫δvdt=∫(1/2)bωgt2dt=(1/6)bωgt3. To overcome the problem of error drift, a bias compensation of gyroscopes is required. When the device is in stationary mode, the deterministic bias of the gyroscope can be estimated by calculating the average gyroscope output during that time.


## 3.2. Magnetometer Perturbation

During operation, especially inside buildings, a magnetometer is subject to many external disturbances such as large metal objects [20]. Other objects like steel structures and electromagnetic power lines can affect the solution of the magnetometer. These kinds of disturbance lead to unpredictable performance of the magnetometer which is a major drawback of using magnetic sensors. In the meanwhile, the magnetic field parameters such as strength, horizontal and vertical magnetic field, and change in the inclination angle can be checked to detect the perturbation in magnetometer measurement by comparing to the reference values which can be found at [21]. Figure 3 shows an example for harsh environment as the magnetometer is totally disturbed due to the steel constructions. The figure shows the total magnetic field in a perturbed area during a walking test compared to the reference value which is 570 mGauss for Calgary [21].Magnetic field perturbation.
(a)
The structure at the CCIT 2nd floor
(b)
Total magnetic field


## 4. Multisensors Heading Fusion Filter

The attitude of the device is commonly estimated using the inertial sensor. There are three main approaches for attitude representation: DCM, Euler angle, and quaternion. Among the three techniques quaternion algebra is the preferred. However, the estimated attitude from the gyroscope is very noisy leading to unbounded growth in the heading errors. An integration scheme for the gyroscope, accelerometer, and magnetometer data is proposed to estimate the device attitude and the gyroscope bias. The proposed scheme is a quaternion-based KF as shown in Figure4.Figure 4
Flow of the Kalman filter process.In order to use the KF-based estimator for quaternion parameters and gyroscope biases estimation for a device which is carried by a pedestrian, the required model for the states and measurements and their respective system and measurement error models are presented in this section.


### 4.1. Quaternion Mechanization

The relationship between the direct cosine matrix (DCM) and Euler angles is given in (3) with the sequence of azimuth, pitch, and roll (ψθϕ) or (RϕxRθyRψz) [22]:
(3)Clb=[cθcψcθsψ-sθsϕsθcψ-cϕsψsϕsθsψ+cϕcψsϕcθcϕsθcψ+sϕsψcϕsθsψ-sϕcψcϕcθ].
A quaternion is a four-dimensional vector which is defined based on a vector q and a rotation angle. The vector q is given as
(4)q=(q1,q2,q3,q4).
The DCM matrix in terms of quaternion vector components can be obtained from using
(5)Cn(q)=[2q12-1+q222q2q3+2q1q42q2q4-2q1q32q2q3-2q1q42q12-1+q322q3q4+2q1q22q2q4+2q1q32q3q4-2q1q22q12-1+q42],
where Cn  represents the DCM matrix in terms of the quaternion vector.The matrix transforms from the body frame to the local level (navigation) frame. The roll, pitch, and azimuth values can be obtained by using theatan2 math function on the values of the Cn propagated inside the sensors navigation equations
(6)ϕ=tan-1(Cn(2,3)Cn(3,3)),θ=-sin-1(Cn(1,3)),ψ=tan-1(Cn(1,2)Cn(1,1)).


### 4.2. Filter States

Basically, the target of the proposed filter is to estimate the device attitude based on the quaternion technique. Consequently, any improvement in the quaternion estimate leads to improving the estimated attitude values. The implementation of the KF is optimal for linear systems driven by additive white Gaussian noise (AWGN). The state model can be written in the following form:(7)x˙=Fx+Gw,
where x is the state vector, F is the state transition matrix, and Gw represents the covariance matrix of the applied state model. The measurement system can be represented by a linear equation of the following form:
(8)Z=Hx+v,
where Z is the vector of measurement updates, H is the design (observation) matrix that relates the measurements to the state vector, and v is the measurement noise.The nonlinear form of the system model in the absence of the known input can be written as(9)x˙(t)=F(x(t),t)+G(t)W(t),
where F(x(t),t) is now a nonlinear function describing the time evolution of the states. Consider a nominal trajectory,  xnom(t), related to the actual trajectory, x(t), as
(10)δx(t)=x(t)-xnom(t),
where δx(t) is a perturbation from nominal trajectory. performing a Taylor series expansion equation (10) about the nominal trajectory yields
(11)x˙(t)≈F(xnom(t),t)+∂F(x(t),t)∂x(t)|x(t)=xnom(t)δx(t)+G(t)W(t)=x˙nom(t)+Fδx(t)+G(t)W(t),x˙(t)-x˙nom(t)=Fδx(t)+G(t)W(t),δx˙(t)=Fδx(t)+G(t)W(t),
where F is now the dynamic matrix for a system with state vector which consists of the perturbed states, δx. Thus, the main states to be estimated are the errors in the quaternion parameters given by:
(12)δq=[δq1δq2δq3δq4]T,
where δqi is the error in the ith quaternion parameter.The quaternion parameters are primarily determined using the angular rates obtained from gyroscopes’ measurements. The deterministic errors associated with the gyroscope can be compensated using data from static interval at the beginning of the test while the stochastic errors in biases are given by(13)bω=[bωxbωybωz]T,
where bωx, bωy, and bωz are the gyroscope biases.The complete state vector is defined as a 7-dimensional vector with the first four components being errors in the elements of the quaternion and the last three being the elements of the gyroscope biases.(14)x=[δq1δq2δq3δq4bωxbωybωz]T.


### 4.3. The State Transition Model

The angular rate is linked to the quaternion parameters as in the following:(15)q˙=12·q⊗ω=12[-q2-q3-q4q1-q4q3q4q1-q2-q3q2q1][ωxωyωz],
where q is the attitude quaternion ωx, ωy, and ωz represent angular rate measurements in the sensor frame obtained using the rate gyroscopes. Quaternion is used to represent attitude in the filter design because it does not have the singularity problem associated with Euler angles. The previous equation can be rewritten as
(16)[q˙1q˙2q˙3q˙4]=12[-q2ωx-q3ωy-q4ωzq1ωx-q4ωy+q3ωzq4ωx+q1ωy-q2ωz-q3ωx+q2ωy+q1ωz].
The Taylor series expansion to first order is shown in the following:
(17)δr˙=∂r˙∂rδr,
where r is the state vector δr is the error in the state. Thus, the quaternion parameters error can be obtained as
(18)[δq˙1δq˙2δq˙3δq˙4]=12[0-ωx-ωy-ωzωx0ωz-ωyωy-ωz0ωxωzωy-ωx0][δq1δq2δq3δq4].
A general equation for the 1st order Gauss-Markov model is given as
(19)b˙=-βb+2βσ2w(t),
where β is the reciprocal of the correlation time and σ2 is the variance of the gyroscope signal. The different parameters of the Gauss-Markov model can be determined as shown in Figure 5.Gauss-Markov model representation.
(a)
Gauss Markov parameters
(b)
Autocorrelation fit for thex gyroscopeAccording to (19), the gyroscopes bias can be modeled as
(20)b˙ω=[b˙ωxb˙ωyb˙ωz]=[-βx000-βy000-βz][bωxbωybωz]+[2βxσx22βyσy22βzσz2]w.
The complete state model can be written as the following:
(21)[δq˙1δq˙2δq˙3δq˙4b˙ωxb˙ωyb˙ωz]=[Fω04×303×4Fb]·[δq1δq2δq3δq4bωxbωybωz]+[00002βxσx22βyσy22βzσz2]·w,
where
(22)Fω=[0-ωx-ωy-ωzωx0ωz-ωyωy-ωz0ωxωzωy-ωx0],Fb=[-βx000-βy000-βz].
Using the dynamic matrix in (21), the state transition matrix can be defined as
(23)Φk+1,k=e(F·Δt)=I+F·Δt+(F·Δt)22!.


### 4.4. Measurement Models

The magnetometer measurements along with the accelerometer data are used as main source of update. The measured magnetic field is tested for perturbation. Once the magnetic field is free of disturbances, the geomagnetic heading is estimated from the calibrated data. Also, the accelerometer measurements are experienced to be noisy as low cost MEMS sensors are used and usually measured at higher rate. Therefore, the average of the measured data over the step time is used to estimate the roll and pitch values. The roll, pitch, and heading estimates are used to calculate the quaternion parameters.The only source of update is the quaternion parameters. Thus, the design matrix can be set as(24)δz=[δq1δq2δq3δq4]=[1000000010000000100000001000]·[δq1δq2δq3δq4bωxbωybωz].


### 4.5. Modeling of Process and Measurement Noises

In order to complete the design of the KF, it is necessary to define the noise covariance matrices, the process noise covariance matrixQ and the measurement noise covariance matrix R. These matrices reflect the confidence in the system model and the measurements, respectively. The covariance of wk is often called the process noise matrix, Qk, and can be computed as
(25)Qk=E[wk·wkT].
The Qk matrix is a 7-dimension square matrix which can be computed using as follows [23, 24]:
(26)Qk=∫tktk+1Φtk+1,τ·G(τ)·Q(τ)·GT(τ)Φtk+1,τTdτ.The measurement noise covariance matrixR is also known as the covariance matrix for v. The Rk matrix represents the level of confidence placed in the accuracy of the measurements and is given by
(27)Rk=E[vk·vkT].
The Rk matrix is a 4-dimension diagonal square matrix. The diagonal elements are the variances of the individual measurements, which can be determined experimentally using measurement data from the used sensors.


### 4.6. Filter State Initialization

The state vector should be initialized at the beginning of the process. For the gyroscope bias states, all biases are initialized as zeros. The quaternion states can be initialized from the DCM matrix using the Euler angles. The mean of the accelerometer calibrated data during a stationary period can be used to estimate the initial roll and pitch using the following relationships [25]:
(28)ϕo=tan-1(-f-yf-x2+f-z2),θo=tan-1(f-x2+f-y2-f-z),
where f- is the mean of the accelerometer data. During the same interval, the roll and pitch estimates are used for leveling the magnetometer data to be in the navigation frame. The calibrated magnetometer data is used to estimate the initial azimuth as
(29)ψo=tan-1(HyHx).
The DCM is calculated using the initial Euler angles values as in (3). Then, the following relation between the quaternion and the DCM [22] is used to calculate the initial quaternion vector:
(30)q=[cosϕ2cosθ2cosψ2+sinϕ2sinθ2sinψ2sinϕ2cosθ2cosψ2-cosϕ2sinθ2sinψ2cosϕ2sinθ2cosψ2+sinϕ2cosθ2sinψ2cosϕ2cosθ2sinψ2-sinϕ2sinθ2cosψ2].
The initial quaternion vector,  qo, is calculated using the initial Euler angles values (ϕo, θo, ψo). Equation (30) is also used to get the updated quaternion parameters.


## 4.1. Quaternion Mechanization

The relationship between the direct cosine matrix (DCM) and Euler angles is given in (3) with the sequence of azimuth, pitch, and roll (ψθϕ) or (RϕxRθyRψz) [22]:
(3)Clb=[cθcψcθsψ-sθsϕsθcψ-cϕsψsϕsθsψ+cϕcψsϕcθcϕsθcψ+sϕsψcϕsθsψ-sϕcψcϕcθ].
A quaternion is a four-dimensional vector which is defined based on a vector q and a rotation angle. The vector q is given as
(4)q=(q1,q2,q3,q4).
The DCM matrix in terms of quaternion vector components can be obtained from using
(5)Cn(q)=[2q12-1+q222q2q3+2q1q42q2q4-2q1q32q2q3-2q1q42q12-1+q322q3q4+2q1q22q2q4+2q1q32q3q4-2q1q22q12-1+q42],
where Cn  represents the DCM matrix in terms of the quaternion vector.The matrix transforms from the body frame to the local level (navigation) frame. The roll, pitch, and azimuth values can be obtained by using theatan2 math function on the values of the Cn propagated inside the sensors navigation equations
(6)ϕ=tan-1(Cn(2,3)Cn(3,3)),θ=-sin-1(Cn(1,3)),ψ=tan-1(Cn(1,2)Cn(1,1)).


## 4.2. Filter States

Basically, the target of the proposed filter is to estimate the device attitude based on the quaternion technique. Consequently, any improvement in the quaternion estimate leads to improving the estimated attitude values. The implementation of the KF is optimal for linear systems driven by additive white Gaussian noise (AWGN). The state model can be written in the following form:(7)x˙=Fx+Gw,
where x is the state vector, F is the state transition matrix, and Gw represents the covariance matrix of the applied state model. The measurement system can be represented by a linear equation of the following form:
(8)Z=Hx+v,
where Z is the vector of measurement updates, H is the design (observation) matrix that relates the measurements to the state vector, and v is the measurement noise.The nonlinear form of the system model in the absence of the known input can be written as(9)x˙(t)=F(x(t),t)+G(t)W(t),
where F(x(t),t) is now a nonlinear function describing the time evolution of the states. Consider a nominal trajectory,  xnom(t), related to the actual trajectory, x(t), as
(10)δx(t)=x(t)-xnom(t),
where δx(t) is a perturbation from nominal trajectory. performing a Taylor series expansion equation (10) about the nominal trajectory yields
(11)x˙(t)≈F(xnom(t),t)+∂F(x(t),t)∂x(t)|x(t)=xnom(t)δx(t)+G(t)W(t)=x˙nom(t)+Fδx(t)+G(t)W(t),x˙(t)-x˙nom(t)=Fδx(t)+G(t)W(t),δx˙(t)=Fδx(t)+G(t)W(t),
where F is now the dynamic matrix for a system with state vector which consists of the perturbed states, δx. Thus, the main states to be estimated are the errors in the quaternion parameters given by:
(12)δq=[δq1δq2δq3δq4]T,
where δqi is the error in the ith quaternion parameter.The quaternion parameters are primarily determined using the angular rates obtained from gyroscopes’ measurements. The deterministic errors associated with the gyroscope can be compensated using data from static interval at the beginning of the test while the stochastic errors in biases are given by(13)bω=[bωxbωybωz]T,
where bωx, bωy, and bωz are the gyroscope biases.The complete state vector is defined as a 7-dimensional vector with the first four components being errors in the elements of the quaternion and the last three being the elements of the gyroscope biases.(14)x=[δq1δq2δq3δq4bωxbωybωz]T.


## 4.3. The State Transition Model

The angular rate is linked to the quaternion parameters as in the following:(15)q˙=12·q⊗ω=12[-q2-q3-q4q1-q4q3q4q1-q2-q3q2q1][ωxωyωz],
where q is the attitude quaternion ωx, ωy, and ωz represent angular rate measurements in the sensor frame obtained using the rate gyroscopes. Quaternion is used to represent attitude in the filter design because it does not have the singularity problem associated with Euler angles. The previous equation can be rewritten as
(16)[q˙1q˙2q˙3q˙4]=12[-q2ωx-q3ωy-q4ωzq1ωx-q4ωy+q3ωzq4ωx+q1ωy-q2ωz-q3ωx+q2ωy+q1ωz].
The Taylor series expansion to first order is shown in the following:
(17)δr˙=∂r˙∂rδr,
where r is the state vector δr is the error in the state. Thus, the quaternion parameters error can be obtained as
(18)[δq˙1δq˙2δq˙3δq˙4]=12[0-ωx-ωy-ωzωx0ωz-ωyωy-ωz0ωxωzωy-ωx0][δq1δq2δq3δq4].
A general equation for the 1st order Gauss-Markov model is given as
(19)b˙=-βb+2βσ2w(t),
where β is the reciprocal of the correlation time and σ2 is the variance of the gyroscope signal. The different parameters of the Gauss-Markov model can be determined as shown in Figure 5.Gauss-Markov model representation.
(a)
Gauss Markov parameters
(b)
Autocorrelation fit for thex gyroscopeAccording to (19), the gyroscopes bias can be modeled as
(20)b˙ω=[b˙ωxb˙ωyb˙ωz]=[-βx000-βy000-βz][bωxbωybωz]+[2βxσx22βyσy22βzσz2]w.
The complete state model can be written as the following:
(21)[δq˙1δq˙2δq˙3δq˙4b˙ωxb˙ωyb˙ωz]=[Fω04×303×4Fb]·[δq1δq2δq3δq4bωxbωybωz]+[00002βxσx22βyσy22βzσz2]·w,
where
(22)Fω=[0-ωx-ωy-ωzωx0ωz-ωyωy-ωz0ωxωzωy-ωx0],Fb=[-βx000-βy000-βz].
Using the dynamic matrix in (21), the state transition matrix can be defined as
(23)Φk+1,k=e(F·Δt)=I+F·Δt+(F·Δt)22!.


## 4.4. Measurement Models

The magnetometer measurements along with the accelerometer data are used as main source of update. The measured magnetic field is tested for perturbation. Once the magnetic field is free of disturbances, the geomagnetic heading is estimated from the calibrated data. Also, the accelerometer measurements are experienced to be noisy as low cost MEMS sensors are used and usually measured at higher rate. Therefore, the average of the measured data over the step time is used to estimate the roll and pitch values. The roll, pitch, and heading estimates are used to calculate the quaternion parameters.The only source of update is the quaternion parameters. Thus, the design matrix can be set as(24)δz=[δq1δq2δq3δq4]=[1000000010000000100000001000]·[δq1δq2δq3δq4bωxbωybωz].


## 4.5. Modeling of Process and Measurement Noises

In order to complete the design of the KF, it is necessary to define the noise covariance matrices, the process noise covariance matrixQ and the measurement noise covariance matrix R. These matrices reflect the confidence in the system model and the measurements, respectively. The covariance of wk is often called the process noise matrix, Qk, and can be computed as
(25)Qk=E[wk·wkT].
The Qk matrix is a 7-dimension square matrix which can be computed using as follows [23, 24]:
(26)Qk=∫tktk+1Φtk+1,τ·G(τ)·Q(τ)·GT(τ)Φtk+1,τTdτ.The measurement noise covariance matrixR is also known as the covariance matrix for v. The Rk matrix represents the level of confidence placed in the accuracy of the measurements and is given by
(27)Rk=E[vk·vkT].
The Rk matrix is a 4-dimension diagonal square matrix. The diagonal elements are the variances of the individual measurements, which can be determined experimentally using measurement data from the used sensors.


## 4.6. Filter State Initialization

The state vector should be initialized at the beginning of the process. For the gyroscope bias states, all biases are initialized as zeros. The quaternion states can be initialized from the DCM matrix using the Euler angles. The mean of the accelerometer calibrated data during a stationary period can be used to estimate the initial roll and pitch using the following relationships [25]:
(28)ϕo=tan-1(-f-yf-x2+f-z2),θo=tan-1(f-x2+f-y2-f-z),
where f- is the mean of the accelerometer data. During the same interval, the roll and pitch estimates are used for leveling the magnetometer data to be in the navigation frame. The calibrated magnetometer data is used to estimate the initial azimuth as
(29)ψo=tan-1(HyHx).
The DCM is calculated using the initial Euler angles values as in (3). Then, the following relation between the quaternion and the DCM [22] is used to calculate the initial quaternion vector:
(30)q=[cosϕ2cosθ2cosψ2+sinϕ2sinθ2sinψ2sinϕ2cosθ2cosψ2-cosϕ2sinθ2sinψ2cosϕ2sinθ2cosψ2+sinϕ2cosθ2sinψ2cosϕ2cosθ2sinψ2-sinϕ2sinθ2cosψ2].
The initial quaternion vector,  qo, is calculated using the initial Euler angles values (ϕo, θo, ψo). Equation (30) is also used to get the updated quaternion parameters.


## 5. Results and Discussion

In this section, the performance of the proposed attitude algorithm is assessed. The device is held in the texting mode. All tests start with around 30 seconds of stationary period to calibrate for the gyroscope while the magnetometer is calibrated by moving the device in the 3D space. The first scenario is conducted to assess the performance of the proposed technique in the indoor environments while the second scenario is performed in the downtown area.


### 5.1. Environment Change Test

In this scenario, the test is started outside the building close to the Olympic Oval at the University of Calgary. The PDR solution is initialized using initial position from GPS and initial heading from the magnetometer. The device is held in stationary at the beginning of the test for about 40 s to calibrate for the gyroscope deterministic bias and to estimate the initial orientation, roll and pitch, of the device. The device is held in the compass, texting mode, and the user keeps going through the first floor with a long corridor inside the building. The trajectory is ended outside the McEwan Student Center in front of Taylor Family Digital Library (TDFL). The time for this trajectory was about 5.5 minutes taken in 594 steps with a total travelled distance of 461 m. Various reasons made this place a candidate for the test.(i)
It is a popular and attractive place for students’ activities.(ii)
It has a long corridor which is a challenging area for magnetometer.(iii)
At the starting point, in front of the building, there is a huge metal structure which can affect the magnetometer performance.(iv)
This place is full of students and simulates the normal walking scenario of a smartphone user.The derived heading form the magnetometer is totally affected at the beginning due to the presence of a huge metal object in the surrounding area as shown in Figure 6. The total magnetic field is distorted yielding incorrect heading estimation from magnetometer during the first 40 seconds. In addition, the calibration process is performed based on data taken outside the building, and so, once the user is moved to be inside the building, the distribution of the magnetic field is different from that outside as shown in Figure 6. Thus, magnetometer-based heading is still not accurate. However, after around 2 minutes the heading from the magnetometer starts to be the main source of update for the attitude filter. As a result, during the perturbation interval the attitude filter does not perform the update stage and keeps propagating the attitude of the device based on the gyroscopes’ measurements.Heading estimation and the effect of the surrounding environment.
(a)
Area at the starting point
(b)
Heading estimateThe magnified part in Figure7 shows the magnetometer heading during a perturbation area. The figure shows that the heading is diverted and scattered when the magnetic field is distorted in contrast to the attitude filter heading during the same interval.Figure 7
Magnetometer heading direction during perturbation area.To evaluate the overall accuracy of the PDR algorithm, a reference trajectory plotted on the map of the test site is used. A reference trajectory for the actual direction of the test is plotted on the map of the first floor of the University of Calgary. As shown in Figure8, the maximum error is less than 5 m during the test. Also, the trajectory is finished at the correct point with position drift of less than 4 m. The error in the distance is about 1.1% of the total traveled distance.Figure 8
PDR trajectory compared to a reference trajectory.


### 5.2. Downtown Test

Downtown is an attractive place for tourists in addition to most people in the city. It has the major attractive sites such as shopping centers, administration offices, museums, restaurants, cafes, and theaters. So, it is important to have a good navigation system which is able to help the pedestrians to their destinations. Some conventional techniques such as GPS suffer from the multipath and signal destruction due to the high buildings. The test is conducted in downtown Calgary to assess the performance of the proposed algorithm in the harsh environments. The performance of the magnetometer is totally affected due to the distortion of the magnetic field in the downtown area. The high buildings, cars, and traffic signals add more complications for the heading estimation based on the magnetometer. The test is started at the intersection of the 7th street SW and the 8th avenue SW downtown Calgary as shown in Figure9. The selected trajectory is a square starting in the east direction followed by north, west, and south directions. The length of the trajectory is 490 m taking about 6 minutes of walking.Figure 9
Test starting point.Although the magnetometer gets good maneuvering at the start of the test the presence of a strong perturbation source affected the heading estimate. As shown in Figure10, the estimated heading from magnetometer is totally unused for major part of the test due to the distortion in the magnetic field. It shows that the heading update is available only for two minutes during the interval of 140 s to 260 s.Figure 10
Heading estimation.Figure11 shows the PDR solution using the attitude filter compared to the same solution based on the heading from the magnetometer stand-alone, gyroscope heading stand-alone, and GPS solution. As shown in the figure, there is no accurate stand-alone solution for the test trajectory. (i)
The gyroscope stand-alone solution gives the accurate direction with position drift due to the uncompensated bias.(ii)
The magnetometer stand-alone solution is diverted at many parts of the test; however, in certain parts it performs well and provides the correct heading.(iii)
The GPS is considered the poorest solution as it does not provide any correct information at all during the test due to the satellites unavailability.However, the PDR algorithm based on the attitude filter, in corporation with the magnetometer anomaly detection technique, provides an acceptable solution.Figure 11
PDR trajectory compared to a reference trajectory.The overall accuracy of the PDR algorithm is evaluated by comparing the PDR solution with a reference trajectory on Google map. As shown in Figure11, the maximum error in position happens at the north direction side of the trajectory where no update is available yet, and it was around 10 m. Also, the trajectory is finished at the correct place with position drift of less than 4 m. The overall error in distance is about 1% of the total traveled distance.


## 5.1. Environment Change Test

In this scenario, the test is started outside the building close to the Olympic Oval at the University of Calgary. The PDR solution is initialized using initial position from GPS and initial heading from the magnetometer. The device is held in stationary at the beginning of the test for about 40 s to calibrate for the gyroscope deterministic bias and to estimate the initial orientation, roll and pitch, of the device. The device is held in the compass, texting mode, and the user keeps going through the first floor with a long corridor inside the building. The trajectory is ended outside the McEwan Student Center in front of Taylor Family Digital Library (TDFL). The time for this trajectory was about 5.5 minutes taken in 594 steps with a total travelled distance of 461 m. Various reasons made this place a candidate for the test.(i)
It is a popular and attractive place for students’ activities.(ii)
It has a long corridor which is a challenging area for magnetometer.(iii)
At the starting point, in front of the building, there is a huge metal structure which can affect the magnetometer performance.(iv)
This place is full of students and simulates the normal walking scenario of a smartphone user.The derived heading form the magnetometer is totally affected at the beginning due to the presence of a huge metal object in the surrounding area as shown in Figure 6. The total magnetic field is distorted yielding incorrect heading estimation from magnetometer during the first 40 seconds. In addition, the calibration process is performed based on data taken outside the building, and so, once the user is moved to be inside the building, the distribution of the magnetic field is different from that outside as shown in Figure 6. Thus, magnetometer-based heading is still not accurate. However, after around 2 minutes the heading from the magnetometer starts to be the main source of update for the attitude filter. As a result, during the perturbation interval the attitude filter does not perform the update stage and keeps propagating the attitude of the device based on the gyroscopes’ measurements.Heading estimation and the effect of the surrounding environment.
(a)
Area at the starting point
(b)
Heading estimateThe magnified part in Figure7 shows the magnetometer heading during a perturbation area. The figure shows that the heading is diverted and scattered when the magnetic field is distorted in contrast to the attitude filter heading during the same interval.Figure 7
Magnetometer heading direction during perturbation area.To evaluate the overall accuracy of the PDR algorithm, a reference trajectory plotted on the map of the test site is used. A reference trajectory for the actual direction of the test is plotted on the map of the first floor of the University of Calgary. As shown in Figure8, the maximum error is less than 5 m during the test. Also, the trajectory is finished at the correct point with position drift of less than 4 m. The error in the distance is about 1.1% of the total traveled distance.Figure 8
PDR trajectory compared to a reference trajectory.


## 5.2. Downtown Test

Downtown is an attractive place for tourists in addition to most people in the city. It has the major attractive sites such as shopping centers, administration offices, museums, restaurants, cafes, and theaters. So, it is important to have a good navigation system which is able to help the pedestrians to their destinations. Some conventional techniques such as GPS suffer from the multipath and signal destruction due to the high buildings. The test is conducted in downtown Calgary to assess the performance of the proposed algorithm in the harsh environments. The performance of the magnetometer is totally affected due to the distortion of the magnetic field in the downtown area. The high buildings, cars, and traffic signals add more complications for the heading estimation based on the magnetometer. The test is started at the intersection of the 7th street SW and the 8th avenue SW downtown Calgary as shown in Figure9. The selected trajectory is a square starting in the east direction followed by north, west, and south directions. The length of the trajectory is 490 m taking about 6 minutes of walking.Figure 9
Test starting point.Although the magnetometer gets good maneuvering at the start of the test the presence of a strong perturbation source affected the heading estimate. As shown in Figure10, the estimated heading from magnetometer is totally unused for major part of the test due to the distortion in the magnetic field. It shows that the heading update is available only for two minutes during the interval of 140 s to 260 s.Figure 10
Heading estimation.Figure11 shows the PDR solution using the attitude filter compared to the same solution based on the heading from the magnetometer stand-alone, gyroscope heading stand-alone, and GPS solution. As shown in the figure, there is no accurate stand-alone solution for the test trajectory. (i)
The gyroscope stand-alone solution gives the accurate direction with position drift due to the uncompensated bias.(ii)
The magnetometer stand-alone solution is diverted at many parts of the test; however, in certain parts it performs well and provides the correct heading.(iii)
The GPS is considered the poorest solution as it does not provide any correct information at all during the test due to the satellites unavailability.However, the PDR algorithm based on the attitude filter, in corporation with the magnetometer anomaly detection technique, provides an acceptable solution.Figure 11
PDR trajectory compared to a reference trajectory.The overall accuracy of the PDR algorithm is evaluated by comparing the PDR solution with a reference trajectory on Google map. As shown in Figure11, the maximum error in position happens at the north direction side of the trajectory where no update is available yet, and it was around 10 m. Also, the trajectory is finished at the correct place with position drift of less than 4 m. The overall error in distance is about 1% of the total traveled distance.


## 6. Conclusion

An enhanced attitude estimation technique based on two different attitude sensors is presented. The algorithm is based on the quaternion mechanization to estimate the device attitude. The filter is working as a complementary technique; the Earth’s magnetic field and the angle rate are integrated to estimate the device heading. The filter is propagated in the prediction mode using the gyroscope measurements while the magnetometer and accelerometer measurements are used for the update stage. The improvement in the attitude estimation leads to an improved PDR result. The PDR-based estimated trajectory results are compared to reference trajectories to show the accuracy of the proposed technique. The results show that the presented algorithm is able to provide the necessary navigation information accurately even in the harsh environments.



---

*Source: 197090-2013-08-22.xml*