Quaternion Based Heading Estimation with Handheld ...

Quaternion Based Heading Estimation with Handheld MEMS in Indoor Environments Valerie Renaudin, Christophe Combettes, François Peyret GEOLOC Laboratory, IFSTTAR, France valerie.renaudin(at)ifsttar.fr

Abstract— Pedestrian Dead-Reckoning (PDR) is the prime candidate for autonomous navigation with self-contained sensors. Nevertheless with noisy sensor signals and high hand dynamics, estimating accurate attitude angles remains a challenge for achieving long term positioning accuracy. A new attitude estimation algorithm based on a quaternion parameterization directly in the state vector and two opportunistic updates, i.e. magnetic angular rate update and acceleration gradient update, is proposed. The benefit of this method is assessed both at the theoretical level and at the experimental level. The error on the heading, estimated only with the PDR navigation algorithms, is found to less than 7° after 1 km of walk.

parameters and the classification of pedestrian activities [2], estimates the traveled distance. Secondly, an attitude estimation algorithm is used to compute the most accurate pedestrian walking direction. Finally, the pedestrian trajectory is obtained following the PDR strategy.

Keywords—Indoor navigation, Mems, Quaternions, Attitude estimation, Kalman Filter, magnetic Angular rate update, Acceleration gradient update.

I.

INTRODUCTION

Navigating with handheld device composed of low cost micro-IMU remains a real challenge. Nowadays MicroElectro-Mechanical-Sensors (MEMS) embedded in smartphone can be merged with Global Navigation Satellite System (GNSS) signals to conceive an accurate inertial navigation system. But GNSS positioning is mainly accurate in outdoor spaces, i.e. without the GNSS wave’s barriers represented by urban canyons or indoor environments. Consequently, autonomous handled devices are considered as interesting unconstrained solutions for pedestrian navigation. In the context of handheld device, many issues raise for achieving accurate and robust estimation of hand movements. At first, inertial and magnetic field measurement units, which are composed of tri-axis accelerometer, tri-axis gyroscope and tri-axis magnetometer, are of low quality inducing significant noises in the recorded signals. A second issue is the misalignment between the walking direction and estimated attitude of the IMU. Contrary to an IMU rigidly mounted in an airplane, sensors in smartphone are sensing parasite hand movements, which are uncorrelated to the mean locomotion of the pedestrian. This phenomenon is described in Fig. 1, where all reference frames involved in self-contained pedestrian navigation solutions are shown. Despite these issues, pedestrian dead-reckoning (PDR) based on handheld IMU remains one of the most promising approaches for navigating indoors and basically everywhere. PDR processing strategy is divided in two parallel tasks. A step length model [1], which depends on physiological

Fig. 1. Reference frames involved in the attitude estimation problem

Principally due to the high dynamic of hand movements, the error on attitude estimation dominates the error budget of PDR solution. Consequently, novel methods and systems for accurately estimating the attitude angles of handheld IMU are needed in order to reduce as much as possible the propagation of angular error in the navigation filter. To achieve an accurate and robust estimation, the main challenges are:  to perform the most realistic sensor noise modeling,  to discover frequent and reliable updating processes and  to minimize any linearization step. Classically, direct cosine matrix (DCM) parameterized by Euler angles are used for representing a rotational frame rate. This method presents some drawbacks: principally nonlinearity and some singularities (detailed in section II.A.1). An alternate solution consists in using quaternions, instead of Euler angles, as parameters in the DCM. This approach removes singularities but still involves strong non-linearity.

Contrary to previous approaches, it is proposed to use directly a quaternion error model in the state vector and benefits from the quaternion algebra properties for improving the attitude angles estimation of a handheld IMU. This strategy leads to a novel quaternion attitude estimation algorithm. Furthermore two innovative frequent updates are proposed:  the magnetic angular rate update (MARU) and  the acceleration gradient update (AGU). Section II gives a state-of-the-art on existing attitude estimation solutions and introduces the proposed innovations. Section III details the theoretical aspects of the novel quaternion error-based attitude heading estimation method. Sections IV and V present, both, the computational benefits of this new algorithm and the outcomes of an experimental validation. II.

STATE OF THE ART

Two options can be explored for reducing errors in heading estimation. First approach consists in choosing the best mathematical parameterization for minimizing all linearization steps. The second option tends to exploit all available sensors data in order to improve the attitude estimation through diverse hybridization schemes [3]. A. Existing Attitude Estimation Solutions This part recalls the DCM method that is applied to model and compute the rotation of a body frame with respect to a navigation frame. The coupling of inertial data with magnetic field, for completing the observation of the body frame rotation, is also presented. 1) Euler angles with Direct-cosine matrix Algorithms based on Euler parameterization track small rotation errors that are applied to correct the estimated attitude rotation [4]. This algorithm uses Euler angles, which describe the three dimensional rotation and the three successive rotations (yaw ψ, pitch θ, and roll  ). Euler angles are used to parameterize the DCM that corresponds to the rotation between the body and the navigation (or local) frames.

Cbn



 cos  cos sin  sin  cos  sin cos    cos  sin sin sin  sin   cos  cos  ...    sin   cos  sin   cos  sin  cos  sin  sin  ...cos  sin  sin  sin  cos    cos  cos  



This form of matrix, parameterized with Euler angles, presents singularities that are known as “Gimbal lock”. For example, several pairs of yaw and roll angles are giving the same rotation when the pitch angle equals π/2. This parameterization is highly non-linear. Indeed, each element of the DCM is a sum of products of sine and cosine functions. 2) Coupling of magnetic field and inertial measurements Exploiting magnetic field measurement is an efficient method for improving the attitude estimation accuracy.

Generally, the hybridization strategies are based on a priori knowledge of the local magnetic field or at least its properties. All methods require a calibration of the magnetic field induced by the platform hosting the tri-axis magnetometer [5] in order to measure only the field that is external to the platform. The literature proposes four different main methods for using sensed magnetic fields. Before detailing the latter, let us define the known local magnetic field in the navigation frame: Bn. It is related to the magnetometer measurement Bb, expressed in the body frame, by the DCM: Cbn . 

Bb  CbnBn 



a) Searching for the geomagnetic Earth field. A first method consists in trying to eliminate all artificial sources of magnetic field that are added to the known Earth magnetic field [6]. Globally, this approach fails to properly discriminate magnetic disturbances. Indeed indoor environments contain a lot of perturbation sources that modify the local magnetic field and that can hardly be eliminated [7]. b) Fingerprinting with magnetic anomalies. A second method was recently proposed for navigating using only mesured magnetic fields. A calibration phase maps all indoor magnetic field [8],[9]. Then, similarly to fingerprinting methods with WiFi receiver signal strength, magnetic anomalies are used to identify the user’s location. In this method, the knowledge about local magnetic field anomalies is fully exploited, but this method is non-ergonomic and laborious. For example, the impact on the performances of this method facing all sof-iron effects in a crowd is not understood and needs to be assessed. c) Sensing the velocity with a magnetometer array. A third method consists in using several magnetometers, separated by a fixed small distance, for measuring the magnetic field gradient and deducting directly the velocity vector [10]. This method is not based on a priori knowledge of the local magnetic field but it relates the magnetic field space gradient (ΔBb/Δx) and the derivate of the position in the body frame (Δx/Δt) to the magnetic field temporal derivate (ΔBb/Δt) by the following relationship: 

Bb x Bb   t t x



With this method, the calibration of individual magnetometers becomes very important since all magnetometers must measure the same magnetic field. This is a real challenge with varying fields on the hosting platform. Furthermore, the distance between each magnetometer is critical for ensuring a good approximation of the space gradient. d) Magnetic angular rate update with DCM. Last method consists in detecting quasi-static magnetic field (QSF) in the navigation frame even if the latter are different from the earth geomagnetic field [11]. In this approach, the magnetic

field may include any magnetic disturbance, but they must be globally constant in the local frame. Under this assumption, the magnetic field temporal gradient provides a direct observation of the body frame angular rate (ω):

dBb  ω  B b , dt

(4)

where ˄ is the cross product of two vectors. The algorithm proposed in this paper utilizes also the magnetometer gradient but contrary to previous magnetometer array-based algorithm, a sole tri-axis of magnetometers is sufficient. B. Innovation of the proposed method This paper describes an algorithm that uses quaternions to parameterize the state vector in a Kalman filter whose aim is to estimate the attitude of the body frame of a handheld device with respect to a local frame. The updating steps are based on magnetic field angular rate (MARU) and on acceleration gradient update (AGU). The following three paragraphs explain the novel aspects of this research. First novel aspect is the use of quaternion directly in the state vector of the Kalman filter. As explained in section III, the Kalman filter estimates an additive quaternion error instead of Euler angle errors or its quaternion form. Gyroscope measurement is not directly processed but it is transformed into a quaternion representing an infinitesimal rotation. The second innovation is the magnetic angular rate update (MARU) with the quaternion parameterization. This update is available indoors, frequently applied and able to mitigate gyroscope bias. Finally, last novel aspect is the use of the accelerometer gradient to reduce the error propagation and estimate accelerometer bias components. Classically, the earth gravity field is considered as a local field reference when the body frame is in a static phase, but the accelerometer signal can be further exploited. Indeed, without lever arm between the inertial sensors, a hand rotation centered on the accelerometer is observable using the accelerometer signal gradient, similarly to the magnetic field angular rate update. III.

QUATERNION BASED ATTITUDE ESTIMATION METHOD

This part describes the algorithm using the three innovative methods, previously introduced, to estimate the attitude of the body frame, i.e. the hand’s attitude, with respect to the navigation frame. The data used to estimate the heading are provided by an IMU composed of a tri-axis accelerometer, a tri-axis magnetometer and a tri-axis gyroscope. The attitude is modeled by quaternion. Details about the quaternion algebra are available in the literature [12] and recalled in Annex I for the completeness of this paper. Quaternion based attitude estimation is known for to keeping some non-linear components, but globally the quaternion parameterization can be reduce non-linearity issues. The proposed algorithm is based on an Extended Kalman Filter (EKF) whose design is now detailed. Section III.A describes

the sensor models used in this filter. Sections III.B and III.C derives all matrices involved in the EKF. A. Sensor signal modelling 1) Global sensor noise modelling A mathematical model has been derived for each sensor signal: acceleration, angular rate and magnetic field. An Allan variance study was conducted for identifying the noise terms and their parameters. A specific model is adopted for each sensor. a) Gyroscope signal can be modelled by:

y bg  ωibb  b  n 





where yg is the three dimensional gyroscope signal; ωib is the true angular rate of the body frame with respect to the inertial frame, expressed in the body frame and bω is the gyroscope time drift, which is modeled as a random walk: b

b  nb 





nb is a zero-mean Gaussian white-noise process with variance  b2 . nω is a zero-mean Gaussian white noise process with variance  2 . The angular rate ωib can be written as: ωin  ωnb , with b

b

b

ωbnb the angular rate of body frame with respect to the local b frame expressed in the body frame and ωin the earth angular rate expressed in the body frame. This value (7.29 10-5 rad/sec), being insignificant compared to the dynamic of the body, it can be assumed that it is included in the gyroscope drift bω. b) The magnetometer is first calibrated for removing all magnetic perturbations produced by the hosting platform [5]. It includes also the two components of the magnetic deviation: the soft and hard iron effects. Only a white-noise remains after calibration.

y bm  mb  nm 





where ym is the local magnetic field vector mb expressed in body frame. It includes the earth magnetic field and the local disturbances. nm is a zero mean Gaussian white noise process described by the variance  m2 . c) The accelerometer signal ya is modeled as:

yba  aibb  ba  na 

 b



where aib comprises two parts: the acceleration and the earth gravity field. ba is the accelerometer time drift modeled with a random walk stochastic process: b a  n ba . (9)

nba is zero mean Gaussian white noise and variance  b2 . na a

is a zero mean Gaussian white noise with variance  a2 . 2) Quaternion parametrization of the gyroscope signal modeling. In this paragraph a new approach for processing gyroscope data is proposed where angular rates are not directly used. Instead they are transformed into small rotations between two successive epochs through the integrating equation (54) of Annex I. The gyroscope quaternion is then defined with a unit quaternion:



qyg

 yg  cos( T) 2   y y  sin( g T ) g  2 yg 



b

ωbnb is the angular rate of the body frame with respect to navigation frame expressed in the body frame at the epoch t. It is considered constant during the time interval [t,t+T[. This quaternion corresponds to the true rotation of the body frame between two successive epochs such as:

0, v (t)  q (t)  0, v (t  T )  q (t)  

 



b

where (0,v (t)) is the quaternion form of a three dimensional vector v in the body frame at epoch t. Derivation of this form is detailed in (52) in the Annex. vb(t+T) is the same expression at epoch t+T.  is the quaternion multiplication defined in (44) 

and q is its conjugate form, defined in (43). The novelty of the proposed approach is to address the gyroscope errors directly in the quaternion space. Same model as (5) is adopted and the parameters are derived from an Allan variance analysis.

qy g  q  bq  nq 





where qω is the true rotational quaternion. bq is the gyroscope quaternion drift modeled by:

bq  nbq . 

n bq



(13)

and nq are zero mean Gaussian white noises defined by

2 the variances  b2q and  q . 

n

n b

b

n* b





The state vector is composed by three unknown parameters:



(15)

where bq is the gyroscope quaternion bias and ba is the accelerometer bias. The latter is correlated to q bn . The additive state error is defined from (15) by:

where ωnb replaces yg.



0, v   q  0, v   q





approximation of true rotational quaternion qω of the body frame between two successive epochs: t and t+T. The true rotational quaternion is defined with an equation similar to (10)

b

1) State vector Let q bn be the quaternion representing the rotation of the body with respect to a navigation frame. This quaternion completely describes the orientation of body frame with respect to navigation frame and is defined by:

x  qbn , bq , ba ,

       

T is the sampling period. The gyroscope quaternion q yg is an

b

B. Dynamic state propagation





δx  δqbn , δbq , δba 





δq bn is the attitude quaternion error. It links the true attitude quaternion q bn to the estimated attitude quaternion qˆ bn by:

qbn  qˆ bn  δqbn 





The error δbqω and δba are defined similarly by:

ˆ  bq  bq  δbq ,  ˆ  ba  ba  δba where bˆ q and

(18)

bˆ a are the estimated gyroscope quaternion and

accelerometer biases. 2) Error state propagtion model. The evolution of a rotation modeled by a quaternion is given by the following differentiel euqation:

qbn 



1 n qb   0, ωbnb   2



Its derivation is explained in (54) in Annex I. Because the angular rate is not exactly known, the estimated rotational quaternion 

qˆ 

is defined by:

qˆ   q y g  bˆ q 



Equations (9), (13) and (19) fully describe the evolution of the state vector. With a 1st order development, the error state model becomes: 

δx  t  T   Fδx(t )  n(t ) 



where F is the dynamic matrix defined by:

n(t) is the state vector noise. M and C are quaternion matrices defined in (48) and (49) in Annex I.

2) Magnetic angular rate update The magnetic angular rate update is applied when a quasistatic period of the magnetic field is detected. This means that the magnetic field remains constant or undergoes very small variations over the period of interest. Two update steps can be achieved thanks when a quasi-static field is detected. The first one links the magnetic field error, expressed in the navigation frame, to the quaternion error. The second update is applied to the magnetic field time evolution and links the magnetic angular rate to the gyroscope quaternion bias error.

C. Inertial bias mitigation with with magnetometer and accelerometer signalsfrom an handheld IMU

a) Quasi static magnetic field (QSF) error model. Let us consider the kth quasi-static period with its reference magnetic



 C(qˆ  ) M(qˆ bn ) 0  F   0 I 0    0 0 I  



1) Static period threshold detection. All updates are related to opportune static periods that must first be detected. They correspond to time intervals during which the magnetic field or the acceleration field are quasistatic, i.e. about constant, in the navigation frame. Main challenge comes from the fact that the sensors data are expressed in the body frame, which is continuously moving with respect to the navigation frame. A specific static period detector has been derived for analyzing on-the-fly raw IMU data. The quasi-static phase detector studies the deviation of the norm for each field sensed during a defined period. This detector is based on the axiom stating that there is only one isometric transformation between the data expressed in the navigation and the body frames. Consequently, a static period is detected if: 

1 N

 f t

b



2

(k )   (t )   

k  t  NT



b

where N is a fixed integer number. f (k ) is either the magnetic field or the acceleration field measured at the epoch k , expressed in the body frame.  is a static period threshold detector defined a-priori during a calibration phase.  is defined by:

 (t ) 



1 t f b (k )   N k t  NT



If the condition (23) is true, a magnetic or acceleration field is computed and expressed in the navigation frame as

n field noted mrefk . At each epoch of this quasi-static period, the

update consists in comparing this reference with the magnetic field measurements ym. This magnetic field is compared with the reference field with: 

0,  zm   0, mnref

k

  qˆ

n b

 (0, y bm )  qˆ bn 



b) Magnetic angular rate update (MARU) model. During the kth quasi-static magnetic field period, the evolution of magnetic field expressed in the body frame is used to ˆ b at t+T in the compute the expected magnetic field m quaternion form: 

0, mˆ (t  T )   qˆ b



(t )   0, y bm (t )   qˆ  (t ) 



since qω is assessed as constant over the interval [t,t+T[ of the QSF period. The estimated magnetic field extracted from (28) is compared to the measured magnetic field:

ˆ b (t  T )   zmMARU  ybm (t  T )  m





The state vector δx is linked to the innovations issued from the magnetometer by the innovations (27) and (29) with:

0 0   z m  h1 (qˆ bn , mnrefk )   x  nm   z  ˆ b ) 0 0 h1 (qˆ ω , m  mMARU  



where the function h1 is defined for a quaternion and a vector t

1 f (t )  f n (k ) .  N k t  NT n ref

(25)

Next issue consists in determining if the following values belong to the detected static period:

 f (t  kT )  f b

n ref

(t )



(q, v)  ( ,

3

) with q  (q0 , u) by the following equation:

h1 (q, v)  2 q0 v  u  v (uT v)I33   q0v  u  v     

 q0 v  u  v   is the skew matrix of vector  q0 v  u  v  . I3x3 is the identity matrix and

2

 ,

where fb(t+kT) is the field at next epoch with k>0.

(26)

3) Acceleration gradient update (AGU) The acceleration gradient update is applied when a static period of the acceleration field is detected. Two updating steps are performed during the static phase. The first one links the acceleration field error to the quaternion error and the accelerometer bias. The second one is applied to the acceleration field time evolution and links the acceleration gradient to the gyroscope quaternion bias error and the accelerometer bias. The updating principle of AGU is similar to the one of the MARU method. The second update can only be applied if there is no lever arm between the accelerometer and the gyroscope tri-axis. a) Acceleration error model. Let a static period noted k n with its reference acceleration field noted a refk . At each epoch

of the QSF period, the update consists in comparing the reference acceleration with the estimated acceleration, both in quaternino form. 

0,  za   0, a

n refk

  qˆ

n b

 (0, y  bˆ a )  qˆ b a

advantage deals with the hypotheses made for linearizing the equations and the inherited approximations. A. A higher confidence level. Numerical values of the gyroscope angular rate and quaternion variances have been estimated with Allan variance analysis. As theoretically expected, it is observed that the magnitude of the quaternion variance is much smaller than the one of the angular rates. Consequently, the precision matrix is composed by higher values when the quaternion-based algorithm is applied as compared to the DCM-based algorithm is used, which implies higher confidence levels on the estimated attitude angles. B. Impact of signal error propagation. Using the gyroscope unit quaternion defined in (10), the propagation of any gyroscope perturbation δyg gets reduced. It is mathematically explained by the following equation.

 qy  A   y g ,

(37)

g

n b 



where  q yg is the gyroscope quaternion error and the matrix A is defined by:

b) Acceleration gradient update model (AGU). During the static acceleration field period, the evolution of acceleration field in the body frame is used to estimated the acceleration at t+T. 

(0, aˆ b (t  T ))  qˆ ω (t )  (0, aˆ b (t ))  qˆ *ω (t ) 



The estimated acceleration field from (33) is compared to the reference one by: 





 zaAGU  y (t  T )  bˆ a  aˆ (t  T )  b a



b

The state vector δx is linked to the innovations (32) and (34) by:



  z a   h1 qˆ bn , anrefk  z  0  a AGU  



h2 (qˆ bn )   x  h1 (qˆ  , aˆ b ) h2 (qˆ  )  0





na

The function h2 is defined for a quaternion q 

(

being

the quaternion space) with q  (q0 , u) and 

h2 (q)  (q02  u )I33  2u  uT  2q0 u   2

IV.

u uT  T    sin( )  2 u (38)  2 . A T  T   u u  T sin( ) I  uu   T cos( ) uu  33 2  u 2  2 u2 u  2   u results from the integration of the angular rate vector over the sampling period T. A is a contracting matrix that reduces the vector error and is proportional to the sampling period T.

The ability of A to reduce the propagation of gyroscope perturbation when T increases is demonstrated in (38), where a direct relationship with the time integration is observed. C. Impact of linearization steps. A reduced impact of linearization is another benefit of the quaternion parameterization. With DCM-based algorithms, the error term is an additive Euler angle component. In the DCM form, this error is transposed into an angle error matrix that is multiplied to the estimated DCM. Expressing the rotation matrix with Euler angles introduces non-linear components that dilute the precision on the estimation of angle errors. This linearization step is now mathematically shown in the following equations. n

First equation links the true DCM Cb to the estimated one.  

ˆ n  ECn  C b b

ˆn C b 

ALGORITHMS PERFORMANCE

In this part, a theoretical analysis of the algorithm advantages is performed in comparison with DCM-based attitude estimation algorithm. A first benefit of the quaternion parameterization comes from the error propagation, which is bigger with Euler angles than quaternions. The second

E is a DCM formed with the attitude angles errors, such as: 



E  DCM    

T





A 1st order Taylor series expansion of (40) concludes the linearization process. 

T Cˆ bn  I         

V.



EXPERIMENTAL VALIDATION

This part presents the experimental validation of the quaternion based heading estimation, which is conducted in the facilities of IFSTTAR in Nantes.

the corridors. Inside the building, no accurate heading reference is available but a-priori walking directions based on the straight geometry of the corridors (Fig. 3) and the 90° turns are considered as a reference to evaluate the performances of the quaternion based attitude estimation algorithm. The walking path of the three test subjects is shown in the same Fig. 3. Dark green lines correspond to the outdoor paths generated with the GNSS differential post-processed solutions. The red paths correspond to the indoor footpaths. The entire trajectory of one test subject is composed by a series of specific footpaths.

A. Description of the experiment Three people, labelled M1, W1 and W2, are equipped with a handheld device recording raw MEMS data. This handheld device, shown in Fig. 2, can be considered as an exploded form of a smartphone. It is composed of a tri-axis accelerometer, a tri-axis gyroscope and a tri-axis magnetometer. All sensors share the same reference frame. Indeed they are grouped in a unique component that is the ten degrees of freedom ADIS16407 from Analog Device [13]. During all experiments, the handheld device is kept pointing toward the walking direction. For each run, the person is evolving in three different surroundings that define three analysis schemes. 1) The first part takes place outdoor. It comprises the calibration and initialization phases. The calibration mitigates the magnetometer errors following the method described in [5]. An underlying hypothesis of this calibratioon is that the magnetometer measure the Earth magnetic field without external magnetic perturbations. Consequently, it was performed outside in a clean environement (in terms of magnetic field perturbations). This period is followed by a static phase that initializes the attitude of the body frame with respect to the navigation frame.

Fig. 2. Handheld device used during the experiment

2) The second part comprises an outdoor walking path for which a reference measurement system is providing the “true” trajectory. As shown in Fig. 3, each person is walking outdoors around the parking lot of the building. The reference measurement system consists in a DL-V3 dual frequency GNSS receiver from NovAtel, carried in a backpack and connected to an AntCom G5 GNSS antenna located on the pedestrian’s cap, and a DL-V3 base station receiver. The reference trajectories of all test subjects were post-processed in a phase GNSS differential mode using GrafNav software from NovAtel [14] achieving a mean 2 cm accuracy. The GNSS reference trajectories are only used to provide the pedestrian’s walking heading reference for assessing the performance in the outdoor environment. It is important to underline that the novel algorithm proposed in this paper only uses handheld MEMS signals to estimate the heading. 3) Last part of the test is located indoors, where the reference measurment system is not available but the building map is. Each person repeated several times the same loop in

Fig. 3. Walking paths of M1, W1 and W2

B. Performance assessment of estimated attitude angles Because the users were asked to walk with the device pointing toward the walking direction, the heading of the handheld device corresponds to the azimuth angle. Its values are brought back to the interval [-180°, 180°].

The “true” headings, which are given by the differential GNSS post-processed solutions, are also depicted in these figures in blue. 150

Walking Heading (°)

After the static phase, the test subject performed a 1st outdoor walk followed by a 1st indoor walk, a 2nd outdoor walk, a 2nd indoor phase. Finally a 3rd outdoor walking phase ends the test run. Transition between indoor and outdoor environments is also used to assess the algorithm performances thanks to the periods when GNSS signals are recovered. The entire walking distance for each test subject varies between 0.9 and 1.1 km, which corresponds to eleven to fifteen min.

The quaternion based heading estimate is shown in red whereas the building corridors main directions are shown with diverse colors of blue. Using the corridors main directions as indoor reference is less accurate than using the GNSS one. Indeed, human indoor walking is not strictly following a unique direction. A ±5° precision on each corridor direction can be assumed. Globally, the headings are correctly estimated along all runs. A 5° mean error is achieved after 2.5 loops for all test subjects. The worst performance is obtained for W2, where a small drift is observed: 10°. A thorough analysis of this dataset showed that this larger error is linked to a worse magnetic calibration during the initialization phase. This underlines the dependence of the attitude estimation on both the initial parameters of the EKF and the calibration of the hosting platform magnetic influence. Another consideration comes from the fact that the indoor references are not as accurate as the ones from the outdoor GNSS post-processed solutions. 3) Comparison between the GNSS reference heading and the DCM and quaternion based heading estimates respectively in the outdoor surroundings. The performances of the quaternion based EKF (in red) are compared in Fig. 10, Fig. 11 and Fig. 12 with those of the DCM algorithm (in green).

50 0 -50

-150 120

140

160

180 Time (s)

200

220

240

Fig. 4. Walking heading estimated with the quaternion algorithm and compared to the GNSS reference for W1 in 1st outdoor phase

GNSS Quaternion

Walking Heading (°)

150 100 50 0 -50 -100 -150 60

80

100 120 Time (s)

140

160

Fig. 5. Walking heading estimated with the quaternion algorithm and compared to the GNSS reference for W2 in 1st outdoor phase

150

Walking Heading (°)

2) Indoor assessment using corridors main directions. Fig. 7, Fig. 8 and Fig. 9 show the walking directions estimated inside the building using the quaternion based EKF. They have been grouped together on each figure for easing the performance analysis. Each group corresponds to the 2.5 indoor loops that are shown in Fig. 3. They can easily be identified with the series of four heading jumps corresponding to the main cooridors directions. Discontinuities are issued from the transition between 180° and -180°. One loop corresponds to 110 meters.

100

-100

1) Comparison of GNSS and self-contained MEMS based headings in the outdoors. Fig. 4, Fig. 5 and Fig. 6 show, in red, the walking heading estimated with the quaternion based algorithm and, in blue, the GNSS coarse over ground heading. No angle drift is observed during this first phase of all runs since both headings overlap for all figures. This assesses the good functioning of the proposed quaternion based heading estimation algorithm over approximately 170 m of walk.

GNSS Quaternion

GNSS Quaternion

100 50 0 -50 -100 -150 40

60

80 100 Time (s)

120

140

Fig. 6. Walking heading estimated with the quaternion algorithm and compared to the GNSS reference for M1 in 1st outdoor phase

The results in Fig. 11 and Fig. 12 are shown after a 10 min walk, which corresponds to approximately 900 m. Fig. 10 shows the algorithm performances after 14 min of walk corresponding to 1.1 km. Small oscillations can be observed in all figures. They come from the shoulders’ oscillations during the walk. To mitigate this effect in the comparison of the diverse algorithms performances, an average value on the total angular error is computed. Fig 11 shows a higher angular drift than Fig. 10 and Fig. 12 with about 15° instead of 5°. The angular drift estimated with the DCM based algorithm is even more important with about 50° for W2. As mentioned previously, this is probably due to a worse magnetometer calibration for this dataset. Consequently, conclusions on the performance assessment with W2 dataset must be considered with precaution.

Walking Heading (°)

150 100 50 0 -50 -100 -150 Quat

400

-4°

450

86°

-94°

500 Time (s)

176°

550

600

Fig. 7. Walking heading from quaternion algorithm compared to building map reference for W1 in 2nd indoor phase

In Fig.10 and Fig.12 the walking direction error between the GNSS reference and the quaternion based estimate is about 5° whereas it is between 10 and 15° for the DCM based algorithm. Clearly, the quaternion based attitude estimation EKF outperforms the DCM one. This result further confirms the theoretical assessment given in section IV, where the advantages of using quaternion algebra on small numerical values over the DCM and Euler parameterization are explained.

100

175

50

Walking Heading (°)

Walking Heading (°)

150

0 -50 -100 -150 Quat

300

-4°

350

86°

-94°

400 Time (s)

176°

450

DCM

GNSS

Quaternion

150 125 100 75 50 25

500

0

Fig. 8. Walking heading from quaternion algorithm compared to building map reference for W2 in 2nd indoor phase

830

840

850 Time (s)

860

870

Fig. 10. Walking heading from quaternion and DCM algorithm compared to building map reference for W1 in 3rd indoor phase

DCM

175

100

Walking Heading (°)

Walking Heading (°)

150

50 0 -50 -100

GNSS

Quaternion

150 125 100 75 50 25

-150 Quat

350

176°

86°

400 Time (s)

-4°

450

0

-94°

500

Fig. 9. Walking heading from quaternion algorithm compared to building map reference for M1 in 2nd indoor phase

595

600

605

610 615 620 Time (s)

625

630

Fig. 11. Walking heading from quaternion and DCM algorithm compared to building map reference for W2 in 3rd indoor phase

[5]

DCM

175

GNSS

Quaternion [6]

Walking Heading (°)

150 125

[7]

100 75

[8]

50 25

[9]

0 600

610

620 Time (s)

630

640

Fig. 12. Walking heading from quaternion and DCM algorithm compared to building map reference for M1 in 3rd indoor phase

[10]

[11]

[12]

VI.

CONCLUSION

The quaternion based attitude estimation algorithm proposed in this paper outperforms existing DCM based attitude estimation algorithm. The benefits have been shown both in the theoretical and experimental domains with handheld sensors. First advantage is related to numerical aspects. The quaternion error is smaller than the corresponding angular error. Consequently, the hypothesis supporting the EKF linearization steps in the Jacobean matrix are better satisfied with the quaternion parameterization and the residual errors become of negligible values. Second advantage is inherited from the quaternion structure. The quaternion gyroscope is defined using the quaternion algebra and its properties, which reduce the linearization impact of the error propagation with only one second order term. The experimental assessment with three test subjects over a 1 km mean trajectory shows that the proposed algorithm successfully mitigates the low cost sensors drifts. Globally, the error on the heading estimates remains below 7° after 1 km for all test subjects. Next step will be to confirm these promising results with additional dataset and by combining the latter with the second element of pedestrian dead-reckoning navigation solution, namely a step length model for handheld device.

[13] [14]

ANNEX 1 Another way to represent and parameterize a rotation consists in using quaternions. Quaternion space and its corresponding composition laws form a fourth dimensional non-commutative real algebra. A quaternion q can be written as: q =  q0 , q1 , q2 , q3  with  q0 , q1 , q2 , q3   , the set of realquadruplets. This space can be considered as an extension of 4

complex numbers space with a form: q   q0 , q v  . q0 is the scalar part and qv   q1 , q2 , q3  is the imaginary or the vector part. The quaternion can be rewritten as: 

[1]

[2]

[3] [4]

V. Renaudin, M. Susi, and G. Lachapelle, "Step Length Estimation Using Handheld Inertial Sensors," Sensors, vol. 12, pp. 8507-8525, 2012. M. Susi, V. Renaudin, and G. Lachapelle, "Motion Mode Recognition and Step Detection Algorithms for Mobile Phone Users," Sensors, vol. 13, pp. 1539-1562, 2013. J. A. Farrell, Aided Navigation: GPS with High Rate Sensors: McGrawHill Companies, 2008. C. Eling, P. Zeimetz, and H. Kuhlmann, "Development of an instantaneous GNSS/MEMS attitude determination system," GPS Solutions, pp. 1-10, 2013

q  q0  iq1  jq2  kq4 



where i, j , k are imaginary numbers. A conjugated number, i.e. complex, can be associated to each quaternion: 

REFERENCES

V. Renaudin, M. H. Afzal, and G. Lachapelle, "Complete Triaxis Magnetometer Calibration in the Magnetic Domain," Journal of Sensors, vol. 2010, 2010. D. Roetenberg, HJ. Luinge, CT. Baten, and P. Veltink, "Compensation of magnetic disturbances improves inertial and magnetic sensing of human body segment orientation.," IEEE Trans Neural Syst Rehabil Eng. , vol. 13, pp. 395-405, 2005. V. Renaudin, H. Afzal and G. Lachapelle, "Magnetic Perturbations Detection and Heading Estimation Using Magnetometers," Journal of Location Based Services, 2012. J. Haverinen and A. Kemppainen, "Global indoor self-localization based on the ambient magnetic field," Robotics and Autonomous Systems, vol. 57, pp. 1028-1035, 2009. W. F. Storms, "Magnetic Field Aided Indoor Navigation," Master of Science thesis, Electrical and Computer Engineering, Air Force Institute of Tehnolcogy, 2009. D. Vissiere, "Guidance, Navigation and Control Solutions for Unmanned Heterogeneous Vehicles in a Collaborative Mission," Part III, PhD thesis, Mathematic and Automatic, Ecole des Mines de Paris, 2008. M. H. Afzal, V. Renaudin, and G. Lachapelle, "Use of earth's magnetic field for mitigating gyroscope errors regardless of magnetic perturbation," Sensors, vol. 11, pp. 11390-11414, 2011 J. B. Kuipers, "Quaternion and rotation sequences," in Geometry, Integrability and Quantization, Varna, Bulgaria, pp. 127-143, 1999. ADIS16407 Product Data Sheet, www.analog.com/static/importedfiles/data_sheets/ADIS16407.pdf, last accessed March 2014. www.novatel.com/products/software/grafnav/, last accessed March 2014.

q   q0 , u  



where ‘*’ is the conjugation operator. The intern multiplication, noted  , is defined for a   a0 , a v  and

b   b0 , b v  , which are two quaternions:



a  b  (a0b0  a vT b v , a0 b v  b0 b v  av  b v )





where ‘ . ’ is the scalar product and ‘  ’is the cross product. As shown in Table 1, the intern multiplication can be also defined using complex number multiplication. TABLE I.

MULTIPLICATION FOR COMPLEX NUMBER



i i 1

1 1 1 i i

where a  a0 a1 a2 a3 

TABLE II.

1 1 1 i i

i i

j

k

j

k

1

k

j

j

j

k

i

k

k

j

1 i

1

q3  . T

q1 q2

a1

 a2

a0

a3

a3

a0

 a2

a1



b3

b0

b2

b1

(49)

T

b2

b0

b3

b3

b0

b2

b1

q0 2  q12  q2 2  q32 

(45)

a3  b0  a2   b1    a1  b2     a0   b3 

b3   a0  b2   a1      b1   a2    b0   a3 



u  1 and     ,   . With these properties, the

rotation operator, in the third dimension, using quaternion is defined by:

 0, x    cos(2 ),sin(2 )u    0, x    cos(2 ),  sin(2 )u    b

where

a

 , u  are the angle and the Euler axis of the rotation

from the frame a to the frame b. The quaternion forms of the vector x expressed in frames b and a respectively are given by: 

 0, x  ,  0, x  



b

simplify

notation,



a

the

rotation

quaternion

    b  cos( ),sin( )u  is noted q a . As shown in Fig. 13, it 2 2   represents the rotation of the frame a with respect to the frame b. 

In this form each four dimensional matrix is noted:



and

 a0 a M ( a)   1  a2   a3

. The



q   cos( ),sin( )u  

with

To

b1

q 



or

 b0 b ab   1 b2   b3

b3

When q  1 , the quaternion is given by:

Using this form, the intern multiplication is expressed with the product of matrices.

 a0 a ab   1  a2   a3

b0

b3  b2  , b1   b0 

The quaternion norm is the same as the one derived from the classic scalar product in 4 : 

A quaternion q   q0 , q v  can also be expressed with a column vector form:

q  q0

b2

latter are the two quaternions, a   a0 , a v  and b   b0 , b v  , in their column form.

MULTIPLICATION FOR QUATERNION VECTOR



b1

and b  b0 b1 b2 b3 

T

Table 2 summarizes the quaternion multiplication using the complex representation. To read the table, the first element of multiplication corresponds to the element of the row.



 b0 b C ( b)   1 b2   b3

a1

 a2

a0

 a3

a3

a0

 a2

a1

 a3  a2   a1   a0 



Fig. 13. Illustration of a rotation with quaternion parameters

The quaternion derivation can simply be expressed with:

qba 



where

qba

1 b a q a   0, ωba   2 b





is the derivative of quaternion q a . 0, ωba a

 is the

quaternion form of ω , the angular rate vector of the frame a with respect to the frame b, expressed in the frame a. a ba