Kalman-Filter-Based Orientation Determination Using Inertial/Magnetic Sensors: Observability Analysis and Performance Evaluation

2.1. Using Quaternions to Represent the Orientation

The 3D orientation of a rigid body is represented using two right-handed coordinate systems: the earth-fixed frame E = {e₁ e₂ e₃} and the body-fixed frame $\mathbf{\text{B}}=\left\{{{\mathbf{\text{e}}}^{\prime }}_1\hspace{0.17em}{{\mathbf{\text{e}}}^{\prime }}_2\hspace{0.17em}{{\mathbf{\text{e}}}^{\prime }}_3\right\}$. In applications of motion tracking the motion of the E-frame due to the earth’s rotation is usually negligible; the E-frame is therefore an inertial frame. In the following, we also assume that the sensitivity axes of the IMU sensors are aligned with the axes of the B-frame, and the physical location of the accelerometer is where the origin of the B-frame is located, i.e., no centripetal acceleration effects exist.

An arbitrary vector x in the 3D space can be represented in terms of the coordinates (or components) with respect to either of the frames:

The subscripts E, B denote in which frame the vector x is represented. The columns of the orientation matrix ${}_{ℰ}^{ℬ}\mathbf{\text{C}}$ are the representations of the e_i, i = 1,…,3 with respect to B, while its rows are the representations of the ${{\mathbf{\text{e}}}^{\prime }}_i$, i = 1,..., 3 with respect to E. The orientation matrix and its transpose allow moving vector representations from (to) E to (from) B, respectively. The orientation matrix is a 3 × 3 orthogonal matrix with unit determinant, which belongs to the 3D special orthogonal group SO(3) of rotation matrices. The quaternion is a more parsimonious representation of orientation than the orientation matrix [24]; it is derived by formulating the orientation matrix as a homogeneous quadratic function of the Euler symmetric parameters q_i, i = 1,...,4:

Here, the axis and angle of rotation (n,θ) are another valid representation of the orientation, closely related to the rotation vector θ = θ n [24]. As implied by Equation (4), in order to represent a valid rotation the quaternion must comply with the normalization constraint:

In the quaternion space the multiplication between two generic quaternions q̄ and q̄′ is defined as follows:

Given the vector quaternion ${\overline{\mathbf{\text{x}}}}_{ℰ}={\left[{\mathbf{\text{x}}}_{ℰ}^T 0\right]}^T$, namely a quaternion with null scalar part associated with the vector x_$ℰ$, the vector quaternion:

When the unit norm constraint Equation (6) is enforced, a quaternion is left with the three degrees of freedom consistent with the SO(3) dimensionality. The four-component unit quaternion has thus the lowest dimension of any globally non-singular orientation parameterization. However, the representation is redundant, since the quaternion −q̄ represents the same rotation as q̄.

2.2. Kinematic Equations

These describe the relations between the temporal derivatives of an orientation representation such as the quaternion q̄ and the angular velocity ω_$ℬ$, namely the angular velocity of the $ℬ$-frame relative to the $ℰ$-frame, as it is measured by a tri-axial gyro fixed to the body:

In the following the time argument will always be dropped to make the notation a little easier. Ω(ω_B) is a 4 × 4 skew symmetric matrix. The kinematic Equation (9) do not involve nonlinear computationally expensive trigonometric functions and are not affected by the presence of singularity points, in contrast to the formulation using representations of orientation such as the Euler angles.

The discrete-time equivalent of Equation (9) is given by:

If the angular velocity is assumed constant in the sampling interval T_s = t_k − t_k−1, we have:

2.3. Observability Analysis

A system is observable when, for any possible sequence of state and control input vectors, the current state can be inferred by measuring the output variables. In other words, the output variables contain all the information needed to determine the behavior of the system. For continuous time-varying nonlinear systems the observability analysis, namely the analysis needed to verify whether a system is observable or not, requires the development of specific tools [25].

Consider a nonlinear system Σ described, in state space form, by a set of equations of the following form:

The time argument in x, u, and y is dropped. x = [x₁, x₂,…, x_n]^T ∈ X ⊂ ℝⁿ, u = [u₁, u₂,…, u_l,]^T ∈ ℝ^l, y = [y₁, y₂,…, y_m]^T ∈ ℝ^m are, respectively, the state vector, the control input vector and the measurement vector. The process function f is input-linear, i.e., it can be decomposed into a sum of independent functions f_i, each one corresponding to a different component u_i of the control input vector u. The process function f and the measurement function h are assumed smooth in their arguments, namely they possess continuous partial derivatives of any order.

A suitable tool for studying the observability properties of Σ is the observability rank condition based on the Lie derivatives of the measurement functions h_k(x), k = 1,...,m. The zero-order Lie derivative of a scalar function is, by definition, the function itself, $ℒ$⁰ h_k(x) = h_k(x), k = 1,…,m. The first-order Lie derivative of the measurement function h_k(x) with respect to the component f_i of the process function f is given by:

The definition of higher-order and mixed Lie derivatives with respect to different components f_i, f_j of the process function f follows the same route as above. For instance, the second-order Lie derivatives with respect to f_j and f_i, given their first-order derivatives with respect to f_i, are given by

The observability matrix is constructed by stacking the gradients of the Lie derivatives:

The number of columns of the observability matrix is equal to the dimension n of the state vector x, while the number of rows may grow unbounded with the order of the computed Lie derivatives. Since the input and measurement functions are smooth, the number of rows of O can be thus virtually infinite.

For nonlinear systems the observability property is related to the concept of indistinguishability of states with respect to the control inputs. Two states x₁ and x₂ are said indistinguishable when, for every admissible control input, the system produces the same outputs in a given interval of time when starting from either x₁ or x₂. The system Σ is weakly observable at the state x₀ if for every open neighborhood U of x₀, x₀ can be distinguished from states in U(x₀), or weakly observable if it is so for every x ∈ X. The system Σ is locally weakly observable at the state x₀ if x₀ can be distinguished from states in an open neighborhood V(x₀) ⊂ U(x₀) when admissible control inputs lead to paths within U(x₀), or locally weakly observable if it is so for every x in X. A sufficient condition for the system Σ to be locally weakly observable at x₀ is that O(x₀) is full rank, namely the gradients of the Lie derivatives of the measurement functions computed at x = x₀ must span the state space X. If it is so for every x ∈ X the system Σ is locally weakly observable. It should be noted that the full-rank condition of the observability matrix O(x) is only sufficient to say that the system is locally weakly observable. In the case that the observability matrix O(x) is rank-deficient, the system’s behavior has to be verified through computer simulations and experimental testing.

3.1. Sensor Modeling

In response to the time-variant body angular velocity ω_body, acceleration (constant gravity g and time-variant body acceleration a_body), and local magnetic field (constant earth’s magnetic field h and any time-variant magnetic variation ^hb acting nearby the sensor) the measured outputs of the gyro, the accelerometer and the magnetic sensor from an integrated IMU can be written as follows:

The scale factor and bias of inertial and magnetic sensors are functions of environmental conditions, in particular ambient temperature. Across the thermal variations typically encountered in practice, thermal effects on accelerometers are of relatively lower quantitative relevance than on gyros, and they are usually negligible on magnetic sensors. Moreover, scale factor drifts of inertial and magnetic sensors usually affect the accuracy of the measurement process to a much lesser extent than the bias drifts of these sensors, in particular gyros. Scale factor and bias errors of accelerometers can be compensated on time scales up to few hours, using the procedure described in [26]; the procedure described in [27] can be used to calibrate scale factors and hard iron offsets ^mb, on similar time scales.

A more serious problem concerns the bias errors of gyros and how they develop over time. This problem is well received in the literature, where orientation estimators are oftentimes developed with built-in devices for gyro-bias compensation. A random-walk vector random process with statistically independent components is widely used for carrying the compensation task in a Kalman filtering framework [28]:

For slowly moving bodies in magnetically clean environments, gyro-free orientation determination systems have been developed with mixed success [3,11]. In these systems a further simplification is made in the sensor model Equation (18), where the only acceleration measured by the accelerometer is gravity. Henceforth, we make the simplifying assumption that a_body ≈ 0.

3.2. Modeling the Magnetic Variation

A magnetic sensor measures the Earth’s magnetic field plus any other magnetic field superimposed to it. For the purpose of orientation determination, an accurately known homogeneous magnetic field in the environment surrounding the tracked body is needed as the horizontal reference for heading estimation. Especially indoors, magnetic homogeneity is difficult to achieve, due to construction iron in floors, walls and ceilings, or to various equipment. The magnetic distortions occur in both the horizontal and vertical plane [6,7]. Suppose that, albeit not homogeneous, the magnetic field existing in a given region of space is at least time-invariant. In this case, an interesting possibility to deal with the problem of magnetic distortions would consist, in principle, of calibrating and mapping the measurement volume. Besides being complicated and time-consuming, this approach is also in contrast with the development of ambulatory sensor systems, whose prior knowledge of existence and location of disturbances cannot be taken for granted: for instance, current IMUs do not produce position information, which makes critical using the magnetic maps of the measurement volume.

IMUs are exposed to magnetic fields that can rapidly change in direction and magnitude, when they move relative to their surroundings in a magnetically non-homogeneous environment. These magnetic distortions, generically denoted with the term magnetic variation ^hb, can be expressed in the earth-fixed frame. In order to prevent heading drift, the orientation estimator has to be developed with a built-in device for compensation of magnetic variations. A first-order Gauss-Markov vector random process with statistically independent components is chosen to model the magnetic variation:

3.3. Filter Implementation

The main difficulty of using quaternion-based state vector components in an EKF lies in the formulation of the filter equations. For a review of these equations with a generic state-space model see [30]. The origin of the problem lies in the lack of independence of the four components of a quaternion, since they are related by the unit-norm constraint. Constraints imposed on the estimated state variables cannot be preserved by EKFs in their standard development [19]. A popular method to preserve the quaternion unit-norm property is to normalize the a posteriori estimate after the measurement update stage (“brute-force” approach). Even though it is neither elegant nor optimal [31], the “brute-force” approach is proven to work generally well [32]. The EKF developed in this paper enforces the unit-norm constraint by the “brute-force” approach [2].

Since the angular velocity is measured from a body-fixed tri-axial gyro, the kinematic equations of a rigid body can be used to obtain the orientation state [19]. Gyro data are treated as external inputs to the filter rather than as measurements, and gyro measurement noise and bias enter the filter as process noise rather than as measurement noise. An advantage of this choice is the reduction in the dimension of the state vector, which may lead to minimal-order, computationally efficient filter implementations.

An important feature of EKFs is the possibility they offer to estimate unknowns by state vector augmentation techniques. In this paper, we concentrate on the self-compensation of gyro bias and magnetic variation, both accounted for in the filter as additional state vector components.

The continuous-time system model combines (9)-(19)-(20) together:

The state vector in Equation (21) is $\mathbf{\text{x}}={[\begin{array}{lll}{\overline{\mathbf{\text{q}}}}^T& {}^h{\mathbf{\text{b}}}^T& {}^g{\mathbf{\text{b}}}^T]\end{array}}^T$. The equations for propagating the state estimates in the model are obtained by applying the expectation operator to Equation (21). By rearranging the equations in a format suitable for computing the Lie derivatives [23], we obtain:

In the following we drop the caret, which denotes the expectation of a random variable, to avoid unnecessary cluttering in the notation. The discrete-time model allows employing the sampled measurements of the IMU for state propagation:

The process noise component ^qw(k − 1) describes how the gyro measurement noise enters the state model through a quaternion-dependent linear transformation, as follows:

The process noise components ^qw(k − 1), ^hw(k − 1), ^gw(k − 1) are assumed uncorrelated; hence, the process noise covariance matrix Q(k − 1) is shown to have the block-diagonal structure:

Equation (18) is used to model the sensor measurements. Each reference vector component needed for vector matching is given a specific equation:

The initialization procedure is carried out as follows: the processing of gyro data does not include the procedure of bias capture; an initialization error is thus introduced by setting the gyro bias component of the state vector to zero. The magnetic field is not taken from a magnetic field model, but is rather computed as follows. The aiding sensor measurements are averaged during the initial rest period (averaging time: 1s). Inclination is estimated by processing the acceleration average vector. The magnetic average vector is then projected in the horizontal plane using the estimated inclination, which allows estimating h in the earth-fixed frame. Finally, we apply the TRIAD method to the acceleration and magnetic average vectors, using the gravity vector g and the estimated value of h as reference vectors [3,33]. The initial quaternion and its covariance matrix are then computed.

3.4. Filter Observability

The observability test consists of demonstrating that the state space of the system with input-linear process function described by Equation (22) is spanned by the gradients of the Lie derivatives of the following measurement functions:

The measurement function φ₃(x) is introduced to account for the quaternion normalization constraint [23]. For its relevance in performing the observability analysis, we introduce, for a generic vector p, the function:

Using Equation (2) we obtain:

Since the zero-order Lie derivatives are the measurement functions themselves, the gradients of the zero-order Lie derivatives are the Jacobian matrices of the measurement functions:

The first-order Lie derivatives of φ₁ and φ₂ (with respect to f₀) can be computed as:

The gradients of the first-order Lie derivatives of φ₁ and φ₂ (with respect to f₀) are then taken:

The next Lie derivative of interest is the first-order Lie derivative of φ₂ with respect to f₁:

The gradient of the first-order Lie derivative of φ₂ with respect to f₁ is obtained by expanding the Lie derivatives as column vectors. These column vectors are stacked together as follows:

Finally, we obtain the 22 × 10 observability matrix O(x):

The salient steps of the observability proof are reported in Appendix A, where the matrices X₁, X₂, X₃, and hence the matrix D, do not need to be computed explicitly. In short, the system of Equation (22) is certainly locally weakly observable when: (a) the magnetic field has components in the horizontal plane, which is necessary to have the horizontal reference for heading estimation; (b) at least one degree of rotational freedom is excited. Remind that the magnetic dip angle, also called magnetic inclination, is the angle the Earth’s magnetic field makes with the surface of the Earth; the observability condition (a) fails when the dip angle is ±90° (e.g., close to the Poles). Running the estimation algorithm during periods when the observability is not guaranteed—when the magnetic dip angle is close to ±90° or the IMU is in quasi-static conditions—is likely to degrade seriously the state estimation accuracy. This fact has to be verified by assessing the system’s behavior by computer simulations and experimental testing.

3.5. Filter Assessment

The performance metrics are based on computing the error quaternion $\Delta \overline{\mathbf{\text{q}}}={\overline{\mathbf{\text{q}}}}_{\mathrm{t}}^{-1}\otimes \overline{\mathbf{\text{q}}}$, where q̄_t and q̄ are the true and the estimated quaternion, respectively. In the present context the term true implies the use of synthetic or experimental motion data to carry out the test procedure. An obvious advantage of working with synthetic signals is that the errors incurred in estimating the state vector components can be compared with the bounds that are predicted by the error covariance matrix produced by the EKF. This is a useful feature to assess the filter convergence and to diagnose a number of potential problems arising in its numerical implementation. Experimental motion data can be captured, e.g., from an optoelectronic motion tracker, so as to define the ground truth reference for filter assessment.

The error quaternion represents the rotation that brings the estimated body frame onto the true body frame. Its scalar component can be used to derive the orientation error Δθ, according to the equation Δθ = 2arccos(Δq₄) The performance metrics are expressed in terms of the root-mean-square-value of the orientation error (RMSE_q), averaged over the number of either the Monte Carlo simulation runs or the experimental trials available. Alternatively, a set of estimated and reference Euler angles (roll, pitch, yaw or heading) can be computed from q̄_t and q̄ using standard conversion formulas, and the filter performance can be summarized by presenting the RMSEs of the Euler angles, again averaged over the number of either the Monte Carlo simulation runs or the experimental trials available. Although the calculations for orientation are not performed using Euler angles, the results can be presented in this way for better interpretation.

3.6. Computer Simulations

The body-fixed frame has a fixed origin, and it is aligned with the Earth-fixed frame at the initial time of each simulation trial. Our choice of the earth-fixed frame follows the North-East-Down (NED) convention: the gravity vector is g = [0 0 g]^T (g = 9.81 m/s²), and the earth magnetic field is h = [h_x 0 h_z]^T (h_x = 0.26 Gauss; h_z = 0.37 Gauss). The simulated field strength is approximately 0.45 Gauss, with a magnetic dip angle δ ≈ 55°, situations typical of our latitudes. A magnetically perturbed environment is simulated by adding h with the output of a routine that generates samples from a Gauss-Markov vector random process with statistically independent components.

The angular velocity time functions are given as input to the simulation program. The true quaternion time functions are obtained by time integrating the kinematic Equation (9) at the sampling frequency of f_s = 4 kHz, which is high enough to make integration errors negligible; the quaternion time functions are then time-decimated down to f_s = 100 Hz (the sampling frequency used in many popular orientation trackers). Two distinct conditions are tested: static conditions, in which case the angular velocity is null, and then the IMU does not change the initial orientation; dynamic conditions, i.e., following an initial period of rest, a pure sinusoidal rotation around the vertical axis is simulated (amplitude: 100°/s; frequency: 1 Hz). In either case simulation trials last 10 min.

The magnetic field and the gravity vector can be expressed in the body-frame using the computed quaternions. White Gaussian noise is then injected into the body-referenced sensor time functions to simulate the effect of specified amounts of measurement noise: σ_a = 1 − 5 mg; σ_h = 1 mGauss. In dynamic conditions the value of σ_a is chosen relatively high as compared with values measured during on-bench calibration tests of MEMS accelerometers, which are less than 1 mg; this is to account for the minute motions that affect on-body IMUs, even when the body is quasi-static. No scale factor or bias errors are considered for the IMU sensors, with the exception of the gyro: the gyro time functions are corrupted by addition of a Gaussian white measurement noise component with standard deviation σ_g = 0.4°/s and a fixed offset (e.g., b_g = [1 − 0.5 0.75]^T °/s).

The different treatment of IMU sensors is motivated by the weight an uncompensated gyro bias has on the error budget of an inertial orientation sensor and consequently by our desire to always enable the compensation of gyro bias in our filter. The following two EKF implementations are considered: EKF-M_A and EKF-M_B, which differ in enabling the compensation of magnetic disturbances or not; in order to disable the compensation of magnetic disturbances, ^bσ_h = 0 and α = 0 are plugged in Equation (27). The two methods are tested in static and dynamic conditions, when either the magnetic variation is markovian, namely it follows the Gauss-Markov model of Equation (20) (magnetically perturbed environment, MPE), or when the magnetic variation is null (magnetically clean environment, MCE). For each combination of method and condition, N = 10 Monte Carlo runs are performed, and the values of orientation RMSE_q are reported (mean ± standard deviation, SD). Another set of simulations is carried for different values of the magnetic inclination, from 55° to 90° (dynamic conditions—method M_A). The aim is to verify the filter behavior in difficult observability conditions. The filter parameter setting is given in Table 1.

3.7. Experimental Validation

The MTx orientation tracker by Xsens Technologies B.V. (Enschede, The Netherlands) is used for carrying out the experimental validation. The raw sensor data are delivered through a USB interface to the host computer at a rate of 100 Hz, together with the Euler angle time functions estimated by the proprietary EKF, henceforth called the Xsens-EKF. The IMU sensors are calibrated before starting the experimental session as described in [26,27]; by contrast, no bias capture is considered for the gyro. The initial orientation of the sensor frame relative to the marker frame is found by taking data during the rest period when the IMU is still.

The tests for validation in static and dynamic conditions are carried out within one of our lab rooms. The static test, whose duration is 10 min, consists of leaving the IMU still on a table that is placed far from current wires, computer appliances, and ferromagnetic materials. A magnetic disturbance is induced at the time when a cell phone is placed close to the IMU; the cell phone is left in place for a while, then it is removed, so as to recover the initial magnetic field. The test for validation in dynamic conditions is performed as follows. The IMU is fastened to a 50 cm 50 cm wooden plate using double-side adhesive tape. The plate is raised by hand slightly over the table and then it is freely moved around; toward the end of the trial, which lasts about 137 s, the plate is replaced approximately in the same pose. The working volume visited by the plate during the motion trial is 100 cm × 50 cm × 60 cm. The plate orientation is recorded using a six-camera Vicon optical motion capturing system with a sampling rate of f_s = 100 Hz. The IMU and Vicon data streams are electronically time-synchronized. The Vicon system measures the position of four reflective markers (diameter: 14 mm) arranged at the corners of the plate to form a square with side 45 cm. The accuracy of the marker frame is determined by analyzing the relative motion of two markers: the RMS of the Vicon orientation error Δθ is assumed in the same order as the RMS distance variation divided by the distance between two markers (Δθ < 0.5°). The RMSEs of the Euler angles and the RMSEq of the orientation are presented using the Euler angles time functions computed by the Vicon system as the truth reference. The filter parameter setting we choose for the experimental validation is reported in Table 2. The orientation sensor delivers the data from the magnetic sensor in arbitrary units.

4.1. Computer Simulations

The Monte Carlo results are reported in Table 3. The EKF-M_A performs better than the EKF-M_B in a magnetically markovian environment, according to a paired-sample t-test (statistically significant difference, SSD at p < 0.001 and p < 0.01, in dynamic and static conditions). In the case of null magnetic variations, the EKF-M_A performs as the EKF-M_B. Figure 1 depicts the typical behavior in dynamic conditions. As for the second condition of observability, the EKF-M_A is tested in the same conditions leading to the data in Figure 1, for different values of the magnetic dip angle, Figure 2.

4.2. Experimental Validation: Static Test

In Figure 3 we plot, in blue, the component of the magnetic field along the X-direction (earth-fixed frame), as is measured by the magnetic sensor. Since the test environment is almost magnetically unperturbed before placing the cell phone nearby the IMU at time t₁ = 476 s, the measured magnetic field is the true magnetic field existing in the room with very good approximation. The EKF-M_A is tested using three different values of the process noise standard deviation ^bσ_h.

All the other parameters being the same as in Table 2, these values are: ^bσ_h = 0.001 a.u./s (red); ^bσ_h = 0.01 a.u./s (black); ^bσ_h = 0.1 a.u./s (magenta). The RMS value of the difference between estimated and measured X-components of the magnetic field up to time t₁ increases from 15 × 10⁻⁵ a.u. (^bσ_h = 0.001 a.u./s) to 8.4 × 10⁻³ a.u. (^bσ_h = 0.1 a.u./s). The magnetic disturbance from t₁ to when the cell phone comes to a stop at time t₂ = 480 s is best tracked with higher values of ^bσ_h. However, when the magnetic field is not time variant anymore from time t₂ on, the filter behavior is unsatisfactory. The estimated heading angle is reported in Figure 4.

4.3. Experimental Validation: Dynamic Test

The norm of the acceleration magnitude is 1 g ± 60 mg throughout the trial, and the coefficient of variation of the norm of the measured magnetic field is about 1%. The RMSE of the Euler angles are reported in Table 4 for the EKF-M_A and the EKF-M_B. As a matter of comparison, we also report the errors statistics in three other cases: (a) when only the gyro angular velocities are used and the aiding sensors are disabled; (b) when only the accelerometer and magnetic sensor data are involved in determining the orientation using the TRIAD method; (c) when the orientation is estimated by the Xsens-EKF. The Euler angle time functions estimated by the Vicon system are reported with superimposed the errors incurred by the EKF-M_A in Figure 5.