This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution license (http://creativecommons.org/licenses/by/3.0/).

In this paper a quaternion-based Variable-State-Dimension Extended Kalman Filter (VSD-EKF) is developed for estimating the three-dimensional orientation of a rigid body using the measurements from an Inertial Measurement Unit (IMU) integrated with a triaxial magnetic sensor. Gyro bias and magnetic disturbances are modeled and compensated by including them in the filter state vector. The VSD-EKF switches between a quiescent EKF, where the magnetic disturbance is modeled as a first-order Gauss-Markov stochastic process (GM-1), and a higher-order EKF where extra state components are introduced to model the time-rate of change of the magnetic field as a GM-1 stochastic process, namely the magnetic disturbance is modeled as a second-order Gauss-Markov stochastic process (GM-2). Experimental validation tests show the effectiveness of the VSD-EKF, as compared to either the quiescent EKF or the higher-order EKF when they run separately.

Accurate determination of the three-dimensional (3D) orientation of rigid bodies is important in several technical fields, including robotics and virtual reality [

Usually, a fully integrated inertial measurement unit (IMU) consists of three vector-sensors, namely an accelerometer, a gyro, and a magnetic sensor; each of them has three mutually orthogonal sensitive axes. The 3D orientation can be computed by time-integrating the gyro output from initial conditions that are given by the aiding sensors (accelerometer and magnetic sensor). The gyro integration errors tend to grow unbounded over time because of time-varying biases and wideband measurement noise superimposed to gyro output. On the other hand, the accelerometer can provide drift-free inclination estimates by sensing the gravity field; the magnetic sensor, which would sense the Earth's magnetic field, helps providing drift-free heading estimates. However, the interpretation of the accelerometer output is critical because of the presence of the acceleration components related to the body motion [

Sensor fusion techniques are needed in order for the aiding sensors to help mitigate the gyro integration errors, while the errors incurred by the aiding sensors are mitigated using the gyro output [

State-augmentation is a useful technique to estimate the state vector of interest,

Because of some difficulties in tracking abrupt magnetic distortions [

The VSD-EKF estimates the following state variables: the quaternion between the body-fixed frame,

The time-derivative
^{T}_{4}]^{T}^{T}

In (

The discrete-time equivalent of (_{n}_{s}_{k}_{k}_{−1}, we have:

In this paper the IMU signals are sampled at _{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 [^{h}

The GM-1 model assumes that ^{h}_{L}_{L}_{L}

The GM-2 model assumes that the time-rate of change of ^{h}_{H}_{H}_{H}

In response to body angular velocity _{b}_{b}^{s}^{h}

The orientation matrix
^{g}^{a}^{m}_{3}); ^{g}^{a}^{m}^{g}^{a}^{m}_{b}

The deterministic errors associated with inertial and magnetic sensors can be compensated using the procedures described in [_{g}

The VSD-EKF adopts two state models: the quiescent state model, where the state vector is ^{T h}^{T g}^{T}^{T}^{T h}^{T h}^{T g}^{T}^{T}

The discrete-time model allows employing the sampled measurements of the IMU for propagating the state vector:

In (_{m}^{g}

The process noise component ^{q}_{k}_{−1} describes how the gyro measurement noise enters the state model through a quaternion-dependent linear transformation:

Since the process noise components ^{q}_{k}_{−1}, ^{h}_{k}_{−1}, ^{g}_{k}_{−1} are assumed to be uncorrelated, the process noise covariance matrix _{k}_{−1} has the following structure [_{k}_{−1} and

The measurement equations can be written as follows:

The rotation matrix can be derived from the quaternion [

The measurement equations are non-linear, hence we need to compute their Jacobian matrix

The detailed calculation of (

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., [

The state-transition matrix _{k}_{−1}, the process noise covariance matrix _{k}_{−1} and the measurement noise covariance matrix _{k}

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 [

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 [

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 [_{s}

The ground-truth orientation was obtained by processing the marker coordinates provided by the optical motion analysis system as described in [

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

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}σ^{2} (filtering option: Y); the gyro-bias compensation was disabled by setting ^{g}σ^{2}. The filter parameter setting reported in

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.

During the static test the variation of the magnetic field intensity and of the dip angle, namely the angle that the total field vector makes with respect to the horizontal plane, are about 62% and 31%, respectively); the RMS value of the magnetic distortion is about 181 a.u..

The fading memory average that is monitored for switching to the higher-order state model is shown in _{1} = [61.98; 73.2] s; _{2} = [121.9; 123.4] s; _{3} = [133.1; 134.1] s, where the fading memory average is zero. The RMS errors are reported in

The dynamic test is carried out in an environment where the RMS value of the magnetic distortion is about 75 a.u. (the variation of the magnetic field intensity and of the dip angle are about 20% and 30%, respectively). The acceleration magnitude is 1

The GM-1 model is used in most reported Kalman-based-filters for explaining the magnetic distortion, either in fixed-parameter or adaptive implementations. The filter response to a magnetic perturbation is faster and more accurate when the standard deviation of the process noise is increased and, although to a more limited extent, its correlation time constant is decreased [

Quite typically, the yaw estimates produced by the adapted quiescent EKF may drift even when quiet magnetic conditions are restored after a disturbance occurrence. This is due to its limited capability of accurately tracking the time-changes of the magnetic field when they occur. The inability of the adapted higher-order EKF to produce accurate yaw angle estimates in magnetically perturbed conditions is quite surprising, in the face of the success met by the SVD-EKF, whose model in magnetically perturbed conditions is essentially the same. A possible explanation is that the higher-order EKF is extremely sensitive to the strength of the GM-2 driving noise. In conditions of magnetic quiet, the higher-order EKF tends to diverge when the driving noise strength is too large. With those values of the driving noise strength that are acceptable in conditions of magnetic quiet, the algorithm tends to perform very poorly when the magnetic field is about to change. This does not occur with the SVD-EKF, which relies on the quiescent EKF in conditions of magnetic quiet. As expected, the switching to the higher-order EKF increases the algorithm capability of accurately tracking the time-changes of the magnetic field. In conclusion, we catch the best by combining the two models together as done in the VSD-EKF, which shows good accuracy of the estimated yaw and good compensation of the magnetic disturbances.

The results of the dynamic test show the effectiveness of the proposed VSD-EKF and basically confirm the results of the static test. Not surprisingly perhaps, the RMS errors affecting the roll and pitch angle estimates are worse than in the static test. A possible reason may be due to slight imperfections in the synchronization process between IMU and Vicon data streams and errors incurred by the optical motion analysis system itself in generating the ground-truth reference orientation data used for computation. Another reason is certainly that the acceleration measurements cannot be explained by gravity alone. Although slow in nature, the IMU motion determines additional components in the acceleration measurements that cannot be taken into account when they are used in the measurement update of the EKF. The trick we choose to mitigate this problem is to increase the values of the accelerometer measurement noise standard deviation in the matrix

Another interesting observation from our tests is that the gyro-bias compensation generally helps produce more accurate estimates of either orientation or magnetic field. This may be quite surprising, since the gyro output is submitted to a bias capture procedure during the filter initialization and the gyro bias cannot change significantly over the limited time duration of the tests. Our interpretation is that adding a limited amount of pseudo-noise to the gyro-bias components of the state vector has a useful stabilizing effect, which is beneficial in producing more accurate estimates. The injection of limited amounts of process noise is often suggested as a means to stabilize the behavior of Kalman-based filters [

The results of the tracking tests reported in this paper are quite representative of the behavior of the investigated filters. We may state that the quiescent EKF with gyro bias compensation and the higher-order EKF without gyro bias compensation perform similarly, and both are inferior to the SVD-EKF, sometimes remarkably.

In conclusion, we have presented in this paper an adaptive variable-state-dimension EKF for processing the data from an IMU integrated with a magnetic sensor. The proposed algorithm estimates the quaternion of rotation and attempts to compensate magnetic disturbances and gyro bias. The filter switches from a quiescent EKF, built around a first-order Gauss-Markov model for describing magnetic distortions, to a higher-order EKF, built around the same stochastic model applied to the time-rate of change of the local magnetic field. Experimental validation tests show the effectiveness of the proposed approach in magnetically perturbed environments.

The author is indebted to Andrea Mannini for his help in performing the experiments described in the paper.

Static test. (

Fading memory average used for switching from the quiescent state model to the higher-order state model in the VSD-EKF (static test).

Estimated yaw angle for the three different filters (static test).

Components of the ground-truth reference magnetic field. The horizontal bars at the top of the plot show the time intervals of sojourns in the higher-order state model (dynamic test).

Ground-truth Euler angles and estimation errors incurred by the VSD-EKF (dynamic test).

Parameter tuning.

Perturbation OFF | Perturbation ON | |
---|---|---|

GM-1 | GM-2 | |

Correlation time constant, s |
0.1^{−2} a.u./s |
0.2^{−1} a.u./s^{2} |

GM-2 | GM-2 | |

Correlation time constant, s^{2} |
0.1^{−5} |
0.2^{−1} |

GM-1 | GM-1 | |

Correlation time constant, s |
0.1^{−2} |
0 |

Perturbation OFF | Perturbation ON | |

GM-1 | GM-2 | |

Correlation time constant, s |
0.1^{−2} a.u./s |
0.2^{−2} a.u./s^{2} |

GM-2 | GM-2 | |

Correlation time constant, s^{2} |
0.1^{−5} |
0.2^{−1} |

GM-1 | GM-1 | |

Correlation time constant, s |
0.1^{−2} |
0.4 |

Parameter tuning.

| ||
---|---|---|

_{g} |
0.4 | 0.5 |

_{a} |
1.0 | 15.0 |

_{m}^{−3}) |
1.0 | 2.0 |

Euler angles and magnetic distortion RMSE (static test).

VSD-EKF-Y | 0.09 | 0.02 | 0.17 | 7.89 |

VSD-EKF-N | 0.09 | 0.02 | 3.16 | 25.31 |

Higher-order EKF-Y | 0.07 | 0.08 | 3.54 | 26.10 |

Higher-order EKF-N | 0.07 | 0.08 | 2.57 | 20.81 |

Quiescent EKF-Y | 0.09 | 0.02 | 1.60 | 60.40 |

Quiescent EKF-N | 0.09 | 0.02 | 4.87 | 70.12 |

Euler angles and magnetic distortion RMSE (dynamic test).

VSD-EKF-Y | 1.39 | 1.21 | 1.90 | 19.78 |

VSD-EKF-N | 1.81 | 1.47 | 6.66 | 33.02 |

Higher-order EKF-Y | 2.19 | 1.47 | 8.20 | 49.39 |

Higher-order EKF-N | 1.99 | 2.06 | 2.18 | 36.89 |

Quiescent EKF-Y | 1.29 | 1.10 | 2.06 | 20.96 |

Quiescent EKF-N | 1.66 | 1.27 | 7.17 | 37.10 |