The time-derivative of the quaternion q̄=[q^T q₄]^T and the angular velocity ω = [p q r]^T of the B-frame relative to the E-frame, resolved in the B-frame, are related by the following system of differential equations[17,18], where the time argument is dropped for notational convenience: (1) q ¯ ˙ = 1 2 [ - [ ω × ] ω - ω T 0 ] q ¯ = Ω ( ω ) q ¯ .

In (1) the operator: (2) [ ω × ] = [ 0 - r q r 0 - p - q p 0 ]is the standard vector cross product.

The discrete-time equivalent of (1) is given by: (3) q ¯ ( t k ) = Φ k - 1 q ¯ ( t k - 1 ) ,with: (4) Φ k - 1 = I 4 ⋅ cos ( ‖ u k - 1 ‖ / 2 ) + Ω ( ω ( t k - 1 ) ) ⋅ sin ( ‖ u k - 1 ‖ / 2 ) ‖ u k - 1 ‖ / 2 ,where I_n is the n × n identity matrix (n = 4) and ‖·‖denotes the standard vector norm. If the angular velocity is assumed constant during the sampling interval T_s = t_k − t_k−1, we have: (5) u k - 1 = ∫ t k - 1 t k ω ( τ ) d τ ≈ ω ( t k - 1 ) T s .

In this paper the IMU signals are sampled at T_s = 0.01 s.

The conditions for accurate orientation determination using a magnetic sensor are that the magnetic field be homogeneous and accurately known. The homogeneity is difficult to achieve [7–9]. The magnetic field can thus unpredictably change in direction and magnitude when the IMU moves relative to the environment. We propose two stochastic models for describing the magnetic distortion ^hb, i.e., the difference between the magnetic field and its vector value in a given point at a given time—the local reference magnetic field h—both expressed in the E-frame.

The GM-1 model assumes that ^hb is the realization of an exponentially time-correlated vector stochastic process whose components are statistically independent: (6) b ˙ h = - α L b h + w L ,where α_L is the reciprocal of the correlation time constant τ_L and w_L is white Gaussian noise, with zero mean and covariance matrix ∑ L h = I 3 ⋅ σ b 2 L.

The GM-2 model assumes that the time-rate of change of ^hb is the realization of a GM-1 vector stochastic process with statistically independent components: (7) b ¨ h = - α H b ˙ h + w H ,where α_H is the reciprocal of the correlation time constant τ_H, and w_H is white Gaussian noise, with zero mean and covariance matrix ∑ H h = I 3 ⋅ σ b 2 H. Henceforth, we refer to σ b 2 H as the strength of the GM-2 model driving noise.

In response to body angular velocity ω_b, acceleration (gravity field g and body acceleration a_b), and magnetic field ^sh (local magnetic field h and magnetic distortion ^hb), the measured outputs of the IMU sensors can be written as [10]: (8) { ω m = G g ω b + b g + v g a m = G a C E B ( - g + a b ) + b a + v a m m = G m C E B ( h + b h ) + b m + v m .

The orientation matrix C E B moves vector representations from the E-frame to the B-frame; ^gG, ^aG, ^mG are the matrices of the scale factors (ideally equal to the identity matrix I₃); ^gb, ^ab, ^mb are the bias vectors (ideally, they are zero); ^gv, ^av, ^mv are uncorrelated white Gaussian measurement noises, with zero mean and covariance matrix ∑ g = I 3 ⋅ σ g 2 , ∑ a = I 3 ⋅ σ a 2 and ∑ m = I 3 ⋅ σ m 2. Equation (8) is a model that does not account for additional error sources, such as cross-axis sensitivity, gyro g-sensitivity, nonlinearity, hysteresis and misalignment [19]. Finally, we also assume for simplicity that the tracked objects are slowly moving, namely a_b ≈ 0.

The deterministic errors associated with inertial and magnetic sensors can be compensated using the procedures described in [19,20], respectively, leaving the time varying biases, in particular of gyros, as the residual errors. In line with a common practice [2,10,14,15], we model the gyro-bias vector as the realization of a random-walk vector stochastic process with statistically independent components: (9) b ˙ g = w g .where w_g is white Gaussian noise, with zero mean and covariance matrix ∑ g = I 3 ⋅ h σ b 2 d.

The VSD-EKF adopts two state models: the quiescent state model, where the state vector is x=[q̄^{T h}b^{T g}b^T]^T and the magnetic distortion is GM-1, and the higher-order state model, where the state vector is x=[q̄^{T h}b^{T h}ḃ^{T g}b^T]^T and the magnetic distortion is GM-2. The quiescent EKF is fully developed in [13]. The continuous-time system equations of the higher-order EKF are written as follows: (10) { q ¯ ˙ = Ω ( ω ) q ¯ b ¨ h = - α H b ˙ h + w H b ˙ g = w g .

The discrete-time model allows employing the sampled measurements of the IMU for propagating the state vector: (11) [ q ¯ k b k h b ˙ k h b k g ] = F k - 1 [ q ¯ k - 1 b k - 1 h b ˙ k - 1 h b k - 1 g ] + [ w k - 1 q 0 3 × 1 w k - 1 h w k - 1 g ]where: (12) F k - 1 = [ Φ k - 1 0 4 × 3 0 4 × 3 0 4 × 3 0 3 × 4 I 3 I 3 ⋅ 1 - exp ( - α H T s ) α H 0 3 × 3 0 3 × 4 0 3 × 3 I 3 ⋅ exp ( - α H T s ) 0 3 × 3 0 3 × 4 0 3 × 3 0 3 × 3 I 3 ]and: (13) Φ k - 1 = I 4 ⋅ cos ( ‖ ω ∼ ( t k - 1 ) ‖ T s / 2 ) + Ω ( ω ∼ ( t k - 1 ) ) ⋅ sin ( ‖ ω ∼ ( t k - 1 ) ‖ T s / 2 ) ‖ ω ∼ ( t k - 1 ) ‖ T s / 2 .

In (13) the angular velocity ω̃ = ω_m − ^gb is taken as the control input.

The process noise component ^qw_k−1 describes how the gyro measurement noise enters the state model through a quaternion-dependent linear transformation: (14) w q k - 1 = - T s 2 Ξ ( q ¯ k - 1 ) v g k - 1and the matrix Ξ(q̄) is given by: (15) Ξ ( q ¯ ) = [ I 3 ⋅ q 4 + [ q × ] - q T ] .

Since the process noise components ^qw_k−1, ^hw_k−1, ^gw_k−1 are assumed to be uncorrelated, the process noise covariance matrix Q_k−1 has the following structure [21,22]: (16) Q k - 1 = [ ( I 4 ⋅ trace ( M ) - M ) ⋅ σ g 2 ( T s 2 ) 2 0 3 × 3 0 3 × 3 0 6 × 3 Q H 0 3 × 3 0 3 × 3 0 3 × 3 I 3 ⋅ σ b g 2 T s ] Q H = [ Q 11 H Q 12 H Q H 12 T Q 22 H ] Q 11 H = I 3 ⋅ 4 exp ( - α H T s ) - 3 - exp ( - 2 α H T s ) + 2 α H T s 2 α H 3 σ H 2 Q 12 H = I 3 ⋅ exp ( - 2 α H T s ) + 1 - 2 exp ( 2 α H T s ) 2 α H 2 σ H 2 Q 22 H = I 3 ⋅ 1 - exp ( - 2 α H T s ) 2 α H σ H 2where: (17) M = q ¯ k - 1 q ¯ k - 1 T + P k - 1 q .q̄_k−1 and P k - 1 q denote, respectively, the expectation and the covariance matrix of the quaternion component of the state vector [22].

The measurement equations can be written as follows: (18) [ a m , k m m , k ] = [ C E B ( q ¯ k ) 0 3 × 3 0 3 × 3 C E B ( q ¯ k ) ] [ - g b s k ] + [ v a k v m k ] .

The rotation matrix can be derived from the quaternion [10]: (19) C E B ( q ¯ ) = I 3 - 2 q 4 [ q × ] + 2 [ q × ] 2 .

The measurement equations are non-linear, hence we need to compute their Jacobian matrix H, as prescribed by the EKF linearization process: (20) H k = [ ψ ( q ¯ k , - g ) 0 3 × 3 0 3 × 3 0 3 × 3 ψ ( q ¯ k , b s ) C E B ( q ¯ k ) 0 3 × 3 0 3 × 3 ] ,where, for a generic vector p, we define: (21) ψ ( q ¯ , p ) = ∂ ∂ q ¯ C E B ( q ¯ ) p .

The detailed calculation of (21) is illustrated in [13]. The measurement noise covariance matrix can be expressed directly in terms of the statistics of the measurement noise affecting each sensor: (22) R = [ ∑ a 0 3 × 3 0 3 × 3 ∑ m ] .

Vector selection methods for measurement noise covariance matrix adaptation can be applied to guard against the effects of body accelerations and magnetic disturbances, as discussed in, e.g., [12,23,24]. They are not implemented here.

The state-transition matrix F_k−1, the process noise covariance matrix Q_k−1 and the measurement noise covariance matrix H_k are dependent on the state. Since the true state is unknown, the state predicted in the prediction stage is used in its place, as it is common practice in EKF formulations. Finally, the unit-quaternion constraint needed for the quaternion to represent a valid rotation is enforced by brute-force normalization after the update stage [10].

The filter initialization is carried out in conditions when the IMU is still and no time-varying magnetic dipoles influence the local magnetic field. The sensor measurements are averaged during an initial rest period of 1 s. The inclination is computed by processing the initial average vector of acceleration measurements. The local magnetic field is estimated by projecting the initial average vector of magnetic measurements using the estimated inclination; the magnetic distortion is initialized to zero. Finally, the initial quaternion and its covariance matrix are computed using the TRIAD method [25]. Computing the average vector of gyro measurements in the rest period allows performing the capture of the gyro-bias, which is then initialized to zero.

The default operating mode of the proposed algorithm is based on the GM-1 model for the magnetic distortion (quiescent EKF). When necessary, extra components are added to the system's state vector, as prescribed by the GM-2 model (higher-order EKF). The higher-order EKF is reverted to the quiescent EKF by another decision. When the magnetic perturbations are small or negligible, using the higher-order EKF increases the state estimation errors. Conversely, using the quiescent EKF may yield maximum estimation accuracy—no information is wasted on estimating components that are zero or small. However, if fast changes of magnetic direction and intensity are experienced, the quiescent EKF may not effectively track the magnetic distortion, which is essential for accurate orientation determination.

To implement the VSD-EKF we apply the technique discussed in [26]. The switching from the quiescent to the higher-order EKF is based on computing a fading memory average of the normalized innovation squared from the quiescent EKF (a = 0.9). The switching takes place when the fading memory average of the normalized innovations exceeds a given up-crossing threshold. The fading memory has an effective window length s, and the onset of the magnetic perturbation is taken at the beginning of this window. As for the initialization of the higher-order EKF, when a magnetic perturbation is detected at time k, the algorithm assumes that the magnetic distortion has a constant rate of change starting at k − s − 1. Then, the higher-order EKF equations reprocess the IMU measurements sequentially up to time k, before sequentially processing incoming measurements at later times. The scheme for reverting to the quiescent EKF is based on using a fading memory average of the normalized innovation squared computed for the extra-state components of the GM-2 model; the fading memory average is then compared to a given down-crossing threshold. When falling below the threshold the quiescent EKF is called on again.

The experimental validation tests were carried out using the Xsens MTx orientation sensor, whose raw data were delivered to a host computer via a USB interface at a rate of 100 Hz. The manufacturer expressed the measurements of the magnetic field in arbitrary units (a.u.).

Before starting each test the IMU sensors were calibrated [19,20]. During the static test, whose duration was 3 min, the IMU remained on a table far from electromagnetic devices and ferromagnetic materials. A magnetic disturbance was generated at the time when a cell phone was moved close to the IMU toward the end of the first and second minute of data recording, for approximately 10 seconds. The dynamic test was performed as follows. The IMU was fastened to a plastic plate with size 10 cm × 13 cm using double-side adhesive tape. The plate was raised by the experimenter slightly over the table and then it was freely moved around, thereby sweeping a volume of about 60 cm × 60 cm × 60 cm. The plate orientation was recorded using a six-camera Vicon optical motion analysis system with a sampling rate of f_s = 100 Hz. The Vicon system measured the position of four reflective markers (diameter: 14 mm) arranged at the corners of the plate. The IMU and Vicon data streams were electronically synchronized. The initial orientation of the sensor frame, i.e., the B-frame, relative to the marker frame, i.e., the E-frame, was computed during the initial rest period of the IMU. Occasionally the IMU was moved close to a loudspeaker magnet placed on the table, a first time after 30 seconds of data recording. In the neighborhood of the loudspeaker, the magnetic field was strongly perturbed. The IMU was either taken at rest by the experimenter for few seconds, or it was moved around in the surroundings before being kept away. The visits to the region where the magnetic field was perturbed occurred every minute for approximately 10 seconds.

The ground-truth orientation was obtained by processing the marker coordinates provided by the optical motion analysis system as described in [8]. The Euler angles time functions delivered by the EKFs were obtained from the estimated quaternion using standard conversion formulas. We then computed the RMS errors incurred by the EKF in estimating the Euler angles using the ground-truth orientation as reference.

The magnetic field was estimated directly from the magnetic sensor measurements during the static test, since we assumed that the E-frame and the B-frame were aligned; as for the dynamic test, the ground-truth orientation was used to project the magnetic sensor measurements from the B-frame into the E-frame, yielding a good approximation to the magnetic field (ground-truth magnetic field). The strength of the magnetic perturbation was computed by taking the difference vector between the ground-truth magnetic field and the initial field h. The overall RMS value was then computed using the root sum-of-square rule applied to single components of the difference vector. Either for static or dynamic tests, the error between the estimated and the ground-truth fields was analyzed in terms of the overall RMS error, computed as described above for the strength of the magnetic perturbation.

The following filter implementations were assessed: the VSD-EKF, the quiescent EKF and the higher-order EKF. Either the quiescent or the higher-order EKF were made adaptive, although we did not develop specific switching rules for adaptation. Our approach was to adapt their state-transition and process noise covariance matrices by changing the value of the parameters valid for the condition Perturbation OFF to those valid for the condition Perturbation ON, or vice versa, using the OFF-ON and ON-OFF switching times computed by the VSD-EKF. For the purpose of a fair comparison between the various filter implementations, the OFF-ON switching times for adapting the quiescent and the higher-order EKFs were taken at the beginning of the fading memory window used by the VSD-EKF to reprocess the IMU measurements. The standard deviation of the random-walk model for gyro-bias compensation was set to ^gσ = 0.01 °/s² (filtering option: Y); the gyro-bias compensation was disabled by setting ^gσ = 0 °/s². The filter parameter setting reported in Tables 1 and 2 was chosen during the static test the process noise covariance matrix Q was tuned using the value of the gyro measurement noise standard deviation, assessed when the IMU was at rest.

The Kalman-based filters were written in Matlab for off-line data processing using a MacBook Pro computer and the virtualization technology from Parallel Desktop 4.0 for Mac. The VSD-EKF cycle time for a single iteration was about 4 ms, on average.