Delay Kalman Filter to Estimate the Attitude of a Mobile Object with Indoor Magnetic Field Gradients

Abstract

More and more services are based on knowing the location of pedestrians equipped with connected objects (smartphones, smartwatches, etc.). One part of the location estimation process is attitude estimation. Many algorithms have been proposed but they principally target open space areas where the local magnetic field equals the Earth’s field. Unfortunately, this approach is impossible indoors, where the use of magnetometer arrays or magnetic field gradients has been proposed. However, current approaches omit the impact of past state estimates on the current orientation estimate, especially when a reference field is computed over a sliding window. A novel Delay Kalman filter is proposed in this paper to integrate this time correlation: the Delay MAGYQ. Experimental assessment, conducted in a motion lab with a handheld inertial and magnetic mobile unit, shows that the novel filter better estimates the Euler angles of the handheld device with an 11.7° mean error on the yaw angle as compared to 16.4° with a common Additive Extended Kalman filter.

1. Introduction

Many smart connected objects have been developed for the internet of things (IoT). They are meant to facilitate daily life activities with many services that are based on embedded sensors. Among these services are geolocation of lost objects, home automation services or monitoring human well-being. These sensors are primarily made of telecommunication chipsets and inertial micro electro-mechanical sensors (MEMS): a tri-axis accelerometer and tri-axis gyroscope. Tri-axis magnetometers are more and more available in connected objects. Indeed, whereas several years ago, hardware manufacturers had to mount separated triads of accelerometers and gyroscopes on printed circuit boards, they are now commonly available in a single 9-degrees-of-freedom magnetic and inertial mobile unit (MIMU), which consists of calibrated and co-aligned triads of accelerometers, gyroscopes and magnetometers. As they are increasingly available in everyday objects, there is an increasing interest in the development of methods to process the magnetometer measurements and propose novel IoT personal applications.

Historically, magnetometer and compass records were processed to estimate the direction toward the Earth North Magnetic Pole. As illustrated in Figure 1, this direction is changed into the azimuth angle, i.e., angular direction toward the True (geographic) North, using the local deviation angle extracted from geographical tables. This popular method can only be applied if the measured magnetic field vector corresponds to the Earth Magnetic Field. Indeed, when the magnetometer measurement is perturbed, the estimated direction points toward the local magnetic perturbation source instead of the North Pole [1]. Consequently, this method is only valid in environments where there is no artificial source of magnetic field. This is typically the case in open outdoor spaces, but not in the urban and indoor spaces where most IoT applications are expected to function. To overcome this limitation, research has been conducted to invent novel processing strategies of magnetic field measurements for urban/indoor surroundings. Three main different approaches have been proposed to process magnetometer data even when they are perturbed by artificial sources of magnetic field:

• Magnetic field fingerprinting (FP);
• Velocity estimation from spatial magnetic field gradients measured with an assembly of magnetometers;
• Attitude angles estimation from magnetic field gradients measured in time with a single tri-axis magnetometer.

1.1. Magnetic Field Fingerprinting (FP)

The first approach is based on the fingerprinting principle that was first developed to geolocate mobile objects using Wi-Fi receiver signal strength (RSS) data. In the context of magnetometer based positioning, instead of using Wi-Fi RSS, magnetic field amplitudes are used [2]. The method works as follows. In an offline phase, there is a map of indoor/urban magnetic field amplitudes. A database stores all amplitude fingerprints along with their geographical coordinates in the map. During the online phase, real time magnetic field measurements are compared with the database to extract the associated geographical positions.

The advantage of this method is that it directly provides the user’s location in the mapped environment and it is built on the diversity and complexity of indoor/urban magnetic fields to distinguish between different geographical positions. Similarly to Wi-Fi FP, this approach assumes that the measured urban/indoor magnetic fields are the same as the one measured to build the map in the offline phase. This is a strong assumption that is not always true. It is the for example the case when the smart object is close to an elevator lift or during emergency situations when all metal doors are automatically closed to prevent the spread of fire. In these situations, the performance will deteriorate.

1.2. Velocity Estimation from Spatial Magnetic Field Gradients Measured with an Assembly of Magnetometers

The second approach uses an array of magnetometers separated by known distances to measure the local spatial magnetic field gradient and derive the velocity estimate. The magnetic field space gradient is related to the derivative of the position and the magnetic field temporal derivate based on Biot-Savart laws [3]. A recursive process is then applied to track the mobile object in time using the successive velocity estimates and knowing the initial geographical coordinates of the magnetometer array.

The performance of this method strongly depends on the calibration of each individual magnetometer because they must all measure the same magnetic field, which can be very challenging when the hosting platform exhibits varying fields. The distance between each magnetometer is also critical to precisely measure the spatial gradient. Globally, the performance of this method increase in surroundings with large magnetic field anomalies, and the best performances are achieved in environments where many artificial sources of magnetic field are present. Because the magnitude of the magnetic field rapidly drops when the separation between the artificial field source and the magnetometer array increases (it follows an inverse cubic law), on many occasions the use of gradients is not sufficient to compute good velocity estimates and complementary methods are needed.

1.3. Attitude Angles Estimation from Magnetic Field Gradients Measured in Time by a Single Tri-Axis Magnetometer

The third approach computes the magnetic field gradient based on magnetometer measurements that are recorded at different epochs while the mobile object is moving. When the local magnetic field is assessed as static over two consecutive epochs, it is used to observe the rotation of the mobile object between the two epochs. The angular rate of the dynamic object is then derived from the successive quasi static magnetic field records [4]. Then, a recursive process estimates the attitude angles of the connected object using the angular rate estimates and the known initial attitude angles. In this approach requires only one tri-axis magnetometer.

When the local magnetic field is fluctuating over time (e.g., power on of an IT object, a moving elevator, etc.), this approach cannot be applied. To mitigate this issue, magnetically derived angular rates are merged with gyroscopes angular rates and accelerations. This hybridization enables continuous tracking of the orientation of the MIMU. It provides also a continuous calibration of the large accelerometer and gyroscope errors that are inherent to the low cost nature of MEMS. Existing attitude estimation filters are principally based on the Extended Kalman Filter (EKF) with a quaternion parameterization of the attitude angles. Quaternion parameterization has also been recently proposed to model the gyroscope signal [5]. Three popular hybridization filters have been tested for manual and locomotion tasks performed by six human subjects [6]. A comparison of four main hybridization filters of MIMU signals has also been recently conducted in a motion laboratory [7]. Despite good performances (several degrees in average), they show that magnetic anomalies are still a challenge for many filters.

1.4. Challenge of Attitude Estimation Filters Based on Magnetic Fields Recorded at Different Epochs

Existing filters, which are based on the magnetically derived angular rate principle, merge magnetic field data that have been recorded at different epochs: at the beginning of the quasi static magnetic field period (t₀) and at the epoch of state’s computation (t_k). However they do not consider the fact that the orientation of the mobile object is different at the two epochs 0 and k, which limits the solution. They also do not consider the correlation between the two epochs and the quality of the orientation estimate at the beginning of the quasi static period. This approximation is made in existing filters where averaged data and instantaneous data are fused to estimate the attitude at epoch k.

These limitations are addressed in this article with a novel delay EKF, which is called the Delay Magnetic, Acceleration Fields and Gyroscope Quaternion (Delay MAGYQ) and considers the states’ correlation over the quasi-static period of magnetic field. The novelty is that this algorithm considers the quality of the magnetic field estimated at the beginning of the quasi static period, which serves as a reference for deriving the angular rates, and its correlation with the current epoch to estimate the orientation angles of the mobile object and its variance. The past MAGYQ filter does not consider this time correlation to estimate the attitude angles.

2. The Delay MAGYQ Attitude Estimation Filter

2.1. MEMS Signals Modelling

A signal model is introduced for each accelerometer, gyroscope and magnetometer measurement. These mathematical models are essential to represent the measurement errors that are known to be very large with MEMS and must be mitigated in the attitude estimation process.

2.1.1. Magnetometer Signal Model

The tri-axis magnetometer senses the local magnetic field vector. In order to measure only the surrounding magnetic field and not the one produced by the hosting platform, it must first be calibrated. Soft and hard iron errors are estimated along with other deterministic errors due to the fabrication process [8,9]. Once the magnetometer is calibrated, it can be modeled by:

$$y_m=m^b+n_m$$

(1)

where $y_m$ is the magnetometer measurement, $m^b$ is the local magnetic field expressed in the body frame (frame of mobile object) and $n_m$ is a white Gaussian noise (0, ${\mathsf{\sigma }}_m^2$).

2.1.2. Accelerometer Signal Model

The accelerometer measures the specific force experienced by the IMU. Its output is a vector that can be modeled by:

$$y_a=a_{ib}^b-g^b+b_a+n_a$$

(2)

where $y_a$ is the measured acceleration vector, $a_{ib}^b$ is the acceleration vector expressed in the body frame with respect to the inertial frame, $n_a$ is a white Gaussian noise (0, ${\mathsf{\sigma }}_a^2$) and $b_a$ is the accelerometer bias.

2.1.3. Gyroscope Signal Model

The gyroscope measures the angular rate of the MIMU in the body frame with respect to the inertial frame and some error terms. The gyroscope signal is modeled by:

$$y_g={\mathsf{\omega }}_{ib}^b+b_g+n_g$$

(3)

where $y_g$ is the gyroscope measurement, ${\mathsf{\omega }}_{ib}^b$ is the angular rate of the MIMU with respect to the inertial frame, expressed in the body frame, $n_g$ is a white Gaussian noise (0, ${\mathsf{\sigma }}_g^2$) and $b_g$ is the gyroscope bias.

Instead of considering the gyroscope’s signal as an angular rate, it can be expressed in terms of rotational elements. The exponential function of the quaternion is used to perform the change of state as follows:

$$\forall q\in ℍ,\exp \left(q\right)={\displaystyle \sum _{i=0}^{+\infty }\frac{{\left(q\right)}^i}{i!}}$$

(4)

The average angular velocity $\mathsf{\omega }$ between two epochs, corresponding to the time interval Δt, induces the rotation $q_{\mathsf{\omega },\mathrm{\Delta }t}$ defined by $q_{\mathsf{\omega },\mathrm{\Delta }t}=\exp \left(\frac{\mathrm{\Delta }t}{2}{\left(\mathsf{\omega }\right)}_q\right)$, with ${\left(\mathsf{\omega }\right)}_q={\left(\begin{array}{cc}0& {\mathsf{\omega }}^T\end{array}\right)}^T$ the quaternion form of a vector. Based on the quaternion set, a new gyroscope signal model is proposed:

$$q_{y_g,\mathrm{\Delta }t}=q_{\mathsf{\omega },\mathrm{\Delta }t}+b_{q_g,\mathrm{\Delta }t}+n_{q_g,\mathrm{\Delta }t}$$

(5)

$q_{y_g,\mathrm{\Delta }t}$ is the quaternion corresponding to the rotation sensed by the gyroscope between two consecutive epochs over the time interval Δt. $q_{\mathsf{\omega },\mathrm{\Delta }t}$ is the quaternion that represents the true rotation of the MIMU gyroscope between two successive epochs over the time interval Δt. $n_{q_g,\mathrm{\Delta }t}$ is a white Gaussian noise (0, ${\mathsf{\sigma }}_{q_g,\mathrm{\Delta }t}^2$) and $b_{q_g,\mathrm{\Delta }t}$ is the rotation bias introduced by the gyroscope.

3. Analysis of the Influence of Past Estimates on the Orientation Estimation with Magnetometer Measurements

3.1. Problem Statement

As introduced earlier, the first algorithms, which assume that the magnetometer records correspond to the Earth magnetic field, cannot be applied in urban and indoor surroundings. Indeed, the large time and space fluctuations of the magnetic field in these surroundings complicate the processing of magnetometer data. The goal is still to estimate the attitude angles of the MIMU in the magnetically perturbed environment. This is still possible using magnetic anomalies since they provide useful information about the orientation changes of the MIMU, especially when the local magnetic field is steady over a time interval. This period is the “quasi static field” (QSF) period and it is defined by:

$$T_{QSF}={\left(t_k\right)}_{0\le k\le N}\text{\hspace{1em}and\hspace{1em}}{m}_{ref}^n$$

(6)

where $m_{ref}^n$ is the steady magnetic field that is the reference vector for the period. The following observation equation is used to estimate the orientation error using the magnetometer data over the QSF interval.

$${\left(\mathsf{\delta }{m}_{t_k}^n\right)}_q={\left(m_{ref}^n\right)}_q-{\widehat{q}}_{b{t}_k}^n\otimes {\left(y_m\left(t_k\right)\right)}_q\otimes {\overline{\widehat{q}}}_{b{t}_k}^n$$

(7)

To estimate $\delta m_{t_k}^n$ in Equation (7), the reference magnetic field $m_{ref}^n$ must first be computed. It is defined at the beginning of the quasi static period $T_{QSF}$ from the magnetometer measurement at epoch $t_0$ by:

$${\left({\widehat{m}}_{re{f}_{T_{QSF}}}^n\right)}_q={\widehat{q}}_{b{t}_0}^n\otimes {\left(y_m\left(t_0\right)\right)}_q\otimes {\overline{\widehat{q}}}_{b{t}_0}^n$$

(8)

The reference magnetic field in Equation (8) is estimated from the measurements and the orientation estimate introducing two different errors. The first error comes from the magnetometer measurement that is associated to a white Gaussian noise. This error can be mitigated with a low pass filter. The second error comes from the orientation estimation of the MIMU at epoch $t_0$, which is not perfect.

Knowing the reference magnetic field from Equation (8), it is possible to estimate the innovation $\mathsf{\delta }{z}_k$, at epoch $t_k$ from Equation (7), which becomes:

$${\left(\mathsf{\delta }{z}_{t_k}\right)}_q={\left({\widehat{m}}_{re{f}_{T_{QSF}}}^n\right)}_q-{\widehat{q}}_{b{t}_k}^n\otimes {\left(y_m\left(t_k\right)\right)}_q\otimes {\overline{\widehat{q}}}_{b{t}_k}^n$$

(9)

3.2. Magnetic Field Based Observation Equation

Equation (9) is expanded using the reference field $m_{ref}^n$ associated to the QSF period.

$${\left(\mathsf{\delta }{z}_{t_k}\right)}_q={\left({\widehat{m}}_{re{f}_{T_{QSF}}}^n\right)}_q-{\left(m_{ref}^n\right)}_q+{\left(m_{ref}^n\right)}_q-{\widehat{q}}_{b{t}_k}^n\otimes {\left(y_m\left(t_k\right)\right)}_q\otimes {\overline{\widehat{q}}}_{b{t}_k}^n$$

(10)

This equation is linked to two error terms. The first one $\mathsf{\delta }{m}_{t_k}^n$ comes from the transformation of the magnetic field measured in the body frame into the navigation frame at epoch $t_k$. The navigation frame corresponds to the local mapping frame (East North Up frame). The second error term $\mathsf{\delta }{m}_{t_0}^n$ comes from the transformation of the magnetic field measured in the body frame into the navigation frame at epoch $t_0$. This second error term also corresponds to the error on the reference magnetic field estimate over the quasi-static period.

$$\mathsf{\delta }{z}_k=\mathsf{\delta }{m}_k^n-\delta m_0^n$$

(11)

The first error, which comes from the transformation of the magnetic field into the navigation frame $\mathsf{\delta }{m}_{t_k}^n$, can be related with a first order development to the error on the quaternion estimate $\mathsf{\delta }{q}_{b{t}_k}^n$by:

$$\mathsf{\delta }{m}_k^n=2\left[\begin{array}{cc}{A}_{t_k}& a_{t_k}Id-\left[A_{t_k}\times \right]\end{array}\right]\mathsf{\delta }{q}_{b{t}_k}^n+C_b^n\left({\widehat{q}}_{b{t}_k}^n\right)n_m$$

(12)

where $a_{t_k}=\langle {\widehat{q}}_{b{t}_k}^n,{\left({\widehat{m}}_{t_k}^n\right)}_q\rangle$ is the scalar product, $A_{t_k}=q_1{}_{t_k}{\widehat{m}}_{t_k}^n-\left[u_q{}_{t_k}\times \right]{\widehat{m}}_{t_k}^n$ with ${\widehat{q}}_{b{t}_k}^n={\left(\begin{array}{cc}{q}_1{}_{t_k}& u_{q{t}_k}\end{array}\right)}^T$, $C_b^n\left({\widehat{q}}_{b{t}_k}^n\right)$ is the rotation matrix calculated from the quaternion estimate ${\widehat{q}}_{b{t}_k}^n$, $\left[y\times \right]=\left(\begin{array}{ccc}0& -y_3& y_2\\ y_3& 0& -y_1\\ -y_2& y_1& 0\end{array}\right)$ is the antisymmetric matrix of the vector $y={\left(\begin{array}{ccc}{y}_1& y_2& y_3\end{array}\right)}^T$.

The innovation $\mathsf{\delta }{z}_{t_k}$, which is sensed by the magnetometer, can be related to the errors on the orientation estimation $\mathsf{\delta }{q}_{b{t}_k}^n$ and $\mathsf{\delta }{q}_{b{t}_0}^n$ by

$$\mathsf{\delta }{z}_k=H_{t_k}\mathsf{\delta }{q}_{b{t}_k}^n-H_{t_0}\mathsf{\delta }{q}_{b{t}_0}^n+M_{t_k,t_0}{n}_m$$

(13)

The Jacobean matrices in Equation (13) are given by:

$$H_{t_k}=2\left[\begin{array}{cc}{A}_{t_k}& a_{t_k}Id-\left[A_{t_k}\times \right]\end{array}\right],H_{t_0}=2\left[\begin{array}{cc}{A}_{t_0}& a_{t_0}Id-\left[A_{t_0}\times \right]\end{array}\right]$$

(14)

and

$$M_{t_k,t_0}=\left[C_b^n\left({\widehat{q}}_{b{t}_k}^n\right)+C_b^n\left({\widehat{q}}_{b{t}_0}^n\right)\right]$$

(15)

4. Design of the Novel Attitude Estimation Filter: Delay MAGYQ

The design of the novel attitude estimation filter Delay MAGYQ is based on [5]. The modification to integrate the delay induced by the use of an averaged angular rate is inspired by [10]. Other articles have also addressed the delay approach in Kalman Filter [11,12].

4.1. State Vector

The eleven parameters state vector is given by

$$x={\left[\begin{array}{ccc}{q}_b^n& b_{q_g,\mathrm{\Delta }t}& b_a\end{array}\right]}^T\in {ℝ}^{11}$$

(16)

where $q_b^n$ is the quaternion that describes the transformation of the MIMU between the body and the navigation frames, $b_a$ is the accelerometer bias introduced in Equation (2) and $b_{q_g,\mathrm{\Delta }t}$ is the gyroscope quaternion bias given in Equation (5).

4.2. Dynamic Evolution of the State Vector

The rotation $q_{\mathsf{\omega }{t}_{k-1},\mathrm{\Delta }t}$ between the epochs $t_{k-1}$ and $t_k$ is first estimated. The subscript Δt is omitted in the development of the following equations to ease the reading. ${\widehat{x}}_{t_k}={\left[\begin{array}{ccc}{\widehat{q}}_{b{t}_k}^n& {\widehat{b}}_{q_g{t}_k}& {\widehat{b}}_{a{t}_k}\end{array}\right]}^T$ is the state vector estimate at epoch $t_k$.

$$\{\begin{array}{c}{q}_{y_g{t}_{{}_{k-1}}}=\exp \left({\left(y_{g{t}_{{}_{k-1}}}\right)}_q\right)\\ {\widehat{q}}_{\mathsf{\omega }{t}_{k-1}}=q_{y_g{t}_{{}_{k-1}}}-{\widehat{b}}_{q_g{t}_{k-1}}\\ {\widehat{q}}_{\mathsf{\omega }{t}_{{}_{k-1}}}=\frac{{\widehat{q}}_{\mathsf{\omega }{t}_{{}_{k-1}}}}{\Vert {\widehat{q}}_{\mathsf{\omega }{t}_{{}_{k-1}}}\Vert }\end{array}$$

(17)

The quaternion $q_b^n$ is propagated using the gyroscope measurements. The biases $b_{q_g}$ and $b_a$ are modeled with random walks.

$$\{\begin{array}{l}{\widehat{q}}_{b{t}_k}^n={\widehat{q}}_{b{t}_{k-1}}^n\otimes {\widehat{q}}_{\mathsf{\omega }{t}_{k-1}}\\ {\widehat{b}}_{q_g{t}_k}={\widehat{b}}_{q_g{t}_{k-1}}\\ {\widehat{b}}_{a{t}_k}={\widehat{b}}_{a{t}_{k-1}}\end{array}$$

(18)

4.3. Dynamic Evolution of the Covariance Matrix

The covariance matrix $P_{t_k}$ is associated to the perturbation of state vector $\mathsf{\delta }{x}_{t_k}$. The time evolution of $P_{t_k}$ depends on the stochastic models chosen to describe the evolution in time of the biases $b_{q_g}$ and $b_a$. This choice impacts the Jacobian matrices $F_{t_{k-1}}$ and $G_{t_{k-1}}$ and the covariance matrix $Q_{t_{k-1}}$ associated with the system noise. They are given by

$$\{\begin{array}{l}{F}_{t_{k-1}}=\left(\begin{array}{ccc}{C}_{{\widehat{q}}_{\omega }{t}_{{}_{k-1}}}& -M_{{\widehat{q}}_{b{t}_{{}_{k-1}}}^n}& 0\\ 0& I& 0\\ 0& 0& I\end{array}\right)\\ G_{t_{k-1}}=\left(\begin{array}{ccc}-M_{{\widehat{q}}_{b{t}_{{}_{k-1}}}^n}& 0& 0\\ 0& T_{s}I& 0\\ 0& 0& T_{s}I\end{array}\right)\end{array}\Rightarrow P_{t_k}=F_{t_{k-1}}{P}_{t_{k-1}}{F}_{t_{k-1}}{}^T+G_{t_{k-1}}{Q}_{t_{k-1}}{G}_{t_{k-1}}{}^T$$

(19)

The matrices $M_q$ and $C_q$ are defined using the quaternion product, which is modified into a product of matrices by

$$q_1\otimes q_2=M_{q_1}{q}_2=C_{q_2}{q}_1$$

(20)

4.4 Time Evolution of the Covariance Matrix Involving Past Orientation Estimate

It is assumed that the orientation information sensed by the magnetometer depends on the attitude angles at epoch t₀. This orientation is labelled $q_{b{t}_0}^n$. The corresponding estimation error $\mathsf{\delta }{q}_{b{t}_0}^n$ is linked to the covariance matrix $P_{q,t_0}$. To integrate the time correlation of orientation estimates, the covariance matrix that relates $\mathsf{\delta }{q}_{b{t}_0}^n$ and $\mathsf{\delta }{x}_{t_k}$ must be computed. It corresponds to the correlation between past orientation estimate and the present one and is given by

$$P_{t_0}=\left[\begin{array}{cc}{P}_{q,t_0}& P_{\left(q,b\right),t_0}\\ P_{\left(q,b\right),t_0}{}^T& P_{b,t_0}\end{array}\right]$$

(21)

with $P_{q,t_0}$ is the covariance matrix of $\mathsf{\delta }{q}_{b{t}_0}^n$, $P_{b,t_0}$ is the covariance matrices of the biases and $P_{\left(q,b\right),t_0}$ is the covariance matrix between $\mathsf{\delta }{q}_{b{t}_0}^n$ and the biases.

We define also the product ${\mathrm{\Gamma }}_{t_0,t_k}={\displaystyle \prod _{p=0}^k{F}_{t_{p-1}}}$ and ${\breve{P}}_{t_k}$ the covariance matrix of the state vector perturbation ${\left[\begin{array}{cc}\mathsf{\delta }{x}_{t_k}{}^T& \mathsf{\delta }{q}_{b{t}_0}^n{}^T\end{array}\right]}^T$. Using previously introduced notations, this covariance becomes:

$${\breve{P}}_{t_k}=\left[\begin{array}{cc}{P}_{t_k}& {\mathrm{\Gamma }}_{t_0,t_k}\left[\begin{array}{c}{P}_{q,t_0}\\ P_{\left(q,b\right),t_0}{}^T\end{array}\right]\\ \left[\begin{array}{cc}{P}_{q,t_0}& P_{\left(q,b\right),t_0}\end{array}\right]{\mathrm{\Gamma }}_{t_0,t_k}{}^T& P_{q,t_0}\end{array}\right]$$

(22)

4.5. Update Equations

The Kalman Gain ${\breve{K}}_{t_k}$ is computed for ${\left[\begin{array}{cc}\mathsf{\delta }{x}_{t_k}{}^T& \mathsf{\delta }{q}_{b{t}_0}^n{}^T\end{array}\right]}^T$ using Equations (13) and (22).

$${\breve{K}}_{t_k}={\breve{P}}_{t_k}{\breve{H}}_{t_k}{\breve{S}}_{t_k}{}^{-1}$$

(23)

where ${\breve{H}}_{t_k}=\left[\begin{array}{ccc}{H}_{t_k}& 0& -H_{t_0}\end{array}\right]$, ${\breve{S}}_{t_k}={\breve{H}}_{t_k}{\breve{P}}_{t_k}{\breve{H}}_{t_k}{}^T+M_{t_k,t_0}{R}_m{M}_{t_k,t_0}{}^T$ with $R_m$ being the magnetometer measurement covariance matrix.

The error state estimate $\mathsf{\delta }{x}_{t_k}$ is updated by:

$$\mathsf{\delta }{x}_{t_k}=K_{t_k}\mathsf{\delta }{z}_{t_k}$$

(24)

where ${\breve{K}}_{t_k}=\left[\begin{array}{c}{K}_{t_k}\\ K_{q,t_0}\end{array}\right]$.

The update of the global covariance matrix ${\breve{P}}_{t_k}$ is classically computed:

$${\breve{P}}_{t_k}={\breve{P}}_{t_k}-{\breve{K}}_{t_k}{\breve{H}}_{t_k}{\breve{P}}_{t_k}$$

(25)

The update of the covariance matrix $P_{t_k}$ is performed:

$$P_{t_k}=\left(Id-K_{t_k}\left[\begin{array}{cc}{H}_{t_k}& 0\end{array}\right]\right)P_{t_k}+K_{t_k}{H}_{t_0}\left[\begin{array}{cc}{P}_{q,t_0}& P_{\left(q,b\right),t_0}\end{array}\right]{\mathrm{\Gamma }}_{t_0,t_k}{}^T$$

(26)

5. Performance Evaluation with Experimental Data

5.1. Experimental Setup

For the experimentation, the subjects hold an MIMU in hand. Two units were used: the ADIS-16488 from Analog Device and the VN-300 from VectorNav (Dallas, TX, USA). Both units comprise a tri-axis gyroscope, a tri-axis accelerometer and a tri-axis magnetometer whose MEMS technical specifications are given in [13,14] respectively. The acquisition frequency is set to 200 Hz for the VN-300 and 100 Hz for the ADIS-16488.

The reference orientations and positions are estimated by the ART IR tracking system [15] installed in a motion capture room (Figure 2a). It consists of 4 cameras on the floor and 4 cameras on the ceiling working at a 60 Hz sampling frequency. The subject is equipped with the ART MoCap target set comprising 6-degrees-of-freedom passive reflective markers. A marker tree is rigidly attached to the handheld MIMU (Figure 2b) in order to track its reference positions and orientations in time. The accuracy of the angular estimation of the ART IT tracking system is about one degree.

The surface of the motion capture room is 9 m², which is too small for performing pedestrian walks. Consequently the test subjects were asked to walk on a treadmill even if this may modify the human walking gait as compared to the natural gait of a pedestrian walking on a non-moving ground. Four test subjects (S1, S2, S3 and S4) participated in the experiment.

5.2. Experimental Scenarios

Three different walking modes were performed: “texting”, “swinging” and unsupervised walking. They correspond to the motions that travelers would do in the context of pedestrian navigation using a connected object. All four subjects performed the texting and swinging scenarios whereas only one subject (S1) performed the unsupervised walking scenario.

• Texting mode: The test subjects are walking on the treadmill at a 5 km/h comfortable with handheld IMU in a fixed position as compared to the pedestrian’s center of mass. This position corresponds to a traveler that is reading navigation instructions given on the screen of the connected object. Each dataset includes 100 to 140 strides, which corresponds to approximately 120 s.
• Swinging mode: The walking speed is again set to 5 km/h but the arm is naturally oscillating during the walk. The MIMU is still carried in hand during the acquisition and its duration is the same as in the texting scenario.
• Unsupervised walking: The walking speed ranges between 1 and 1.7 m/s. MIMU data was acquired over a 13 min walk, which corresponds to a 780 m to 1.3 km range of distances. No specific instruction was given to the test subject on how to carry the handheld MIMU. He/she walks freely on the treadmill, simulating the use of a connected object that gives navigation indications. During the experiment, several object carrying modes are observed. They correspond to the outcomes of the human activity classification algorithm defined in [16]. Among them are the “Swinging” mode (natural arm oscillation), the “Texting” mode (the upper limbs are constrained), the “Phoning” mode or the Irregular mode (unclassified).

Figure 3 and Figure 4 show, in blue, the norms of the MIMU’s acceleration vector and the sensed local magnetic field for the unsupervised scenario respectively. For comparison purposes, the norms of the Earth gravity field and the Earth magnetic field are plotted in red on the same figures. Figure 3 shows that the subject performed large-amplitude hand motions during the experiment. Figure 4 shows that many artificial sources of magnetic field are present in the motion lab room with a strongly perturbed Earth magnetic field.

5.3. Attitude Estimation Algorithms Comparison Approach for AEKF, MAGYQ and Delay MAGYQ

The MIMU dataset are post-processed with three different attitude estimation algorithms: AEKF, MAGYQ and MAGYQ Delay.

• Additive Extended Kalman Filter. The AEKF is a well-known algorithm that estimates the orientation with a quaternion parameterization in its additive form [17]. It assumes that the tri-axis magnetometer measures the Earth magnetic field. This hypothesis is common in most of existing algorithms [18,19].
• Magnetic, Acceleration Fields and Gyroscope Quaternion (MAGYQ) Based Attitude Estimation. This processing exploits steady magnetic field, even perturbed by artificial sources, to estimate the orientation angles. The reference field is not anymore the Earth field but the magnetometer vector measured at the beginning of the quasi static field period. Magnetic angular rates are then derived. Furthermore this algorithm proposes a gyroscope error modelling directly in the quaternion space to reduce linearization errors [5].
• Delay MAGYQ that is proposed in this paper and consider the correlation between the orientation estimate at the beginning of the quasi static magnetic field period and the orientation estimate at the epoch of the filter’s calculation.

Three different features are used to compare these algorithms:

• The convergence, which is used to assess the filter’s ability to correctly estimate the state parameters.
• The convergence speed, which completes the convergence criteria by including the time needed to achieve the convergence.
• The stability for assessing the filter’s ability to continue to correctly estimate the state parameters once the convergence is achieved.

The choice of the initial state parameters impacts the filter’s behavior. The filter’s convergence is more easily assessed when the initial state is different from the “true” values. When the filter is initialized with a state vector that is close to the correct values, it is rather the filter’s stability that is assessed. In what follows, all algorithms are initialized using the outcomes of the ART motion tracking system. This allows concentration on assessing the ability of the filter to correctly estimate the state parameters when large hand motions and large magnetic field fluctuations occur. The accelerometer and magnetometer noises are set to be 10% of the signal to noise ratio. The gyroscope noise is set according to the datasheet, around 0.0035°/(s·Hz^1/2).

5.4. Experimental Results

The performance analysis is conducted on the Euler angles estimates. The angular errors for the roll, pitch and yaw angles are computed using the reference orientations computed by the ART IR tracking system.

5.4.1. Texting Scenario

Table 1. Angular errors of AEKF, MAGYQ and Delay MAGYQ attitude estimation filters for the texting scenario with all four subjects.

Algorithms	AEKF		MAGYQ		Delay MAGYQ
Mean/standard deviation	μ	σ	μ	σ	μ	σ
Roll (°)	0.6	0.5	1.8	1.1	1.0	0.9
Pitch (°)	1.8	0.6	2.7	1.8	1.6	1.3
Yaw (°)	6.7	4.7	6.5	4.4	6.2	4.7

gives the angular errors for all four subjects in the texting scenario. μ corresponds to the absolute value of the mean error and σ is the associated standard deviation.

The results are similar for all subjects and algorithms, with not more than a 1° difference between the three algorithms. In the texting scenario on the treadmill at 5 km/h, the MIMU’s acceleration is relatively small compared to the Earth’s gravity field (Figure 5). Consequently, the measured accelerations give a good observation of the gravity field in the MIMU’s frame and the roll and pitch angles are well estimated by all algorithms. The yaw angle is also correctly estimated with a 6.2° to 6.7° average error. This is explained by the fact that the local magnetic field is relatively stable and close to the Earth’s magnetic field. This is can be seen in Figure 6 that show the differences between the Earth magnetic field vector and the local magnetic field vector computed with the MIMU’s records that have been transformed into the local frame using the MotionLab orientation data. In this texting scenario, the good magnetic field conditions and the rather small range of MIMU motions explain the good accuracy of the orientation estimates with all algorithms.

5.4.2. Swinging Scenario

Table 2. Angular errors of AEKF, MAGYQ and Delay MAGYQ attitude estimation filters for the swinging scenario with all four subjects.

Algorithms	AEKF		MAGYQ		Delay MAGYQ
Mean/standard deviation	μ	σ	μ	σ	μ	σ
Roll (°)	5.3	14.0	5.0	12.0	3.2	5.3
Pitch (°)	3.8	6.0	3.8	5.0	4.1	5.1
Yaw (°)	24.4	10.6	21.3	9.3	19.0	8.6

gives the angular errors for all four subjects in the swinging scenario. Similarly to the texting scenario, μ corresponds to the absolute value of the mean error and σ is the corresponding standard deviation.

In the swinging scenario, the orientation estimation performance is reduced for all algorithms as compared to texting scenario. The hand movements, which are synchronized with the oscillations of the arm, induce large accelerations as compared to the amplitude of the Earth’s gravity field (Figure 7). These accelerations strongly impact the roll and pitch angles estimation. Consequently the average error is increased by 2° to 4° for these two angles. The drop in performance is even greater on the standard deviation that increases by approximately 10° for the AEKF and MAGYQ filter.

Large magnetic field variations are observed for all four subjects. This phenomenon is illustrated in Figure 8 for the subject S4. These magnetic field fluctuations especially occur during the arm oscillations when the MIMU gets closer and further apart from the surrounding ferromagnetic compounds. This explains why the accuracy of the estimated yaw angles is fairly poor.

However, the three algorithms have different performances. Delay-MAGYQ achieves better angle estimates with smaller mean errors and smaller standard deviations. Contrary to the MAGYQ filter, Delay–MAGYQ integrates the covariance data of the orientation estimates to compute the reference field, which improves the orientation estimation. Indeed the two filters will process the measurement error in different ways. With the algorithm MAGYQ, the measurement error is completely passed onto the attitude estimate at the current time, whereas with Delay-MAGYQ, it is distributed not only on the estimate at the current time but also on the orientation at the first epoch of the quasi-static phase.

5.4.3. Unsupervised Walking Scenario

The orientation estimation errors and the corresponding statistics are detailed in

Table 3. Orientation estimates and their statistics for the unsupervised walking scenario.

Algorithms	AEKF		MAGYQ		Delay MAGYQ
Mean/standard deviation	μ	σ	μ	σ	Μ	σ
Roll (°)	5.8	7.4	6.0	8.8	5.0	8.5
Pitch (°)	2.0	2.6	3.9	4.2	3.3	3.3
Yaw (°)	18.1	8.6	11.3	10.2	8.2	9.8

for the unsupervised walking scenario. The yaw estimates for the AEKF, MAGYQ and Delay MAGYQ filters are also plotted in Figure 9 for comparison purposes.

It is observed that the best orientation estimates are achieved with the filter Delay MAGYQ whereas the AEKF gives the worst results. Indeed, the errors on the roll and pitch angles are in the same order of magnitude for both algorithms but the yaw estimate is strongly deteriorated with the AEKF. This is explained by the experimental environment where large magnetic field anomalies are present. The motion acquisition room is equipped with many electronic devices, wires and metallic walls that strongly modify the Earth magnetic field. This can be observed in Figure 2 where the Earth magnetic field norm and the measured local magnetic field norm are plotted in red and blue respectively. Consequently, the AEKF functioning hypothesis H₀, which assumes that the magnetometer measures the Earth magnetic field, is not valid and the orientation gets biased.

MAGYQ and Delay-MAGYQ algorithms are immune to local perturbations because the assumption H₀ is not required to process the raw inertial signals and magnetometer measurements. Consequently, better yaw estimates are obtained thanks to the magnetically derived angular rates with a reference magnetic field that is closer to the reality, i.e., the field at the beginning of the quasi static period. Furthermore the use of magnetic field gradients, when the local field is steady, enables continuous calibration of the gyroscope errors.

Because the Delay MAGYQ algorithm considers the impact of past orientation estimates on the magnetic reference field estimate and the current orientation estimate, it gives the best results with a slightly larger standard deviation. With the Pedestrian Dead Reckoning (PDR) approach, which is applied to provide navigation services on connected objects, the accuracy of the yaw angle directly impacts the accuracy of the geolocation services, whereas roll and pitch angles have a smaller impact on the location estimate. Indeed, current PDR position is derived from the previous position knowing the yaw angle and the step length. This highlights why it is important to consider the correlation between states estimated at different times to enhance the accuracy of the yaw angle and provide more accurate handheld based navigation services.

An innovation test is performed to analyze the gain of Delay-MAGYQ as compared to the MAGYQ filter. The test is performed over a QSF period of magnetic field during which the MIMU is static. The initial orientation is set as equal to the orientation (the reference) estimated by the MotionLab for both filters. The performance of the two filters is analyzed with the norm of the orientation’s residuals: $\mathsf{\delta }{q}_{bt}^n$. Figure 10 shows the residual errors estimated by both filters.

The main observation is that the norm of the innovation is lower for Delay MAGYQ than for MAGYQ. Because the initial orientation of both filters corresponds to the “true” one and there is no orientation change, the filter that exhibits the lowest innovation will perform better. This is observed with Delay MAGYQ, which includes covariance data of the first QSF state’s estimate in the computation of the Kalman gain. Indeed, MAGYQ filter gain is classically given by

$$K_{t_k}=P_{t_k}{H}_{t_k}{}^T{\left(H_{t_k}{P}_{t_k}{H}_{t_k}{}^T+R_m\right)}^{-1}$$

(27)

whereas the gain of Delay MAGYQ filter integrates the orientation error $\mathsf{\delta }{q}_b^n$ in

$$K_{t_k}=P_{t_k}{H}_{t_k}{}^T{\breve{S}}_{t_k}{}^{-1}-{\mathrm{\Gamma }}_{t_0,t_k}\left[\begin{array}{c}{P}_{q,t_0}\\ P_{\left(q,b\right),t_0}{}^T\end{array}\right]H_{t_0}{}^T{\breve{S}}_{t_k}{}^{-1}$$

(28)

$P_{q,t_0}$, $P_{\left(q,b\right),t_0}$, ${\breve{S}}_{t_k}$ and ${\mathrm{\Gamma }}_{t_0,t_k}$ are detailed in Equations (21) and (23).

6. Conclusions

Whereas some MIMU-based attitude estimation filters try to mitigate the impact of an artificial field on the angular estimation, other exploit the distorted field when the perturbation is assessed as static. They are based on magnetic field gradients and a reference field, which is estimated at the beginning of the period of interest. However, these filters do not consider the time correlation of past state estimates on the current orientation estimate.

A novel Delay MAGYQ filter is proposed to overcome this limitation. The design of the filter directly integrates the time correlation of the orientation estimates, in particular with a covariance matrix that relates the error on the quaternion estimates at previous times with the current state estimate. The impact of including the time correlation in the angles estimation is assessed in a motion laboratory with four persons. Three scenarios are built for the context of pedestrian navigation with a handheld MIMU. They correspond to a total of 30 min of data collection.

The novel filter is found to better estimate the Euler angles of the handheld device. Looking at the yaw angle, it is estimated with an 11.7° mean error by Delay-MAGYQ as compared to a 16.4° with the Additive Extended Kalman filter and a 12.7° mean error without considering the time correlation of the filter’s estimates. These mean errors are computed for the entire data collection. The same standard deviation, ranging from 7.7° to 8°, is associated to the error on the yaw angle for all three filters. Another finding is that the linear acceleration sensed by the MIMU perturbs the orientation estimation for all filters. Globally, Delay-MAGYQ is found to be more robust thanks to an improved processing of errors at past epochs for the orientation estimates.

7. Perspectives

Despite the good performances achieved for the orientation estimation, the identification of the periods during which the local magnetic field is assessed as steady should be improved in order to better apply opportune QSF updates. Being able to mitigate large accelerations in the orientation estimation is also planned in future research, especially when the arm is oscillating during natural walking (e.g., with a smartwatch).