Magnetic, Acceleration Fields and Gyroscope Quaternion (MAGYQ)-Based Attitude Estimation with Smartphone Sensors for Indoor Pedestrian Navigation

Abstract

The dependence of proposed pedestrian navigation solutions on a dedicated infrastructure is a limiting factor to the deployment of location based services. Consequently self-contained Pedestrian Dead-Reckoning (PDR) approaches are gaining interest for autonomous navigation. Even if the quality of low cost inertial sensors and magnetometers has strongly improved, processing noisy sensor signals combined with high hand dynamics remains a challenge. Estimating accurate attitude angles for achieving long term positioning accuracy is targeted in this work. A new Magnetic, Acceleration fields and GYroscope Quaternion (MAGYQ)-based attitude angles estimation filter is proposed and demonstrated with handheld sensors. It benefits from a gyroscope signal modelling in the quaternion set and two new opportunistic updates: magnetic angular rate update (MARU) and acceleration gradient update (AGU). MAGYQ filter performances are assessed indoors, outdoors, with dynamic and static motion conditions. The heading error, using only the inertial solution, is found to be less than 10° after 1.5 km walking. The performance is also evaluated in the positioning domain with trajectories computed following a PDR strategy.

1. Introduction

The advent of new wearable devices has widened the application field of pedestrian navigation systems and methods. Among these devices, handheld units remain of prime interest for Location Based Services (LBS) [1]. Existing technologies are still lacking in terms of positioning and navigation performance. Either they depend on a dedicated infrastructure, which is not continuously available during pedestrian journeys introducing coverage gaps, or the accuracy of the position estimate is not sufficient for locating the user on the correct street location (sidewalk, stairway, etc.). The future of pedestrian navigation solutions certainly relies on combining all existing technologies depending on the user's context. Finding the appropriate criteria for shaping the hybridization filter remains a great challenge.

One option consists in developing self-contained sensor navigation solutions. Still today, the quality of inertial sensors embedded in smartphone devices is too low for solving all free-inertial pedestrian navigation issues. This statement is true irrespective of the adopted processing strategy [2]: inertial strapdown navigation (Figure 1) or pedestrian dead-reckoning (Figure 2). However the latest performance improvements of micro-electro-mechanical sensors (MEMS) combined with novel algorithms for mitigating the sensor errors using opportunistic signals are pushing the boundaries of existing inertial pedestrian navigation solutions.

Whether using a strapdown inertial navigation method or a pedestrian dead-reckoning one, the attitude angles, including the heading, are estimated by integrating the angular rates sensed by a gyroscope. With this approach, the gyroscope errors are propagated at a time cubic rate. With low cost handheld units, the options for mitigating the sensors drift and noise are limited. Indeed, as compared to what would have been applied with foot mounted systems during the stance phases of the walking gait, no zero angular rate or zero velocity update can be applied. With respect to magnetic fields, no geomagnetic heading update can be easily and frequently applied in urban and indoor spaces because the earth magnetic field is strongly perturbed by surrounding artificial fields [3].

Another source of error comes from the parameterization of the rotation between the mobile unit and the navigation frame. It is known that Euler parameterization in attitude angles estimation filter introduces gimbal lock problems. Because the hand performs fast motions in the entire 3D space without privileging any direction, frequent occurrence of this issue is observed. With free inertial pedestrian navigation based on handheld device comes another problem: the time varying relative orientation between the reference frame of the handheld device and the one related to the pedestrian's body. Existing methods [4,5] propose to solve this issue using the accelerations in the navigation frame, which reinforces the critical role of accurate attitude angles estimation in the overall PDR accuracy budget. All these factors highlight the critical role of heading estimation in the overall performance of inertial pedestrian navigation solutions with handheld smartphone. It definitively motivates the research work presented in this article.

In this paper, a novel Magnetic, Acceleration fields and GYroscope Quaternion (MAGYQ)-based attitude and heading estimation filter is proposed and demonstrated with handheld sensors. First, the filter takes advantages of the four-dimensional quaternion algebra for reducing the errors introduced by the mathematical representation of rotations and uses a quaternion based state vector. Second, it exploits specific states of the measured magnetic field and acceleration vector combined with some hand motions of opportunity for observing the attitude angles and mitigating the sensor errors.

Section 2 starts with a state-of-the-art on existing attitude estimation solutions and continues with the innovations that are proposed in this article. Theoretical aspects of the novel quaternion error-based attitude heading estimation method including a new angular rate signal model are detailed in Section 3. The algorithms involved in the attitude angle estimation filter MAGYQ are detailed in Sections 4–6. MAGYQ headings are combined with step lengths that are computed using a model recalled in Section 7. Section 8 is dedicated to the experimental assessment. A performance analysis of the attitude angles estimation is first conducted and followed by an evaluation in the positioning domain.

2. State of the Art

2.1. Existing Attitude Estimation Approaches

Existing attitude and heading estimation filters are principally based on the use of Direct Cosine Matrix (DCM) parameterized with Euler angles. It describes the transformation between coordinate frames that are linked to the handheld unit: the body frame and a local navigation frame. Tri-axis gyroscope, tri-axis accelerometer and tri-axis magnetometer are the most common sensors used for estimating this rotation.

2.1.1. Euler Angles with Direct Cosine Matrix

Classically small rotation errors are tracked using Euler parameterization for estimating the attitude rotation matrix [6]. The inherited algorithms use Euler angles to describe the three dimensional rotation and the three successive rotations (yaw ψ, pitch θ, and roll φ). The DCM ${\mathbf{\text{C}}}_b^n$ is parameterized using Euler angles and defines the rotation between the body frame and the navigation (or local) frame. Unfortunately, DCM parameterized with Euler angles presents singularities, which are known as the “Gimbal lock” problem. If the pitch angle equals π/2, several pairs of yaw and roll angles can achieve the same rotation. Furthermore this parameterization is highly non-linear since the DCM is composed of sums of products of sine and cosine functions. Therefore proposing an attitude angles estimator that remains in the quaternion set is of interest to overcome these limitations.

2.1.2. Coupling of Magnetic Field and Inertial Measurements

Many filters have been proposed for improving the attitude estimation using magnetic field measurement. They are based on hybridization techniques that use either a priori knowledge of the local magnetic field or at least its properties. Irrespective of the chosen approach, they all require a magnetometer calibration for removing the magnetic field induced by the platform [7] in order to measure only the surrounding magnetic field. Four different main magnetic field based methods, which have been proposed in the literature, are now recalled:

• Searching for the geomagnetic earth field

• Fingerprinting with magnetic anomalies

• Sensing the velocity with a magnetometer array

• Magnetic angular rate update with DCM

Let Bⁿ be the known local magnetic field in the navigation frame. This field is related to the magnetic field B^b expressed in the body frame by the DCM with:

$${\mathbf{B}}^n={\mathbf{C}}_b^n{\mathbf{B}}^b$$

(1)

Searching for the Geomagnetic Earth Field

The first method tries to remove all artificial sources of magnetic field that are perturbing the known Earth magnetic field [8]. This approach involves geomagnetic field detectors that often fail to correctly discriminate magnetic perturbations from the geomagnetic field. This is especially the case in indoor spaces, where many artificial magnetic field sources modify the Earth field [3]. This method is often combined with pitch and roll angles estimation using accelerations that are measured during static phase of the inertial mobile unit (IMU). During these periods, the accelerometer measures the Earth gravity directly. Combining the measurement of these two fields, it becomes possible to rebuild the complete orientation following for example the QUEST [9] or MARG [10,11] algorithms.

Fingerprinting with Magnetic Anomalies

A more recent method, which is growing in popularity, consists in navigating using only measured magnetic fields. At first, the indoor (even outdoor) magnetic fields are mapped during a calibration phase [12,13]. Following Wi-Fi receiver signal strength based fingerprinting techniques, the user's location is extracted from the database of magnetic anomalies. This method benefits from the singularities in the local magnetic field anomalies, but it is non-ergonomic and laborious. Up to now, its limitations have hardly been investigated. One limitation among others is the influence of soft-iron effects in a crowded environment, which has not yet been assessed.

Sensing the Velocity with a Magnetometer Array

Another approach, frequently applied to robotics, uses array of magnetometers for measuring the magnetic field gradient and deducting the velocity vector [14]. No a priori knowledge about the local magnetic field is required for this approach where the magnetic field space gradient (ΔB^b/Δx) and the derivate of the position in the body frame (Δx/Δt) are related to the magnetic field temporal derivate (ΔB^b/Δt) by:

$$\frac{\mathrm{\Delta }{\mathbf{B}}^b}{\mathrm{\Delta }t}=\frac{\mathrm{\Delta }\mathbf{x}}{\mathrm{\Delta }t}\frac{\mathrm{\Delta }{\mathbf{B}}^b}{\mathrm{\Delta }\mathbf{x}}$$

(2)

The biggest challenge of this method comes from the individual calibration of all magnetometers since the precision of the measured gradient directly impacts the estimated velocity. This estimation becomes really difficult with varying fields on the hosting platform. Finally, the distance between each magnetometer strongly impacts the good approximation of the space gradient, which might be a limiting factor for handheld sensors.

Magnetic Angular Rate Update with DCM

The last technique exploits Quasi-Static magnetic Field (QSF) in the navigation frame for mitigating gyroscope error. This approach works even if the sensed field differs from the earth geomagnetic field [15]. An important feature of this method is its immunity to local magnetic field disturbances, as long as they are globally constant in the local frame. If this hypothesis holds, the magnetic field temporal gradient gives a direct observation of the body frame angular rate (ω):

$$\frac{d{\mathbf{B}}^b}{dt}=-\mathbf{\omega }\wedge {\mathbf{B}}^b$$

(3)

∧ is the cross product of two vectors. The novel MAGYQ filter proposed in this paper is based on this approach. Let us note that unlike previous magnetometer array-based algorithms, it uses only one tri-axis magnetometer.

2.2. Innovation of the Proposed Method

The first innovation is that the Magnetic, Acceleration fields and GYroscope Quaternion (MAGYQ)-based attitude and heading estimation filter uses quaternions to parameterize the state vector and the angular rates measurements. Instead of using Euler angle errors or its quaternion form, MAGYQ filter estimates an additive quaternion error. A new gyroscope signal model in the quaternion set is also proposed and a gyroscope quaternion bias is introduced. The filtering strategy is based on an Extended Kalman Filter (EKF) whose aim is to estimate the attitude of the body frame of a handheld device with respect to a local frame. In Section 3, this novel approach of angular rate modelling in the quaternion set is detailed and justified for the Kalman Filter underlying hypothesis.

The second innovation comes from the update steps. Observation equations are proposed using the magnetic field angular rate (MARU) in the quaternion set and using the acceleration gradient update (AGU). The benefit of MARU is that it can frequently be applied for bounding the gyroscope errors even in indoor spaces where the Earth magnetic field is disturbed. The accelerometer gradient update reduces the error propagation and estimates the accelerometer bias components. In the literature, the Earth gravity field is used as a local reference field during static periods. It is here proposed to go beyond this approach. Similarly to MARU, the rotation of the IMU is observable using the accelerometer signal gradient if no lever arm between the triads of gyroscopes and the accelerometers exists. AGU takes advantage of opportune hand motions for mitigating the accelerometer errors but also observing the gyroscope bias.

3. Gyroscope Quaternion Modelling

The proposed attitude estimation algorithm is based on angular rates and accelerations sensed with a handheld low cost inertial mobile unit. Contrary to existing solutions, however, the gyroscope error modeling is not performed in the signal domain but in the quaternion set. This approach should improve the accuracy of the estimated angles thanks to reduced linearization steps and ambiguity issues. Prior to presenting the quaternion based error modeling, main properties of the quaternion set used for expressing the rotations are recalled.

3.1. Quaternion Algebra

This part recalls some quaternion properties and the different notations used in this paper. Quaternions are used as parameters for estimating the orientation of a rigid body [9,16].

3.1.1. General Content

The set of quaternion real numbers, noted ℍ in tribute to the mathematician William Rowan Hamilton, is a fourth dimensional non-commutative algebra including specific composition laws [17]. An element q ∈ ℍ is defined with a real-quadruplet (q₁,q₂,q₃,q₄). Each quaternion q can be expressed as a unique pair (q₁,u_q) in which q₁ is the scalar part and ${{\mathbf{u}}_{\mathbf{\text{q}}}}^T=\left(\begin{array}{ccc}{q}_2& q_3& q_4\end{array}\right)$ is the vector or imaginary part. In this paper, the quaternion multiplication is noted ⊗. In the scalar-vector form, the product of two quaternions x and y, noted x ⊗ y, is:

$$\left(\begin{array}{c}{x}_1\\ {\mathbf{u}}_{\mathbf{x}}\end{array}\right)\otimes \left(\begin{array}{c}{y}_1\\ {\mathbf{u}}_{\mathbf{y}}\end{array}\right)=\left(\begin{array}{c}{x}_1{y}_1-\langle {\mathbf{u}}_{\mathbf{x}},{\mathbf{u}}_{\mathbf{y}}\rangle \\ x_1{\mathbf{u}}_{\mathbf{y}}+y_1{\mathbf{u}}_{\mathbf{x}}+{\mathbf{u}}_{\mathbf{x}}\wedge {\mathbf{u}}_{\mathbf{y}}\end{array}\right)$$

(4)

with ̂ the cross product and <,> the classic scalar product in ℝ³. A second form to represent the quaternion multiplication is to use the matrix product:

$$\begin{array}{cc}\begin{array}{ll}\mathbf{\text{x}}\otimes \mathbf{\text{y}}\hfill & =\mathbf{\text{M}}\mathbf{\left(}\mathbf{\text{x}}\mathbf{\right)}\mathbf{\text{y}}\hfill \\ \hfill & =\left(\begin{array}{cccc}{\text{x}}_1& -{\text{x}}_2& -x_3& -x_4\\ x_2& x_1& -x_4& x_3\\ x_3& x_4& x_1& -x_2\\ x_4& -x_3& x_2& x_1\end{array}\right){\left(\begin{array}{c}{\text{y}}_1\\ {\text{y}}_2\\ y_3\\ y_4\end{array}\right)}_{\text{or}}\hfill \end{array}& \begin{array}{ll}\mathbf{x}\otimes \mathbf{y}\hfill & =\mathbf{\text{C}}\mathbf{\left(}\mathbf{y}\mathbf{\right)}\mathbf{x}\hfill \\ \hfill & =\left(\begin{array}{cccc}{y}_1& -y_2& -y_3& -y_4\\ y_2& y_1& y_4& -y_3\\ y_3& -y_4& y_1& y_2\\ y_4& y_3& -y_2& y_1\end{array}\right)\left(\begin{array}{c}{x}_1\\ x_2\\ x_3\\ x_4\end{array}\right)\hfill \end{array}\end{array}$$

(5)

Other properties such as conjugation and norm are defined in the quaternion set. For each quaternion q, there exists a unique conjugate noted q̅ and a norm ‖q‖ defined by:

$$\{\begin{array}{l}\overline{\mathbf{q}}=\left(\begin{array}{c}{q}_1\\ -{\mathbf{u}}_{\mathbf{q}}\end{array}\right)\\ \Vert \mathbf{q}\Vert =\sqrt{{q_1}^2+{q_2}^2+{q_3}^2+{q_4}^2}=\sqrt{{q_1}^2+{\Vert {\mathbf{u}}_{\mathbf{q}}\Vert }^2}\end{array}$$

(6)

For a vector x ∈ ℝ³, a unique quaternion form is defined and links the quaternion set to the three dimensional space by:

$${\left(\mathbf{x}\right)}_{\mathbf{q}}=\left(\begin{array}{c}0\\ \mathbf{x}\end{array}\right)$$

(7)

3.1.2. Quaternion and Rotation

A unit quaternion q can be rewritten with a pair (θ, u) with u a unit vector and θ∈]− π, π]:

$$\mathbf{q}=\left(\begin{array}{c}cos\mathrm{\theta }\\ sin\mathrm{\theta }\mathbf{u}\end{array}\right)$$

(8)

The quaternion defined in Equation (8) describes a rotation [18] in the three dimensional space. u is the rotation axis and θ the rotation angle. By specifying the direction axes, there exists a unique unit quaternion noted ${\mathbf{q}}_a^b$ that represents the rotation from frame a to frame b. The vector x expressed in frame a, can be computed in frame b with:

$${({\mathbf{x}}^b)}_{\mathbf{q}}={\mathbf{q}}_a^b\otimes {\left({\mathbf{x}}^a\right)}_{\mathbf{q}}\otimes {\overline{\mathbf{q}}}_a^b$$

(9)

Similarly to the rotational matrix ${\mathbf{\text{C}}}_a^b$, expressed with Euler angles roll-pitch-yaw (φ,θ,ψ) sequences, quaternion offers another parameterization of a rotation. Quaternion differential equation can also be used for expressing the evolution of the rotation between the two frames a and b. The rotational quaternion derivative noted ${\dot{\mathbf{q}}}_a^b$ is given by:

$${\dot{\mathbf{q}}}_a^b=\frac{1}{2}{\mathbf{q}}_a^b\otimes {\left({\mathbf{\omega }}_{ba}^a\right)}_{\mathbf{q}}$$

(10)

where ${\mathbf{\omega }}_{ba}^a$ is the angular rate of frame b with respect to frame a, expressed in the frame a.

3.2. Sensor Signal Modelling

Because the sensors are of MEMS quality, signal models are proposed for each sensor: tri-axis magnetometer, tri-axis accelerometer and tri-axis gyroscope. All sensor frames are assumed to be orthogonal, co-aligned with the same origin. Consequently, a unique sensor frame for all sensors is defined and labelled body frame with the symbol b. This part presents the error model chosen for each sensor: at first the magnetometer, then the accelerometer and finally the gyroscope.

3.2.1. Magnetometer

The magnetometer measures the local magnetic field in the body frame. In order to remove all sensor perturbation sources: scale factor, non-orthogonally, bias and magnetic deviation due to the hosting platform, a calibration phase is first realized [19]. Only white noise remains:

$${\mathbf{y}}_m^b={\mathbf{m}}^b+{\mathbf{n}}_m$$

(11)

where y_m is the magnetometer signal, m^b is the local magnetic field expressed in the body frame. It includes the earth magnetic field and the local disturbances. The zero mean Gaussian white noise process is noted n_m and characterized by a standard deviation σ_m.

3.2.2. Accelerometer

The sensor axis is non-orthogonally calibrated using a dedicated platform for aligning each axis with the gravity field and applying a least squares based estimation. An Allan variance study [20] is applied to the accelerometer signal, which was acquired during a static phase, for inferring the noise characteristics. Figure 3 shows the Allan variance results plotted for one of the sensor axes.

Three main noise components are identified: an angular random walk, given by the −½ slope part, the bias instability, given by the 0 slope curve part and the beginning of a ½ slope curve at high average time. Consequently, the accelerometer signal is modelled by:

$${\mathbf{y}}_a\mathbf{=}{\mathbf{f}}_{ib}^b\mathbf{+}{\mathbf{b}}_a\mathbf{+}{\mathbf{n}}_a$$

(12)

where the three dimensional sensor signal y_a is composed by:

• ${\mathbf{\text{f}}}_{ib}^b$ the specific force of the body frame with respect to the inertial frame, expressed in the body frame;

• b_a is the bias of the accelerometer and;

• n_a is a zero-mean Gaussian white noise with a standard deviation noted σ_a.

The bias b_a is assumed to follow a Gauss-Markov model whose parameters are determined with the Allan variance study. The bias is mathematically expressed by:

$${\dot{\mathbf{b}}}_a=\mathbf{\beta }{\mathbf{b}}_a+{\mathbf{n}}_{{\mathbf{b}}_a}$$

(13)

where ḃ_a is the acceleration bias derivative, β is a constant and n_ba is zero-mean Gaussian white noise with a standard deviation noted σ_ba. The values of the accelerometer noise components are given in

Table 1. Accelerometer noise parameters.

	Velocity Random Walk	Bias Instability	Correlated Noise
	Allan Deviation (mg/√Hz)			Correlated Time (s)
Axis X	0.0650	0.1223	0.1484	18.16
Axis Y	0.0717	0.0326	0.0373	48.05
Axis Z	0.0639	0.0626	0.0881	197.2

.

3.2.3. Gyroscope

Similarly to the accelerometer signal, the gyroscope signal is studied over a long static phase and its Allan variance or spectral density is extracted. The angular rate is modeled by the following expression:

$${\mathbf{y}}_g\mathbf{=}{\mathbf{\omega }}_{ib}^b\mathbf{+}{\mathbf{b}}_{\omega }\mathbf{+}{\mathbf{n}}_{\omega }$$

(14)

where:

• y_g, the three-dimension signal of the gyroscope, is composed with ${\mathbf{\omega }}_{ib}^b$ the angular rate of the body with respect to the inertial frame;

• b_ω is the gyroscope bias equivalent to an angular drift and;

• n_ω is a zero-mean Gaussian white noise with a standard deviation noted σ_ω.

The gyroscope bias is modelled as a random walk:

$${\dot{\mathbf{b}}}_{\omega }\mathbf{=}{\mathbf{n}}_{{\mathbf{b}}_{\omega }}$$

(15)

where ḃ_ω is the gyroscope bias derivative and n_bω is a zero-mean Gaussian white noise with standard deviation σ_bω. All deviations are extracted from the Allan variance plotted in Figure 4.

The angular rate ${\mathbf{\omega }}_{ib}^b$ can be decomposed in two parts Equation (16) by introducing the navigation frame, which is the plane locally tangent to the Earth. The angular rate of the body frame with respect to the navigation frame ${\mathbf{\omega }}_{nb}^b$ is given by:

$${\mathbf{\omega }}_{ib}^b={\mathbf{\omega }}_{in}^b+{\mathbf{\omega }}_{nb}^b$$

(16)

In the context of pedestrian navigation, the rotation of the navigation frame with respect to the inertial frame ${\mathbf{\omega }}_{in}^b$ is rather small, as compared to the body dynamic expressed with ${\mathbf{\omega }}_{ib}^b$. So ${\mathbf{\omega }}_{in}^b$ is assumed to be a residual part embedded in the gyroscope bias b_ω. The noise component values are given in

Table 2. Gyroscope noise parameters.

	Angular Random Walk	Bias Instability	Rate Random Walk
	Allan Deviation (rad/s/√Hz)
Axis X	0.00413	0.00117	0.00158
Axis Y	0.00445	0.00121	0.00142
Axis Z	0.00442	0.00105	0.00157

.

3.3. Gyroscope Quaternion

3.3.1. Design of the Gyroscope Quaternion Model

Instead of using directly the gyroscope model, the gyroscope signal is interpreted as a rotation between two successive epochs. A mathematical model of this rotation, named the quaternion gyroscope, is proposed and demonstrated. This new model results from the integration of Equation (10) using the navigation and body frames:

$${\mathbf{q}}_b^n(t+T_s)={\mathbf{q}}_b^n(t)\otimes {\mathbf{q}}_{\mathbf{\omega }}\left(t\right)$$

(17)

where:

• q_ω is the rotational quaternion between the two epochs t and t +T_s defined in (18);

• T_s is the sampling period and;

• ${\mathbf{\omega }}_{nb}^b$ is the angular rate of the body frame with respect to the navigation frame. The latter is assumed to be constant over the period t to t +T_s.

$${\mathbf{q}}_{\mathbf{\omega }}=f({\mathbf{\omega }}_{nb}^b)=\left(\begin{array}{c}cos\left(\frac{\Vert {\mathbf{\omega }}_{nb}^b\Vert }{2}{T}_s\right)\\ sin\left(\frac{\Vert {\mathbf{\omega }}_{nb}^b\Vert }{2}{T}_s\right)\frac{{\mathbf{\omega }}_{nb}^b}{\Vert {\mathbf{\omega }}_{nb}^b\Vert }\end{array}\right)$$

(18)

The gyroscope quaternion is constructed similarly to Equation (18), but instead of being composed of ${\mathbf{\omega }}_{nb}^b$, the quaternion gyroscope (noted q_yg) is composed of the gyroscope measurement (noted y_g):

$${\mathbf{q}}_{{\mathbf{\text{y}}}_g}=f({\mathbf{y}}_g)$$

(19)

The quaternion q_yg is a rotational quaternion representing an approximation of q_ω, which is the rotation between two successive epochs. Following previous sensor error modelling, an Allan variance study of q_yg, which has been created using all angular rates recorded during the static period, is conducted in order to assess the noise components of q_yg. The same trend is observed for all four components. The low average time part corresponds to white noise. Figure 5 shows also a bias instability and a correlated or a rate random walk.

Instead of modeling the gyroscope errors in the signal domain, it is performed in the quaternion set:

$${\mathbf{q}}_{{\mathbf{y}}_g}={\mathbf{q}}_{\omega }+{\mathbf{b}}_{{\mathbf{q}}_{\omega }}+{\mathbf{n}}_{{\mathbf{q}}_{\omega }}$$

(20)

where n_qω is a zero-mean Gaussian white noise with standard deviation σ_qω. The stochastic process chosen to model the gyroscope quaternion bias b_qω is a random walk:

$${\dot{\mathbf{b}}}_{{\mathbf{q}}_{\omega }}={\mathbf{n}}_{{\mathbf{b}}_{{\mathbf{q}}_{\omega }}}$$

(21)

where ḃ_qω is the gyroscope quaternion bias derivative and n_bqω is a zero-mean Gaussian white noise with a standard deviation noted σ_bqω.

In order to a better understand the meaning of b_qω, let us analyze the gyroscope quaternion q_yg. This quaternion corresponds to a rotation that approximates the true rotation q_ω between two successive epochs. As a consequence, b_qω appears to be the difference between both rotations. When it is small, it links the rotation q_ω to q_yg by:

$${\mathbf{q}}_{{\mathbf{y}}_g}={\mathbf{q}}_{\mathbf{\omega }}\left(I+{\overline{\mathbf{q}}}_{\mathbf{\omega }}\otimes {\mathbf{b}}_{{\mathbf{q}}_{\mathbf{\omega }}}\right)$$

(22)

where (I + q̅_ω⊗b_qω) represents an infinitesimal rotation linking the q_ω to q_yg.

3.3.2. Analysis of the Gyroscope Quaternion Bias

MAGYQ attitude angles estimation filter is based on an Extended Kalman Filter (EKF) whose working hypothesis (H) is that only white noise components are not modeled in the state vector. Because the proposed angular rate modeling in the quaternion set is new, previous EKF working hypothesis is now demonstrated for the Model (20). Two approaches are followed for the demonstration. The first analysis studies the physical meaning of the quaternion bias term when the latter is small. The second approach exploits simulated angular rates transformed into the quaternion set for analyzing the distribution of the noise terms.

Mathematical Derivation of the Gyroscope Quaternion

Assuming that the angular rates and the gyroscope measurements are small (<10 rad/s), in terms of amplitude over the sampling period T_s, it is possible to re-write the expressions of the rotational quaternion q_ω and the gyroscope quaternion q_yg:

$$\begin{array}{l}{\mathbf{q}}_{{\mathbf{y}}_g}={\mathbf{I}}_q+{\left(\frac{T_s}{2}{\mathbf{y}}_g\right)}_q+\underset{T_s\to 0}{o\left(T_s\right)}\\ {\mathbf{q}}_{\omega }={\mathbf{I}}_q+{\left(\frac{T_s}{2}{\mathbf{\omega }}_{nb}^b\right)}_q+\underset{T_s\to 0}{o\left(T_s\right)}\end{array}$$

(23)

where I_q is the identity quaternion. $\underset{T_s\to 0}{o\left(T_s\right)}$ is a function that can be neglected when T_s tends to zero.

Grouping both expressions in the gyroscope model Equation (14), the gyroscope quaternion q_yg becomes:

$${\mathbf{q}}_{{\mathbf{y}}_g}={\mathbf{q}}_{\omega }+{\left(\frac{T_s}{2}{\mathbf{b}}_{\omega }\right)}_q+{\left(\frac{T_s}{2}{\mathbf{n}}_{\omega }\right)}_q+\underset{T_s\to 0}{o\left(T_s\right)}$$

(24)

with a first order approximation of Equation (24), only a random walk and a zero-mean white Gaussian noise terms remain in the last three components of q_yg. Indeed the noise of the first quaternion component equals zero and is removed in the MAGYQ filter. This observation is confirmed by the small value that is observed in the Allan Variance plot (Figure 5). This mathematical derivation ends the first demonstration and proves that hypothesis H is verified at the first order.

Validation of the Gyroscope Quaternion Modeling by Simulation

Angular rates are simulated in order to demonstrate the validity of the gyroscope quaternion model proposed in Equations (20) and (21). The signal ỹ_g is simulated for a static phase. Consequently it is only composed of a zero-mean white noise and a random walk. Using the simulated gyroscope signal and Equation (19), it is possible to construct the corresponding gyroscope quaternion q_ỹg. A first comparison of the simulated angular rates and the gyroscope quaternions is performed in the frequency domain for assessing the similarities.

The spectral density, computed with Fast Fourier Transform [21,22], is used to analyze the noise terms embedded in q_ỹg. Figures 6 and 7 show respectively the spectral density of ỹ_g and q_ỹg. The same trend is observed in both cases. At low frequency, a curve with a slope of −20 db/decade, which is characteristic of a random walk, is visible. At high frequency, a flat curve slope characterizing a white noise is also visible. These similarities validate the correctness of the proposed gyroscope quaternion modelling.

In order to test if hypothesis H is verified with the proposed model, a Kolmogorov-Smirnov test [23] is conducted to evaluate the distance between two cumulative distribution functions (CDF). The first CDF is computed with q_ỹg over 100,000 samples. The second CDF is computed with the simulated gyroscope quaternions q̃_yg using the Model (20) over the same time interval. The white noise and random walk stochastic processes parameters are extracted from the Allan Variance analysis previously detailed.

The hypothesis H₀ under test is that “the two signals are following the same distribution law”. In order to be unaffected by the choice of stochastic parameters, the distributions are centered and normalized. The Kolmogorov-Smirnov test is performed considering a 5% risk of wrongly rejecting the hypothesis:

$$T=\mathit{\text{proba}}\left(\underset{k}{sup}\left(\left|D_1\left(k\right)-D_2\left(k\right)\right|\right)\ge \frac{1.36}{\sqrt{N_{\mathit{\text{samples}}}}}\right)$$

(25)

H₀ is verified with a probability greater than 80% for the second, third and fourth quaternion elements. Only the first component of the quaternion slightly fails the hypothesis H₀. However the very low noise level of this term justifies the chosen model, i.e., white noise and random walk.

From this analysis, it can be concluded that the proposed quaternion based error model is correctly capturing non-stationary noises and only white noise remains. Consequently, designing an EKF for estimating the orientation of the body frame with respect to navigation frame using the proposed novel quaternion based angular rates model is justified.

4. Dynamic State Propagation

MAGYQ, the attitude and heading estimation filter, is now described. The state vector is first presented and followed by the initialization process. Finally, the mathematical expression of the state propagation is described.

4.1. State Vector

The complete state vector includes the quaternion form ${\mathbf{q}}_b^n$ of the rotation between the body and the navigation frame, but also all sensor error sources: the gyroscope quaternion bias b_qω and the accelerometer bias b_a. The state vector x is:

$${\mathbf{x}}^T=\left[\begin{array}{ccc}{\mathbf{q}}_b^{nT}& {{\mathbf{b}}_{{\mathbf{q}}_{\omega }}}^T& {{\mathbf{b}}_a}^T\end{array}\right]$$

(26)

4.2. Initialization

The initial rotation is computed using the accelerometer and magnetometer measurements during a static phase without magnetic disturbances. During the initialization, it is expected that the sensors are directly measuring the gravity and the Earth magnetic fields.

4.3. Error State Propagation Model

The additive error state vector δx links the estimated state vector noted x̂ to the true state vector noted x, defined in Equation (26), by:

$$\mathbf{x}=\widehat{\mathbf{x}}+\delta \mathbf{x}$$

(27)

The evolution laws of the acceleration and gyroscope quaternion biases are those of the random walk stochastic process. The discrete equations with a first order approximation are:

$$\{\begin{array}{l}{\mathbf{b}}_{{\mathbf{q}}_{\omega }}(t+T_s)={\mathbf{b}}_{{\mathbf{q}}_{\omega }}(t)+T_s{\mathbf{n}}_{{\mathbf{b}}_{{\mathbf{q}}_{\omega }}}(t)\\ {\mathbf{b}}_a(t+T_s)=\left(1-\mathbf{\beta }{T}_s\right){\mathbf{b}}_a(t)+T_s{\mathbf{n}}_{{\mathbf{b}}_a}(t)\end{array}$$

(28)

The propagation of the rotation ${\mathbf{\text{q}}}_b^n$ is given by the integration of Equation (10):

$${\mathbf{q}}_b^n(t+T_s)={\mathbf{q}}_b^n(t)\otimes {\mathbf{q}}_{\mathbf{\omega }}\left(t\right)$$

(29)

The quaternion q_ω is unknown, only an approximation noted q̂_ω is accessible through the gyroscope quaternion and the estimated bias:

$${\widehat{\mathbf{q}}}_{\omega }={\mathbf{q}}_{{\mathbf{y}}_g}-{\widehat{\mathbf{b}}}_{{\mathbf{q}}_{\omega }}$$

(30)

The estimated rotation ${\widehat{q}}_b^n$ is propagated using the estimated rotation between two successive epochs q̂_ω:

$${\widehat{\mathbf{q}}}_b^n(t+T_s)={\widehat{\mathbf{q}}}_b^n(t)\otimes {\widehat{\mathbf{q}}}_{\mathbf{\omega }}\left(t\right)$$

(31)

In this step, ${\widehat{\mathbf{q}}}_b^n$ and q̂_ω are assumed to be rotational quaternions, so they must satisfy $\Vert {\widehat{\mathbf{q}}}_b^n\Vert =\Vert {\widehat{\mathbf{q}}}_{\mathbf{\omega }}\Vert =1$ A normalization step is applied. In the remaining equations, all quaternion components ( )_q are normalized to comply with the rotation subset of the quaternion set. With a first order development of Equations (28) and (29), the state error model becomes:

$$\delta \mathbf{x}\left(t+T_s\right)=\left(\begin{array}{ccc}{\mathbf{C}}_{{\widehat{\mathbf{q}}}_{\omega }}& -{\mathbf{M}}_{{\widehat{\mathbf{q}}}_b^n}& \mathbf{0}\\ \mathbf{0}& \mathbf{I}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \left(\mathbf{I}-\mathbf{\beta }{T}_s\right)\end{array}\right)\delta \mathbf{x}(t)+\left(\begin{array}{ccc}-{\mathbf{M}}_{{\widehat{\mathbf{q}}}_b^n}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& T_s\mathbf{I}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& T_s\mathbf{I}\end{array}\right)\left(\begin{array}{c}{\mathbf{n}}_{{\mathbf{q}}_{\omega }}(t)\\ {\mathbf{n}}_{{\mathbf{b}}_{{\mathbf{q}}_{\omega }}}(t)\\ {\mathbf{n}}_a(t)\end{array}\right)$$

(32)

Given that the first component of the state vector is the orientation of the body frame with respect to the navigation frame and that the second component is the gyroscope quaternion bias, Equation (32) shows the correlation between these two components. It indicates that the error in the orientation comes from the precedent error combined with the rotation error between two successive epochs.

5. Static Period Detection Threshold

All updates are based on a priori knowledge of opportune local fields in the navigation frame. As it will be explained in next part, static acceleration or magnetic field in the navigation frame informs about the attitude angles and can be used to correct the state parameters. But the challenge is that the data are only available in the body frame. A reference field must be found for inferring the presence of a static field period in the navigation frame using the measurements in the body frame.

By definition, the vector norm is invariant through isometric transformation. Consequently, the static period detection process is primarily based on the study of the norm. It consists in a statistical test of the field time derivative, such as likelihood ratio test (LTR) [24]. It performs a comparison with a predetermined threshold [16] and rejects the static hypothesis if the innovation is too large. Last approach is known as adaptive Kalman Filtering with Fault Detection and Exclusion (FDE) technique. In all cases, the goal is to identify if at current epoch, the acceleration or magnetic field in the navigation frame is constant.

At epoch t, the static field detector is based on the study of the variance of the field of interest and a comparison with an initially fixed threshold:

$$\frac{1}{N+1}\sum _{k=t-N{T}_s}^{t+T_s}{\left(\Vert {\mathbf{field}}^b(k)\Vert -{\xi }_{\mathit{\text{ref}}}\right)}^2<{\gamma }_1$$

(33)

where field refers to the acceleration or magnetic field. N is the length of the static period. ξ_ref is the reference norm for the current static period and γ₁ is a threshold.

The reference norm is defined at the beginning of the static period as the mean of the N_first first values of the static period. The threshold γ₁ is defined as the deviation of the field during a static and undisturbed period. This first detector compares a global deviation with a threshold. In this approach, each sample data must be close to the reference norm, which is a limiting factor. Consequently a variant is introduced:

$$\left|\Vert {\mathbf{field}}^b(k)\Vert -{\xi }_{\mathit{\text{ref}}}\right|\le {\gamma }_2$$

(34)

where γ₂ is a threshold different from γ₁ but also fixed during a static and undisturbed period. This threshold is used to reject outliers. It completes the first detector dedicated to the global study of deviation over the current static period.

6. Magnetometer and Accelerometer-Based Heading and Bias Correction

The use of accelerometers and magnetometers can assist the heading estimation problem. In the literature, it is well known that if known static fields are sensed, e.g., Earth magnetic field or Earth gravity field, a direct observation of the angles is possible. The situation is more challenging if the measured fields are unknown. Even in this case, it is still possible to extract heading information by correcting the angular drift modelled in the gyroscope quaternion bias. This is now explained.

6.1. Quasi-Static Field (QSF) Error Model

Two similar Quasi-Static Field (QSF) error models are proposed for the magnetic field and the acceleration field. They translate novel observations to correct the state vector when conditions (33) and (34) are verified, i.e., the acceleration or magnetic field is assumed to be constant in the navigation frame. The two possible QSF updates are now explained, starting with the quasi-static magnetic field error model and continuing with the quasi-static acceleration field.

Let us consider the k-th quasi-static period of the magnetic field with a reference field noted ${\mathbf{\text{m}}}_{re{f}_k}^n$. At each epoch of the present static period, the sensed magnetic field y_m is transformed into the navigation frame using the estimated rotation ${\widehat{\mathbf{q}}}_b^n$ and compared to the reference field. The innovation δz_m obtained by this correction is:

$${\left(\delta {\mathbf{z}}_m\right)}_{\mathbf{q}}={\left({\mathbf{m}}_{re{f}_k}^n\right)}_{\mathbf{q}}-{\widehat{\mathbf{q}}}_b^n\otimes {({\mathbf{y}}_m^b)}_{\mathbf{q}}\otimes {\overline{\widehat{\mathbf{q}}}}_b^n$$

(35)

For the acceleration field, the principle is the same, but the accelerometer measures y_a and gives a biased estimation of the acceleration field, so the acceleration bias must be removed. Let ${\mathbf{\text{f}}}_{re{f}_k}^n$ be the reference specific force at the k-th quasi-static period. This period is not necessarily the same as the quasi-static magnetic field period. The innovation noted δz_a is:

$${(\delta {\mathbf{z}}_a)}_{\mathbf{q}}={\left({\mathbf{f}}_{re{f}_k}^n\right)}_{\mathbf{q}}-{\widehat{\mathbf{q}}}_b^n\otimes {({\mathbf{y}}_a^b-{\widehat{\mathbf{b}}}_a)}_{\mathbf{q}}\otimes {\overline{\widehat{\mathbf{q}}}}_b^n$$

(36)

One of the difficulties is to determine the reference field. In the first case, the norm of the field equals the norm of the Earth gravity field or the Earth magnetic field. In this case, there is a global correction of the orientation. But in indoor environments, perturbed by magnetic disturbances, only few such cases occur. The second case uses any quasi-static field as long as the Earth magnetic field disturbances are evaluated as constant. The reference field is then computed as the mean of the N_first values of the field of interest. The QSF error models are derived with a first order development of Equations (35) and (36):

$$\{\begin{array}{l}\delta {\mathbf{z}}_m=\left[\begin{array}{ccc}{h}_1\left({\widehat{\mathbf{q}}}_b^n,{\mathbf{m}}_{re{f}_k}^b\right)& \mathbf{0}& \mathbf{0}\end{array}\right]\delta \mathbf{x}-h_2\left({\widehat{\mathbf{q}}}_b^n\right){\mathbf{n}}_m\\ \delta {\mathbf{z}}_a=\left[\begin{array}{ccc}{h}_1\left({\widehat{\mathbf{q}}}_b^n,{\mathbf{a}}_{re{f}_k}^b\right)& \mathbf{0}& -h_2\left({\widehat{\mathbf{q}}}_b^n\right)\end{array}\right]\delta \mathbf{x}-h_2\left({\widehat{\mathbf{q}}}_b^n\right){\mathbf{n}}_a\end{array}$$

(37)

The functions h₁ and h₂ are defined by:

$$\begin{array}{l}{h}_1:\left(\mathbf{q},\mathbf{x}\right)\in (ℍ,{ℝ}^3)\mapsto 2\left[\begin{array}{cc}{q}_1\mathbf{x}+\left[{\mathbf{u}}_q\times \right]\mathbf{x}& \langle {\mathbf{u}}_q,\mathbf{x}\rangle {\mathbf{I}}_{3\times 3}-\left[\left(q_1\mathbf{x}+\left[{\mathbf{u}}_q\times \right]\mathbf{x}\right)\times \right]\end{array}\right]\\ h_2:\mathbf{q}\in ℍ\mapsto \left({q_1}^2-{\Vert {\mathbf{u}}_q\Vert }^2\right){\mathbf{I}}_{3\times 3}+2{\mathbf{u}}_q\cdot {{\mathbf{u}}_q}^T+2q_1\left[{\mathbf{u}}_q\times \right]\end{array}$$

(38)

6.2. Magnetic Angular Rate and Acceleration Gradient Updates

Magnetic angular rate update (MARU) or acceleration gradient update (AGU) are applied during QSF periods. When the field is constant in the navigation frame, its variation between two successive epochs in the body frame is only due to the body frame rotation with respect to the navigation frame:

$${({\mathbf{field}}^b(t))}_q={\mathbf{q}}_{\mathbf{\omega }}\left(t\right)\otimes {({\mathbf{field}}^b\left(t+T_s\right))}_q\otimes {\overline{\mathbf{q}}}_{\mathbf{\omega }}\left(t\right)$$

(39)

where q_ω is defined in Equation (18).

Similarly to the precedent part on QSF, two applications of this update are possible. The first is the magnetic angular rate update and the second is the acceleration gradient update.

6.2.1. Magnetic Angular Rate Model

The evolution law of the magnetic field, expressed in body frame, during a quasi-static magnetic field period k, is derived from Equation (39). The magnetic field m̂ at epoch t + T_s is estimated from the magnetic field sensed at epoch t and the estimated rotation q̂_ω between the epochs t and t + T_s:

$${({\widehat{\mathbf{m}}}^b(t+T_s))}_q={\overline{\widehat{\mathbf{q}}}}_{\mathbf{\omega }}\left(t\right)\otimes {\left({\mathbf{y}}_m^b\left(t\right)\right)}_q\otimes {\widehat{\mathbf{q}}}_{\mathbf{\omega }}\left(t\right)$$

(40)

This estimated value is compared to the magnetometer data at epoch t + T_s, which gives the magnetic angular rate innovation δz_MARU:

$$\delta {\mathbf{z}}_{\mathit{\text{MARU}}}={\mathbf{y}}_m^b\left(t+T_s\right)-{\widehat{\mathbf{m}}}^b(t+T_s)$$

(41)

A first order development of Equation (41) gives:

$$\delta {\mathbf{z}}_{\mathit{\text{MARU}}}=\left[\begin{array}{ccc}\mathbf{0}& -h_3\left({\widehat{\mathbf{q}}}_{\omega },{\mathbf{m}}^b(t)\right)& \mathbf{0}\end{array}\right]\delta \mathbf{x}+\left[\begin{array}{cc}{\mathbf{I}}_{3\times 3}& -h_2\left({\overline{\widehat{\mathbf{q}}}}_{\omega }\right)\end{array}\right]\left[\begin{array}{c}{\mathbf{n}}_m\left(t+T_s\right)\\ {\mathbf{n}}_m\left(t\right)\end{array}\right]$$

(42)

where the function h₂ is defined in Equation (38) and the function h₃ is given by

$$h_3:\left(\mathbf{q},\mathbf{x}\right)\in (ℍ,{ℝ}^3)\mapsto 2\left[\begin{array}{cc}{q}_1\mathbf{x}-\left[{\mathbf{u}}_q\times \right]\mathbf{x}& \langle {\mathbf{u}}_q,\mathbf{x}\rangle {\mathbf{I}}_{3\times 3}+\left[\left(q_1\mathbf{x}-\left[{\mathbf{u}}_q\times \right]\mathbf{x}\right)\times \right]\end{array}\right]$$

(43)

This update enables to correct the angular drift modelled in the gyroscope quaternion bias.

6.2.2. Acceleration Gradient Model

Similarly to the magnetic angular rate update, the acceleration gradient update is based on identified QSF. During a static phase of the acceleration field, its evolution at epoch t + T_s is predictable using the rotation between two epochs and the acceleration field at epoch t. Following this approach, the acceleration field is estimated by:

$${({\widehat{\mathbf{f}}}^b(t+T_s))}_q={\overline{\widehat{\mathbf{q}}}}_{\mathbf{\omega }}\left(t\right)\otimes {\left({\mathbf{y}}_a^b\left(t\right)-{\widehat{\mathbf{b}}}_a\left(t\right)\right)}_q\otimes {\widehat{\mathbf{q}}}_{\mathbf{\omega }}\left(t\right)$$

(44)

The acceleration gradient innovation δz_AGU is finally computed by

$$\delta {\mathbf{z}}_{\mathit{\text{AGU}}}={\mathbf{y}}_a^b\left(t+T_s\right)-({\widehat{\mathbf{f}}}^b(t+T_s)+\left(\mathbf{I}-\mathbf{\beta }{T}_s\right){\widehat{\mathbf{b}}}_a\left(t\right))$$

(45)

In Equation (45), the acceleration bias term b̂_a (t) is added to the specific force f̂^b for providing a comparison with the acceleration ${\mathbf{\text{y}}}_a^b$. With a first order development, the acceleration gradient model becomes:

$$\delta {\mathbf{z}}_{\mathit{\text{AGU}}}=\left[\begin{array}{ccc}\mathbf{0}& -h_3\left({\widehat{\mathbf{q}}}_{\omega },{\mathbf{y}}_a^b\left(t\right)-{\widehat{\mathbf{b}}}_a\left(t\right)\right)& \mathbf{I}-\left(\mathbf{\beta }{T}_s+h_2\left({\overline{\widehat{\mathbf{q}}}}_{\omega }\right)\right)\end{array}\right]\delta \mathbf{x}+\left[\begin{array}{cc}{\mathbf{I}}_{3\times 3}& -h_2\left({\overline{\widehat{\mathbf{q}}}}_{\omega }\right)\end{array}\right]\left[\begin{array}{c}{\mathbf{n}}_a\left(t+T_s\right)\\ {\mathbf{n}}_a\left(t\right)\end{array}\right]$$

(46)

where the functions h₂ and h₃ are defined in Equations (38) and (43). This update offers very interesting observations since it involves both the accelerometer and gyroscope quaternion biases.

7. Step Length Estimation

Following a PDR approach, step lengths and headings are merged for recursively propagating the pedestrian's position at epoch t + ΔT from the one at epoch t. Whereas the headings are computed using MAGYQ, the step lengths are computed following a three steps process [25]. First, the motion mode, the handheld device carrying mode and the step frequency f_step are estimated based on a decision tree and several features extracted from time and frequency domain analyses of the accelerations and angular rates. Step events are estimated in a separated independent process that is adaptive to the pedestrian walking pace. Finally, the step lengths are estimated with the following model:

$$s=h\left(a{f}_{\mathit{\text{step}}}+b\right)+c$$

(47)

The step length s is a function of the step frequency f_step, the user's height h and three parameters {a,b,c}. These parameters are estimated using a least square method based on known users' walking speed that is determined with an accurate differential GPS positioning technique. This satellites signal based solution is also the reference for the experimental tests that are now presented.

8. Experimental Validation

Experimental tests are conducted for evaluating the performances of the MAGYQ attitude estimation filter. After a description of the experimental protocol, the filter outcomes are assessed in terms of orientation and position.

8.1. Description of the Experiment

The IMU used for the experiment is the ADIS 16488 [26]. This inertial and magnetic unit is composed by a tri-axis magnetometer, a tri-axis accelerometer and a tri-axis gyroscope. All sensors are co-aligned and their measurements are expressed in a unique common frame: the body frame. The IMU is connected to an acquisition device that registers the sensor data at 100 Hz for an offline processing.

Three people, whose age is between 25 and 56 years and height varied between 1.62 and 1.84 m, were equipped with this hardware. During the data acquisition, they kept the body frame roughly fixed with respect to it. This means that the sensor pointing direction is approximately the same as the walking direction and no misalignment is considered in this paper. The three test subjects are labelled M1, M2 and W1 (where M is for man and W for woman), in the following figures. The total walking path length was about 1.5 km including successive periods in outdoor and indoor environments. The experiment starts outdoors with the magnetometer calibration and the initialization phase. This initialization must be performed in surroundings without magnetic disturbances where only the earth magnetic field is measured. The people continued the tests walking successively in the parking lot and inside a building.

To evaluate the performances of the present algorithm, a reference system is needed. In outdoor spaces, where Global Navigation Satellite Signals (GNSS) are available, a post-processed differential trajectory was calculated using GrafNav software from NovAtel [27]. The 5 cm accurate reference solution is post-processed at 5 Hz for the GNSS antenna attached on the pedestrian's cap, as shown in Figure 8. The GNSS reference trajectory is only available in outdoor spaces. Inside the building, the main corridor directions are used. Let us notice that the width of these corridors is small and the subjects walked along these directions. The latter are extracted from an accurate office map. The performances are analyzed with the different footpaths overlayed on the building background map. It is important to recall that even outside, the results are only computed using the handheld MEMS signals and the proposed MAGYQ filter. The same MEMS signals are used for all tests.

8.2. Performance Assessment of Estimated Angles

Prior to assessing MAGYQ performances combined with step lengths in the positioning domain, the attitude angles estimation is evaluated with two different tests:

• Indoor data collection with static IMU.

• Dynamic outdoor data collection.

8.2.1. Indoor Static Test

For the static test, 5000 seconds of static IMU data were collected in an indoor space, where the local magnetic field is constant but different from the earth magnetic field. Three attitude estimators are applied to the recorded data. Two of them are state of the art estimators. All angle outcomes are shown in Figure 9.

The angles in blue are estimated with the GyInt approach, for Gyroscope Integration. This state of the art estimator corresponds to a nominal gyroscope bias corrected integration only. In this strategy, gyroscope measurements are corrected with the nominal angular rate bias, extracted from the Allan Variance analysis, and then propagated in time. It can be observed that GyInt attitude estimator presents a large angular random walk drift.

The second estimator, shown in green, corresponds to the well-known QUEST estimator. Corrections are issued from the detection of the earth gravity or earth magnetic field. In this indoor configuration, only the earth gravity measurement hypothesis is verified. Consequently, only roll and pitch angles are corrected. The yaw angle is not observable and becomes badly corrected. A norm, close to the geomagnetic field, is detected and biases the solution.

The last estimator, shown in red, is the MAGYQ filter. Contrary to the previous two estimators, it can be observed that the attitude is continuously corrected even if the sensed magnetic field is not the earth magnetic field. As expected, MAGYQ is able to absorb the angular drift into the quaternion gyroscope bias even with “perturbed magnetic field” as long as they are constant. The attitude is found to be stable over the 1 h and 23 min period.

8.2.2. Dynamic Outdoor Data Collection

The test consists in an outdoor walk without changing misalignment between the walking direction and the sensor pointing direction. The aim of this test is to verify if MAGYQ filter corrects the initial gyroscope bias, even if the latter is wrongly calibrated. The reference heading, shown in green in Figure 10, is post-processed using the Differential GPS (DGPS) solution.

It is important to mention that some outliers are present in the GPS positions, leading to some inaccurate heading estimates. These outliers are principally due to satellite signals multi-path and fading effects induced by the surrounding vegetation and buildings. Globally, the DGPS solution provides an accurate estimation of the heading at a lower rate than the IMU one. GyInt estimator is also applied but the initial bias used to correct the angular rates is slightly exaggerated in order to better observe the performance of MAGYQ filter. This is the reason why it is labelled GyInt Biased and depicted in blue in the same figure.

An important time drift of the yaw angle is observed with this approach. On the contrary, MAGYQ filter, depicted in red, provides an important correction of the gyroscope quaternion bias, which deletes the angular drift. Because MAGYQ filter takes advantages of accelerometer and magnetometer signals for correcting the gyroscope and accelerometer drifts in much more situations than the other filters, the observed angle estimation performances are better. The heading is continuously readjusted and its estimation error remains below 10° over the 1.5 km walk. This is computed using the DGPS heading during the 15 min period.

8.3. Performance Assessment of Estimated Pedestrian Trajectory

After verifying the filter behavior, the MAGYQ based heading is mixed with step length for estimating the PDR footpath. Figures 11, 12 and 13 show the three trajectories corresponding to the test described in Section 8.1. The outdoor reference is given by the DGPS solution and is plotted in green.

The GyInt and MAGYQ inertial trajectories are estimated using the same step length calculated with the Model (47) and individually calibrated {a,b,c} parameters. The trajectory estimation only differs in the heading process. The GyInt estimation (blue) is performed by integrating the angular rates, knowing the nominal gyroscope bias. The red trajectory corresponds to the positions estimated using the heading from MAGYQ outcomes. A matching point is applied to initialize the free-inertial footpath using the DGPS position calculated at the entrance of the building. This enables to assess the heading estimation quality inside the buildings using the main corridor directions.

In Figure 11, the gyroscope propagation (GyInt) solution is quite accurate and there is no significant difference with MAGYQ estimation. In this case, the gyroscope quaternion bias remains relatively stable. But in Figures 12 and 13, GyInt solution is drifting over the entire walking path, whereas MAGYQ algorithm is able to correct a substantial part of the drift. It is observed that the step length errors are dominating the positioning error budget.

9. Conclusions/Outlook

A novel Magnetic, Acceleration fields and GYroscope Quaternion (MAGYQ) based attitude and heading estimation filter is proposed and demonstrated with handheld sensors. This filter parameterizes the state vector directly in the quaternion set in order to reduce the filter error propagation and some of the linearization issues following an Extended Kalman Filter approach. Instead of using a gyroscope signal modeling at the signal level, a new quaternion based model is proposed introducing the gyroscope quaternion bias term. The compatibility of this new angular rate modeling with the Extended Kalman Filter underlying assumptions is demonstrated at the theoretical level and with a simulation.

Among the novel aspects are the introduction of new observation equations that enable to correct the drifting part of the acceleration and gyroscope data using the gradient of the observed fields. A new quaternion set based magnetic angular rate (MARU) update is proposed. It is applied whenever the local magnetic field, perturbed or not, is identified as constant over a sliding window. This update is frequently applied along the pedestrian's route and is found to be very powerful for estimating the gyroscope drift even inside buildings. Whenever the acceleration is found to be steady over a time interval, a new quaternion based acceleration gradient update (AGU) is also proposed. Through this update, not only is the acceleration bias observed but also the gyroscope quaternion bias.

Outcomes of the experimental tests assess the good performance of MAGYQ in correctly estimating the attitude angles as compared to a differential GPS solution. The evaluation is also performed during static acquisition conditions. A heading error below 10° over a 1.5 km walk is observed for two man and one woman test subjects. The results are also compared with the well-known QUEST algorithm and the GyInt algorithm, i.e., a gyroscope integration scheme with initial bias correction. MAGYQ is found to provide better results than the other approaches, especially in indoor space where the earth magnetic field is disturbed and the other strategy fail to mitigate the drifting noise components. The heading estimated with MAGYQ is also combined with step lengths estimated with an individually calibrated model for observing the performance in the positioning domain. Direct comparison with background building map is finally proposed. It is observed that step length errors are dominating the positioning error. No misalignment between the pedestrian walking direction and the handheld device pointing direction is considered in this research, this aspect belongs to future research perspectives.