For motion on or near the earth surface, at speeds far below orbital velocity, it is convenient to describe the orientation of a rigid body using two coordinate systems: the earth-fixed coordinate system, specified by the right-handed orthonormal basis E = {e₁ e₂ e₃}, whose coordinate axes are directed in the local north, east and down directions (NED)—for all practical purposes, an inertial coordinate system; the non-inertial coordinate system, aka body-fixed coordinate system, specified by the right-handed orthonormal basis B = { e 1 ′     e 2 ′     e 3 ′ }, whose coordinate axes are conventionally named “out the nose”, “out the right side” and “out the belly” in the aeronautics jargon (Figure 1).

We recall that an orthonormal basis T = {i j k} is said to be right-handed if it satisfies: (1) i × j = k ,         j × k = i ,         k × i = j   .where the symbol × denotes the standard vector cross product.

An arbitrary vector x in the 3D space can be written in the equivalent forms: (2) x = x 1   e 1 + x 2   e 2 + x 3   e 3 x = x 1 ′ e 1 ′ + x 2 ′   e 2 ′ + x 3 ′   e 3 ′ .

The vector x can therefore be represented in terms of the coordinates (or components) with respect to either basis: (3) x E = [ x 1     x 2     x 3 ] T x B = [ x 1 ′     x 2 ′     x 3 ′ ] T .

The subscripts E, B indicate which basis is used for representing the vector x. The representations in (3) are related to one another as follows: (4) x B = E B C   x E .

The columns of the direction cosine matrix (DCM) E B C are the representations of the e_i, i = 1,…,3 with respect to B, while its rows are the representations of the e i ′, i = 1,…,3 with respect to E. The DCM, also called orientation (attitude) matrix, and its transpose allow therefore moving vector representations from (to) the earth-fixed frame to (from) the body-fixed frame, respectively. The orientation matrix is a 3 × 3 orthogonal matrix with unit determinant, which belongs to the three-dimensional special orthogonal group SO(3) of rotation matrices. Although the orientation matrix is the fundamental representation of the orientation, the orthogonality requirement forces six constraints on its nine elements, namely the column (row) vectors have unit norm and are mutually orthogonal, yielding that the special orthogonal group SO(3) of rotation matrices has dimension three.

Lower-dimensional parameterizations of orientation can be derived based on the following considerations [30]. As shown in Figure 2, a rotation about the e₃-axis through an angle θ is expressed as: (5) { e 1 ′ = cos   θ   e 1 + sin   θ   e 2 e 2 ′ = − sin   θ   e 1 + cos   θ   e 2 e 3 ′ = e 3 .

The resulting rotation matrix is: (6) R ( e 3 , θ ) = [ c θ s θ 0 − s θ c θ 0 0 0 1 ] .where c_θ and s_θ are compact notation for cosθ and sinθ, respectively. By analogy with (6), the rotation matrices that describe rotations about the e₂-axis and the e₁-axis through an angle θ are: (7) R ( e 2 , θ ) = [ c θ 0 − s θ 0 1 0 s θ 0 c θ ]         R ( e 1 , θ ) = [ 1 0 0 0 c θ s θ 0 − s θ c θ ] .

We note that: (8) { R ( e 3 , θ )   e 1 = c θ   e 1 − s θ   e 2 = c θ   e 1 − s θ   e 3 × e 1 R ( e 3 , θ )   e 2 = c θ   e 2 + s θ   e 1 = c θ   e 2 − s θ   e 3 × e 2 R ( e 3 , θ )   e 3 =   e 3 .

For the vector cross product, an equivalent expression is: (9) u × v = [ u × ]   v ,where [u ×] is the skew-symmetric matrix: (10) [ u × ] = [ 0 − u 3 u 2 u 3 0 − u 1 − u 2 u 1 0 ] .

Be n any unit column vector, and be v_⊥ the projection of a column vector v onto the plane perpendicular to n (Figure 3). By analogy with (8) we can write: (11) { R ( n , θ )   v ⊥ = c θ   v ⊥ − s θ   [ n × ]   v R ( n , θ )   n = n .

For arbitrary vectors a, b and c the Grassman identity yields: (12) a × ( b × c ) = ( a ⋅ c )   b − ( a ⋅ b )   c .

The symbol □ denotes the standard vector dot product. Equation (12) allows deriving the general decomposition of v into components that are parallel (v_P) and perpendicular (v_⊥) to n: (13) n × ( n × v ) = ( n ⋅ v )   n − ( n ⋅ n )   v   →   v = nn T v − [ n × ] 2   v = v P + v ⊥ .

In conclusion: (14) R ( n , θ )   v = R ( n , θ )   ( v P + v ⊥ ) = v P + c θ   v ⊥ − s θ   [ n × ]   v ⊥ .

It follows that two equivalent expressions of the rotation matrix are written (Euler’s formula): (15) { R ( n , θ ) = c θ   I 3 + ( 1 − c θ )   nn T − s θ   [ n × ] R ( n , θ ) = I 3 − s θ   [ n × ] + ( 1 − c θ )   [ n × ]   2 .where I_n denotes the n × n identity matrix.

Euler’s theorem states that the most general motion of a rigid body with one point fixed is a rotation by an angle θ (rotation angle) about some axis n (rotation axis), yielding another representation of the orientation in terms of the rotation vector: (16) θ = θ   n .

All rotations can thus be mapped to points inside and on the surface of a sphere of radius π in rotation vector space (θ ∈]−π, π]). Since points at opposite ends of any diameter of the sphere represent the same orientation, the parameterization of orientation through the rotation vector is redundant, with four parameters and one constraint enforced on its norm. Moreover, no points of singularity exist in the rotation vector space.

The orientation of the body-fixed frame relative to the earth-fixed frame can also be described using the Euler angle formulation, namely in terms of three consecutive rotations through three body-referenced Euler angles [31]. Although, in principle, twelve possible ways exist to define three independent body-referenced Euler angles, just a subset of them have received attention; we discuss here the 3-2-1 rotation sequence, which is the one commonly adopted in the aeronautics community. The orientation of the body-fixed frame (nose-wing-belly) relative to the earth-fixed frame (NED) is described by performing the three rotations as follows. Start with a body-fixed frame in the reference orientation, i.e., one in which all of its body-fixed axes are aligned with the corresponding earth axes; first, the body is rotated about the belly axis through an angle ψ usually called heading angle, or yaw (ψ ∈ ]−π, π]); second, the object is rotated about the wing axis through an angle ϑ (elevation angle, or pitch attitude) (ϑ ∈]−π/2, π/2]); third, the object is rotated about the nose axis through an angle φ (bank angle, or roll attitude), so as to match the body-fixed frame (φ ∈]−π, π]). We can then write: (17) x B = [ 1 0 0 0 c φ s φ 0 − s φ c φ ]   [ c ϑ 0 − s ϑ 0 1 0 s ϑ 0 c ϑ ]   [ c ψ s ψ 0 − s ψ c ψ 0 0 0 1 ]   x E .

The rotation matrix as a function of the three Euler angles is given by: (18) R ( ψ , ϑ , φ ) = [ c ϑ c ψ c ϑ s ψ − s ϑ s φ s ϑ c ψ − c φ s ψ s φ s ϑ s ψ + c φ c ψ s φ c ϑ c φ s ϑ c ψ + s φ s ψ c φ s ϑ s ψ − s φ c ψ c φ c ϑ ]   .

The gravity vector is therefore represented in the body-fixed coordinate system as follows: (19) g B = [ c ϑ c ψ c ϑ s ψ − s ϑ s φ s ϑ c ψ − c φ s ψ s φ s ϑ s ψ + c φ c ψ s φ c ϑ c φ s ϑ c ψ + s φ s ψ c φ s ϑ s ψ − s φ c ψ c φ c ϑ ]   [ 0 0 g ]   = g [ − s ϑ s φ c ϑ c φ c ϑ ]   .

Equation (19) shows that a body-fixed tri-axial accelerometer does not convey heading information.

The time rates of change of the Euler angles are related to the components of the angular velocity ω_B = [p q r]^T resolved in the body-fixed frame by the following system of first-order nonlinear differential equations [31]: (20) [ d φ dt d ϑ dt d ψ dt ] = [ 1 s φ s ϑ c ϑ s φ s ϑ c ϑ 0 c φ − s φ 0 s φ c ϑ c φ c ϑ ]   [ p q r ]   .

Equation (20) can be used to update the orientation of the rigid body in time given the angular velocity. As for all unconstrained representations of orientation, Euler angles suffer from singularities, commonly referred to as gimbal-lock: for instance, in the case of the 3-2-1 rotation sequence, if the pitch angle ϑ is ± π/2, the last two terms of the first and last rows in (20) go to infinite and the Euler angle integration becomes indeterminate. Gimbal lock corresponds to loosing a degree of freedom in the rotation matrix (18); for instance, when ϑ is π/2, the rotation matrix becomes: (21) R ( ψ , π 2 , φ ) = [ 0 0 1 s φ − ψ c φ − ψ 0 c φ − ψ − s φ − ψ 0 ]   .

The rotation depends on the difference φ –ψ; only one degree of freedom therefore exists instead of two. In other terms, changes of φ and ψ result in rotations about the same axis.

Finally, since matrix multiplication is not generally commutative, finite rotations in space do not commute, unless infinitesimal rotation angles δψ, δθ, δφ are considered, in which case we have: (22) R ( δ ψ , δ ϑ , δ φ ) ≈ [ 1 δ ψ − δ ϑ − δ ψ 1 δ φ δ ϑ − δ φ 1 ]   .

In fact, the Euler’s formula (15) can be approximated to first order as follows: (23) R ( n , δ θ ) ≈ I 3 − [ δ θ × ] + O ( | δ θ | 2 ) ≈ [ 1 δ θ 3 − δ θ 2 − δ θ 3 1 δ θ 1 δ θ 2 − δ θ 1 1 ]   .where δθ = δθn is the infinitesimal rotation vector, and its components δθ₁, δθ₂, δθ₃ are termed infinitesimal angles.

Finally, another mathematical representation of orientation can be constructed by rewriting the Euler’s formula (15) as follows: (24) R ( q , q 4 ) = [ q 1 2 − q 2 2 − q 3 2 + q 4 2 2 ( q 1 q 2 + q 3 q 4 ) 2 ( q 1 q 3 − q 2 q 4 ) 2 ( q 1 q 2 − q 3 q 4 ) − q 1 2 + q 2 2 − q 3 2 + q 4 2 2 ( q 2 q 3 + q 1 q 4 ) 2 ( q 1 q 3 + q 2 q 4 ) 2 ( q 2 q 3 − q 1 q 4 ) − q 1 2 − q 2 2 + q 3 2 + q 4 2 ]   .

The rotation matrix (24) is formulated as a homogeneous quadratic function of the quantities q_i, i = 1,…,4, called the Euler-Rodrigues symmetric parameters or quaternion [32]: (25) R ( q , q 4 ) = ( q 4 − | q | 2 )   I 3 + 2 qq T − 2 q 4 [ q × ] ,where: (26) q = [ q 1 q 2 q 3 ] = sin   ( θ 2 )   n ,   q 4 = cos   ( θ 2 ) .

It is commonplace to refer to q as the vector part and to q₄ as the scalar part of the quaternion q̄ = [q^T q₄]^T. As implied by (26), the rotation quaternion satisfies the simple normalization constraint: (27) q 4 + | q | 2 = 1 .

The following two basic operations are defined in the quaternion space: (28) addition q ¯ ⊕ q ¯ ′ = [ ( q + q ′ ) T   q 4 + q 4 ′ ] T multiplication q ¯ ⊗ q ¯ ′ = [ ( q 4 ′ q + q 4 q ′ + q × q ′ ) T   q 4   q 4 ′ − q ⋅ q ′ ] T

In contrast with quaternion addition, quaternion multiplication is not generally commutative. Moreover, by analogy with complex numbers, we define the conjugate of a quaternion; the definitions of quaternion norm and inverse follow: (29) conjugate q ¯ * = [ − q T   q 4 ] T   →   q ¯ * ⊗ q ¯ = [ 0 T   q 4 + | q | 2 ] T norm | q ¯ | = q ¯ * ⊗ q ¯ = q 4 + | q | 2 = ∑ i = 1 4 q i 2 inverse q ¯   − 1   such that   q ¯ − 1 ⊗ q ¯ = [ 0 T   1 ] T   →   q ¯   − 1 = q ¯ * | q ¯ | 2

The Euler-Rodrigues formulation predates the discovery of quaternions by Hamilton, who was not apparently interested in developing quaternion algebra as means of describing rotational transformations [31]. Hamilton’s quaternions can be considered as 4-component extended complex numbers of the form: (30) q ¯ = q 1   i + q 2   j + q 3   k + q 4 ,whose imaginary components i, j, k have the computation rules: (31) i ⊗ i = j ⊗ j = k ⊗ k = i ⊗ j ⊗ k = − 1 ,   i ⊗ j = k ,   j ⊗ i = − k .

Alternatively, quaternions can be considered as vectors embedded in the four-dimensional Euclidean space R⁴. The set of quaternions with null vector parts can be identified with R the set of quaternions with null scalar part, aka vector quaternions, can be identified with vectors in the Euclidean space R³. At last, unit quaternions, namely quaternions with unit norm, lie on the three-dimensional sphere S³ with unit radius in R⁴. Henceforth, the vector presentation is used in place of the representation as extended complex numbers.

The connection existing between unit quaternions and the problem of describing orientations starts with examining (24–26). Given the vector quaternion p̄ = [p^T 0]^T, the vector quaternion: (32) p ¯ ′ = q ¯   − 1 ⊗ p ¯ ⊗ q ¯is shown to be p rotated about the n-axis through an angle θ [32]. Any general three-dimensional rotation θ about an arbitrary unit vector n can be therefore described by a unit quaternion. The rule of composition of rotations is achieved by multiplying the corresponding quaternions. Let q̄₁ and q̄₂ be arbitrary unit quaternions. Rotation by q̄₁ followed by rotation by q̄₂ is shown equivalent to rotation by q̄₂ ⊗ q̄₁ (33) R ( q ¯ ) = R ( q ¯ 2 )   R ( q ¯ 1 )   ↔   q ¯ = q ¯ 2 ⊗ q ¯ 1

The four-component unit quaternion has the lowest dimension of any globally non-singular orientation parameterization. Enforcing the unit norm constraint on a quaternion leaves it with the three degrees of freedom consistent with the SO(3) dimensionality. Moreover, the quaternion representation is redundant, as the rotation vector. The quaternion −q̄ represents the same rotation as q̄ a rotation through the angle θ about the n-axis can also be expressed as a rotation through an angle −θ about the n’-axis (n’ = −n).

The kinematic equations that describe the motion of a rigid body capture the relations existing between the temporal derivative of the orientation representation and the angular velocity; we have already discussed the formulation of these equations in the case that the 3-2-1 rotation sequence of Euler’s angles are chosen for representing the orientation, see (20).

Suppose that the orientation changes with time: E B C   ( t + δ t ), i.e., the rotation matrix representing the orientation at time t + δt, differs from E B C   ( t ), the rotation matrix at time t, see (23): (34) E B C   ( t + δ t ) = Φ   ( t , t + δ t )   E B C   ( t )   ←   Φ   ( t , t + δ t ) = I 3 − [ δ θ   ( t ) × ] + O ( | δ θ | 2 ) .where δ θ(t) is the infinitesimal rotation vector. We can write: (35) E B C   ( t + δ t ) − E B C   ( t ) δ t = − [ δ θ   ( t ) × ] δ t E B C   ( t ) .

Taking the limit of (35) as δ t tends to zero, one obtains a system of first-order linear differential equations, aka the Poisson’s kinematic equations: (36) d dt E B C = − [ ω B × ] E B C .where ω_B is the body-referenced angular velocity, defined as: (37) ω B = lim δ t → 0 δ θ δ t .

The time dependence of the angular velocity and the rotation matrix is not made explicit in (36)–(37) to avoid unnecessary cluttering of the notation. Alternatively, the time evolution of a time-varying quaternion with angular velocity ω_B is given by the solution to the following system of first-order linear differential Equation [32]: (38) d dt q ¯ = 1 2 q ¯ ⊗ [ ω B T   0 ] T = 1 2 [ − [ ω B × ] ω B − ω B T 0 ]   q ¯ = Ω ( ω B )   q ¯ ,where Ω(ω_B) is a 4 × 4 skew symmetric matrix. If the angular velocity is time constant, then the closed-form solution to (38) with given initial conditions is given by: (39) q ¯   ( t ) = Φ   ( t , t 0 ; ω B )   q ¯   ( t 0 ) = exp   [ ( t − t 0 ) Ω   ( ω B ) ]   q ¯   ( t 0 ) .

The matrix exponential can be written: (40) Φ ( t 0 , t ; ω B ) = cos   ( | ω B | t − t 0 2 )   I 4   +   sin   ( | ω B | t − t 0 2 ) | ω B | t − t 0 2 Ω ( ω B ) .

It is worth noting that all three-dimensional representations of orientation are invariably associated to non-linear kinematic equations; on the other hand, higher-dimensional representations of orientation, such as the orientation matrix and the quaternion, present linear kinematic equations, as shown in (36–38).

The gimbal-lock singularity and the presence of computationally taxing trigonometric functions in the numerical integration of the system (20) are critical elements against the choice of the Euler angles. Mathematically, (36) preserves the orthogonality of the orientation matrix, although errors associated with its numerical integration can cause some degradation in the orthogonality of the matrix, which forces to adopt suitable methods to recover it [33]. Errors associated with numerical integration of the kinematic equations for orientation have been analyzed and characterized for both the rotation matrix and the quaternion parameterizations, and the superiority of the latter is widely recognized [33,34]. In addition to that, another relevant advantage of the quaternion formulation is vastly increased computational speed: trigonometric functions are not to be computed, with further savings that are provided by the reduced number of floating operations involved in numerically integrating (38) as compared with (36) [31].

The claim that the physical interpretation of the quaternion is much less intuitive than that associated with Euler angles does not imply that the quaternion cannot find ample diffusion even in the biomechanical community. A systematic development of the kinematics equations is possible in terms, equivalently, of direction cosines, the rotation vector, Euler angles, the quaternion, and well-established relationships link all these descriptors to one another: at any stage of the processing and visualization tasks, one can adopt the descriptor that is more suited to the application specifics. Henceforth, we direct our attention exclusively to the quaternion-based formulation of the kinematic equations of a rigid body.

Deterministic single-frame estimation algorithms can be proposed in connection with the operation of gyro-free aiding sensor systems. Four variants of the same approach are surveyed here: TRIAD (TRi-axial Attitude Determination), QUEST (QUaternion ESTimator), FQA (Factored Quaternion Algorithm) and Gauss-Newton (GN) optimization. They can be used to solve Wahba’s problem without the need for an a priori estimate. They all are based on the concept of vector matching, which requires, in principle, that measurements of constant reference vectors (e.g., gravity and earth magnetic field) are performed. In their original formulation, they are unable to provide sequential estimates of a time-varying orientation and of other parameters than the orientation, such as sensor biases. In the presence of uncompensated sensor biases, the estimated orientation can be therefore grossly inaccurate.

Suppose that two nonparallel reference unit vectors v₁, v₂ are available, e.g., in the direction of the gravity field and the earth magnetic field, and resolved in the earth-fixed frame. The corresponding observation vectors w₁, w₂ are measured in the body-fixed frame and normalized in amplitude to one. The TRIAD algorithm attempts to solve Wahba’s problem by finding an orthogonal matrix A such that the pair (w₁, w₂) is optimally related to the pair (v₁, v₂), namely Av_i = w_i, i = 1, 2, which gives rise to an over-determined system of algebraic equations [40].

First, two triads of orthonormal reference and observation vectors are constructed: (41) r 1 = v 1 ;   r 2 = r 1 × v 2 | r 1 × v 2 | ;   r 3 = r 1 × r 2 s 1 = w 1 ;   s 2 = s 1 × w 2 | s 1 × w 2 | ;   s 3 = s 1 × s 2 .

Second, the two orthogonal matrices M_ref and M_obs are formed, and the optimal estimate of the orthogonal matrix A is then computed as follows: (42) { M ref = [ r 1   r 2   r 3 ] M obs = [ s 1   s 2   s 3 ]   →   A = M obs M ref T .

The main disadvantage of the TRIAD algorithm is that it is sensitive to the order at which the algorithm receives the two vector pairs—the pair (v₁, w₁) is received first in (41). In fact, part of the information conveyed by the second vector pair is discarded: the cross products that are needed to compute r₂ and s₂ eliminate any contribution of v₂ and w₂ relative to the vertical axis. Since the accuracy of the orientation estimate is more influenced by the vector pair that is processed first, the best choice would be to process first the observation vector of greater accuracy. Another disadvantage of the TRIAD algorithm is that it accommodates only two observation vectors.

The basic QUEST delivers the optimal quaternion that minimizes the loss function: (43) L ( A ) = 1 2 ∑ i = 1 n a i | w i − Av i | 2 .

The loss function (44) can be transformed into a quadratic gain function of the unit quaternion: (44) G ( A ( q ¯ ) ) = q ¯ T K   q ¯where K is a 4 × 4 matrix constructed from the reference vectors v_i, measurement vectors w_i, and weighting coefficients a_i. The optimal unit quaternion is proven to be the eigenvector of the K matrix corresponding to its largest eigenvalue [40].

In contrast with the TRIAD algorithm, the QUEST is capable of accommodating more than two observation vectors; moreover, it is optimal with sensors with different accuracies by properly selecting the weighting coefficients a_i. Although the quaternion produced by the QUEST is unit-norm and globally non-singular, a method is needed for avoiding the singularity that arises when the angle of rotation is π. In fact, the QUEST uses a three-dimensional parameterization, namely the Gibbs vector, in its derivation: (45) a g = q ¯ q 4 = n   tan θ 2

The singularity problem is eliminated in the QUEST by employing the method of sequential rotations, at the expense of computational cost [40].

The FQA is specifically created in the attempt to overcome the limitation of both the TRIAD algorithm and the QUEST that orientation errors arise from errors in just one of the sensor data [41]. Suppose that the two reference vectors are the gravity field, g, normalized in amplitude to one (vertical reference); and the earth's magnetic field, or more precisely, the local magnetic field, h, normalized in amplitude to one (horizontal reference). Let g_m and h_m denote the corresponding measurement vectors, normalized in amplitude to one. In the FQA, acceleration data are used in computing the pitch and roll angles, while local magnetic field data are used only in yaw angle computations. This decoupling eliminates the influence of magnetic variations on calculations that determine pitch and roll angles.

Upon examination of (19), the value of the sine of the pitch angle can be expressed as: (46) sin   ϑ = g mx   →   cos   ϑ = 1 − sin 2   ϑ ,   ϑ ∈ ] − π / 2 ,   π / 2 ] .

From trigonometric half-angle formulas, half-angle values are given by: (47) sin ϑ 2 = sign   ( sin   ϑ ) ( 1 − cos   ϑ ) / 2 cos   ϑ 2 = ( 1 + cos   ϑ ) / 2 .yielding the following expression of the pitch quaternion: (48) q ¯ p = [ 0   sin ϑ 2   0   cos ϑ 2 ] T

The values of the sine and cosine of the roll angle can be expressed as: (49) sin   φ = − g my / cos   ϑ cos   φ = − g mz / cos   ϑ .

Exploiting the half-angle formulas, the roll quaternion is given by: (50) q ¯ r = [ sin φ 2   0   0   cos φ 2 ] T

The values of the sine and cosine of the yaw angle can be determined by matching the magnetic field reference vector in the horizontal plane [h_x h_y]^T, normalized in amplitude to one, and the measured magnetic vector [m_x m_y]^T, projected from the body-fixed frame to the horizontal plane via the pitch and roll quaternions, and normalized in amplitude to one: (51) [ cos   ψ sin   ψ ] = [ m x m y − m y m x ]   [ h x h y ] .

Exploiting the half-angle formulas, the yaw quaternion is: (52) q ¯ y = [ 0   0   sin ψ 2   cos ψ 2 ] T

The rule of composition (33) is applied to (48), (50) and (52) to yield the quaternion estimate representing the orientation of the rigid body: (53) q ¯ = q ¯ r ⊗ q ¯ p ⊗ q ¯ y .

Since the FQA uses three angles to derive the quaternion estimate, it suffers from a singularity, which occurs when the pitch angle is ± π/2. In order to circumvent this singularity, a method similar to the one proposed in the QUEST algorithm is adopted in the numerical implementation of the FQA. In essence, the FQA is very similar to the tilt compensation procedure customarily used in a strap-down magnetic compass to derive heading [17], Figure 4.

The last method reviewed in this Section is the GN optimizer [42]. First, construct the error vector as follows: (54) ɛ ( q ¯ ) = [ g m − A ( q ¯ )   g ρ ( h m − A ( q ¯ )   h ) ]where ρ is a suitably chosen weighting factor. Then, the optimal quaternion is computed by minimization of the square of the error vector: (55) q ¯ opt = arg min q ¯ { 1 2 ɛ ( q ¯ )   T ɛ ( q ¯ ) }

The conventional GN method algorithm provides an iterative solution to this problem:

Give an initial guess, q̄₀;

The main merit of a GN optimizer is recognized in its robust estimation capability, nonetheless the intensive calculations required in step (2) and the need for iteratively evaluations until convergence may diminish its importance for real-time applications. It is reported that four-five iterations are requested for typical values of the sensor measurement noise, when the initialization errors are large. Fortunately, during tracking, when errors are much smaller, GN iteration typically converges to sufficient accuracy in only one-two steps. Clever algorithms are reported in the literature in order to reduce the computational burden of GN optimizers [25,42].

For an approach based on single-frame deterministic algorithms to work properly in human motion tracking, the acceleration and magnetic measurement vectors are to be determined by the gravity field and by the reference magnetic field, respectively. However, this assumption can cause serious errors in the orientation solution if any body accelerations and magnetic variations of affect the sensor signals: in principle, only slow motions occurring in magnetically clean environments would be allowed. The widespread practice of low-pass filtering acceleration signals in order to reduce the effect of dynamic motions leads to latency in the estimates produced by the algorithm, with the additional problem of how to optimally select the cut-off frequency of the filter. Alternatively, acceleration measurements would be screened before their use in the algorithm by computing the absolute value of the difference between their norm and the known value of gravity; if the computed value exceeds a preset threshold value, the measurement reliability is considered low. As for the impact of magnetic variations on the reliability of magnetic measurements, a similar approach can be considered by comparing the norm of the sensed magnetic field with the norm of the reference magnetic vector. A more detailed explanation of these vector selection techniques is deferred to Section 5.4, after that, in the next Section, stochastic estimation algorithms are presented and discussed.

Stochastic estimation algorithms use a model for predicting aspects of the time behaviour of a system (dynamic model) and a model of the sensor measurements (measurement model), in order to produce the most accurate estimate possible of the system state. KF algorithms lend themselves perfectly to this task [37]. There appears to be wide consensus that, e.g., in the VR/AR community the KF is recognized “perhaps the perfect tool for elegantly combining multisensory fusion, filtering, and motion prediction in a single fast and accurate framework” [43].

For the sake of generality, our discussion starts here with considering the Bayesian approach to dynamic state estimation [44]; KFs represent a special class of algorithms for recursive Bayesian state estimation. The Bayesian approach is based on propagating the probability density function (PDF) of the system state in a recursive manner through the application of the Bayes’ rule. The state dynamics is modelled as a Markov process: (60) p   ( x k | x 1 : k − 1 ) = p   ( x k | x k − 1 ) ,where x_1:k−1 = [x₁ x₂ … x_k−1] is the collection of the states traversed by the system up to time k−1, included. The state at time t_k is conditioned only on the previous state and it is independent of the past. This allows for a state representation according to the following discrete-time stochastic model: (61) x k = f k − 1   ( x k − 1 , w k − 1 ) ,where f is a (linear or non-linear), generally time-variant function mapping the previous state to the current state, and w_k−1 represents the process noise. Process noise accounts for any mismodelling effects or disturbances in the dynamic model. Here the index k is associated to a continuous-time instant t_k, and the sampling interval T_k−1 = t_k – t_k−1 may be time-dependent, i.e., function of k. Henceforth, we will assume that the sampling interval T_s is constant.

The system state x_k is related to the measurements by the measurement model: (62) z k = h k   ( x k , v k ) ,where z_k is the measured state of the process at time t_k, h is a generally nonlinear time-variant function mapping of the state of the system to the measured state z_k, and v_k represents the measurement noise. The random processes w_k−1 and v_k are assumed to be white, with known PDFs, and mutually independent. The initial state x₀ is assumed to have a known PDF p(x₀) and also to be independent of w_k−1 and v_k.

The goal of filtering can be stated as finding estimates of the states given z_1:k. This requires the calculation of the posterior PDF p(x_k|z_1:k). Suppose that the required posterior PDF p(x_k−1|z_1:k−1) at time t_k−1 is available. The prediction stage involves the dynamic model (61) to obtain the prior PDF of the state at time t_k: (63) p   ( x k | z 1 : k − 1 ) = ∫ p   ( x k | x k − 1 )   p   ( x k − 1 | z 1 : k − 1 )   d x k − 1 .

The transitional PDF p(x_k|x_k−1) is defined by the dynamic model (62) and the known statistics of the process noise w_k−1.

The update stage, at time t_k when a new measurement becomes available, is based on the Bayes’ rule: (64) p ( x k | z 1 : k ) ∝ p   ( z k | x k )   p   ( x k | z 1 : k − 1 ) ,where the likelihood function p(z_k|x_k) is defined by the measurement model (62) and the known statistics of the measurement noise v_k.

The Equations (63)–(64) give a recursive way to propagate the posterior density. Bayesian filtering can thus be seen as a two-stage process, a prediction stage of the new state using (63), and an update stage where the prediction is modified by the new measurement using (64). The knowledge of the posterior PDF p(x_k|z_1:k) allows estimating the state, and obtaining measures of the accuracy of these estimates. The Bayesian solution cannot be determined analytically, expect that in a restrictive set of cases, including the KF.

The KF assumes that the posterior density at every time step is multivariate Gaussian, and hence it can be completely characterized by the mean vector and the covariance matrix. If the PDF p(x_k−1|z_1:k−1) is Gaussian, the PDF p(x_k|z_1:k) is proven to be Gaussian provided that:

w_k−1 and v_k are drawn from Gaussian PDFs with known parameters;

In other words (61)–(62) can be rewritten as: (65) x k = F k − 1 x k − 1 + w k − 1 z k = H k x k + v k ,where F_k−1 (of dimension n_x × n_x) and H_k (of dimension n_z × n_z) are known matrices. The additive noises w_k−1 and v_k are mutually independent zero-mean white Gaussian, with known covariance matrices Q_k−1 and R_k, respectively. Note that the system and measurement matrices F_k−1 and H_k, as well as the covariance matrices Q_k−1 and R_k, are allowed to be time-variant. For the reader’s convenience, the KF equations are reported in Appendix A.

The Extended Kalman Filter (EKF) is derived for nonlinear systems with additive noise: (66) x k = f k − 1 ( x k − 1 ) + w k − 1 z k = h k ( x k ) + v k .

The additive noises w_k−1 and v_k are mutually independent, zero-mean white Gaussian with known covariance matrices Q_k−1 and R_k, respectively. The EKF is based on the assumption that local descriptions of the nonlinear functions f_k−1(x_k−1) and h_k(x_k) can be obtained by approximating them using only the first term in the Taylor series expansion. The posterior PDF p(x_k|z_1:k) is therefore approximated by a Gaussian density. The local linearization requires the computation of the Jacobian matrices of the dynamic model, the measurement model or both with current predicted states, see Appendix A.

The EKF and its many variants are referred to as analytic approximations because the Jacobian matrices F_k−1 and H_k have to be computed analytically. Moreover, it is worthy noting that the EKF always approximates the posterior PDF p(x_k|z_1:k) as a multivariate Gaussian. If the nonlinearity in models is severe, the non-Gaussian nature of the posterior PDF p(x_k|z_1:k) can be pronounced, e.g., it can be multimodal, heavily-tailed or skewed: the approximation to first-order is grossly inaccurate and the performance of the EKF can therefore be seriously degraded. In general, besides the computational costs incurred in the calculations of the Jacobian matrices, other disadvantages of the linearization procedure implemented in an EKF concern the sensitivity to initial conditions, biases in the estimation errors, critical problems of convergence and filter stability, especially when the sampling interval is too small.

The Unscented Kalman Filter (UKF) is developed with the aim to overcome these limitations [45]. The UKF hinges on the assumption that it is easier to approximate a Gaussian distribution than it is to approximate an arbitrary nonlinear function. Instead of linearizing using Jacobian matrices, the UKF adopts a deterministic sampling approach to capture the estimates of the mean vector and the covariance matrix with a minimal set of sample points; this “capture” is accurate to the second-order Taylor series expansion for any nonlinearity. The UKF can be used with non-differentiable functions, it does not require the derivation of Jacobian matrices (derivative-free), and is valid to higher-order expansions than the standard EKF. Some work concerning applications for aircraft guidance and control shows the superiority of UKF over EKF, particularly in the presence of large initialization errors [46]. However, this is not usually the case with human body motion tracking applications. This is because, mostly, the filter operation starts with the human body being typically at rest within magnetically clean regions, and well-calibrated inertial/magnetic sensors, see Section 5. Hence, a gyro-free aiding sensor system can feed the EKF with accurate data for initialization at first contact. Moreover, there appears to be wide consensus that, in human body motion capture applications, although the EKF and the UKF may have roughly the same accuracy, the computational overhead of the UKF, the simplicity of the calculations of the Jacobian matrices, and the quasi-Gaussian nature of the posterior PDF p(x_k|z_1:k) contribute to make the EKF a preferred choice, even in the most demanding scenarios [47,48]. In order to keep the length of this paper within reasonable limits, UKF and more advanced Bayesian filters, namely particle filters [48,49], are not further addressed here.

The most principled way to preserve the unit-norm property of the estimated quaternion is to create an algorithm where the error between the true and estimated quaternions is itself a quaternion and is multiplied (in the sense of quaternion multiplication) with the a priori quaternion estimate to yield the a posteriori estimate [51]. This kind of EKF is called multiplicative EKF (MEKF), which differs from the classic additive EKF (AEKF), which employs quaternion subtraction in place of quaternion multiplication [52]. An MEKF parameterizes the global orientation with a non-singular unit quaternion, while any unconstrained three-dimensional representation is used to represent the orientation errors [53]. The dynamic model in an MEKF describes therefore the kinematic equations of a rigid body in terms of the relationships existing between the three-dimensional orientation error and the angular velocity [46]. Strong similarities exist between the MEKF approach and the so-called indirect-state formulation of the Kalman process. In analogy with an MEKF, the indirect-state EKF includes a three-dimensional orientation error in the state vector [54]. The potential advantages of an indirect-state filter are that the state dimension is smaller as compared with a direct-state filter, with subsequent computational savings [50].

In spite that MEKFs are theoretically correct in treating the normalization constraint, most reported implementations regard the application of AEKFs. AEKFs relax the quaternion unit-norm constraint and treat the four components of the quaternion as independent parameters. A method to preserve the quaternion unit-norm property is to derive a sort of quaternion measurement model from the non-linear equation that expresses the unit-norm property (pseudo-measurement model) [47]. Another popular means is to normalize the a posteriori estimate after the measurement update stage (“brute-force” approach). Even though it is neither elegant nor optimal, the “brute-force” approach is often the preferred choice and is proven to work well [52]. The exemplary EKF developed in the following is direct-state, additive and enforces the unit-norm constraint by the “brute-force” approach.

The dynamic equations for describing the orientation of the parts to be tracked would cause severe difficulties in the filter modelling [55], and especially in human body motion capture applications, where the inputs, i.e., muscle forces and torques, are unknown inputs [14,42]. The use of gyros as the primary means to estimate orientation allows circumventing these problems. Since the angular velocity of human body parts is obtained from the gyro data, the kinematic equations of a rigid body can be used to obtain the orientation state (model replacement). In other words, it is highly convenient to treat gyro data as external inputs to the filter rather than as measurements, and consequently gyro measurement noise and bias enter the filter as process noise rather than as measurement noise [22,56]. Another 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 additional important feature of stochastic estimation algorithms like the EKFs is that gyro drift bias can be estimated by state vector augmentation techniques [57]. Oftentimes, this feature is exploited, especially for applications of air and spacecraft attitude estimation, so as to compensate the gyros before performing the numerical integration of kinematics equations [36,46,56]. This is very important in the case that the only aiding comes from occasional orientation fixes. Drift biases of the gyro-free aiding sensor system may be also estimated in the same way [14,22]. In general, however, these other biases cannot be estimated simultaneously with the orientation and gyro drift bias due to possible problems of system observability [58].

Most studies in VR/AR fields employ head motion trackers that directly provide orientation measurements. In these cases, quaternions can be used with an EKF to estimate the angular velocity, which is needed to predict the future head orientation. Sometimes, angular velocity is measured with inertial sensors; the state vector can therefore be augmented with additional components, i.e., the angular acceleration [48]. Different motion models are implemented in the filtering algorithm, in order to improve the ability of the EKF to predict the head orientation for latency compensation [54,59]. For instance, the Constant Velocity (CV) model assumes a simple first-order Gauss-Markov (GM) model for each angular velocity component [60]: (67) d ω dt = − 1 τ ω + 2 σ 2 τ w ,where w is a Gaussian white noise, with null mean and unit variance, τ is the decorrelation time constant of the GM model and σ² is a variance factor. The GM model reflects underlying assumptions about the nature of human movements, namely, (a) the change of viewing direction is infrequent; (b) the angular velocity and acceleration are nonzero only during the infrequent changes in orientation. The decorrelation time constant and the variance factor are tuned in order that the spectral properties of the signal generated by the model match those of the angular velocities for paradigmatic motions [42]. More sophisticated models are also investigated, which opens the way to an interesting avenue of research concerning the development of multiple models of human (head) motion, and multiple model adaptive estimation techniques (MMAE) [61].

In this paper, the application specifics concern orientation measurement devices that use angular velocity to estimate orientation using an EKF. At the sampling intervals that are common for these devices, say between 100–500 Hz, the angular velocity can be considered constant in the time interval between successive measurements, leading to the numerical integration of kinematic equations via (39)–(40); the addition of some process noise may further help improving filter stability.

In a fully integrated IMMU, the gyro, the accelerometer and the magnetometer are each tri-axial, with mutually orthogonal sensitivity axes. Their output in response to the body angular velocity ω_body, acceleration (gravity g and body acceleration a_body), and local magnetic field (earth’s magnetic field h_earth and some local magnetic effect modelled as a time-invariant magnetic vector h_ext) are: (68) { ω B = g K   ω body + g b + g v a B = a K E B C   ( − g + a body ) + a b + a v   h B = h K E B C   ( h earth + h ext ) + h b + h v ,where ^gK, ^aK and ^mK are the matrices of the scale factors (ideally, they are equal to I₃); ^gb, ^ab and ^hb are the bias vectors (ideally, they are null); ^gv, ^av and ^hv are assumed uncorrelated white Gaussian measurement noise, with null mean and covariance matrix Σ g = σ g 2 I 3, Σ a = σ a 2 I 3 and Σ h = σ h 2 I 3. Equation (68) is a simplified model that does not account for additional error sources, such as cross-axis sensitivity, gyro g-sensitivity, nonlinearity, hysteresis and misalignment [62]. It is worthy noting that a further simplification is made in the gyro model by omitting the earth’s angular velocity of 15°/hour, since state-of-the-art MEMS gyros are unable to sense this component. Their bias stability is indeed in the order of 1°/s—the bias stability is usually specified as a 1σ value, and it describes how the bias may change over a specified period of time, typically around 100 s, in fixed conditions (usually including constant temperature) [26].

To proceed in the discussion of the sensor model (68), remind that an accelerometer measures the projection along its sensitive axis of the specific force f it is submitted. The specific force additively combines the linear acceleration component a, due to body motion, and the gravitational acceleration component, –g, both projected along the sensitive axis of the accelerometer, Figure 5. In common parlance, the high-frequency component, aka the AC component, is related to the dynamic motion the subject is performing, e.g., walking, hand weaving, head shaking, and so forth, while the low-frequency component of the acceleration signal, aka the zero-frequency (DC) component, is related to the influence of gravity, and it can be exploited to identify static postures [63].

The bias and scale factor of inertial and magnetic sensors are functions of environmental conditions, in particular ambient temperature; this is especially true for gyros [64]. Temperature effects on accelerometers are of relatively lower quantitative relevance, and they are usually negligible on magnetic sensors across the thermal variations that they may encounter in practice. 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. The influence of temperature on the gyro bias drift is particularly significant after that power is applied to gyros, as a result of device self-heating. Provided that gyros are allowed warm-up and thermal stabilization for few minutes, then their biases tend to change quite slowly with time. In practice, gyro bias errors can be calibrated and compensated effectively by so-called “zero attitude updates”, which require keeping the gyros from rotating. It is dependent on the nature of the specific application whether occasional rests can be assumed for the human body part to be monitored and tracked [65]. As for the scale factor calibration, procedures suited for in-field use are available [62,66]. The scale factor and bias errors of accelerometers can be calibrated and compensated by so-called “zero-velocity updates”, which require keeping the accelerometers from moving, although they are difficult to implement and may require specific manoeuvres to work properly [67]. In analogy with accelerometers, the scale factor and electronic bias errors of magnetic sensors can be calibrated and compensated effectively by in-field procedures [68]. In the case of experimental sessions lasting few minutes, it is quite safe to assume that the scale factor and bias errors of inertial and magnetic sensors are null, provided that the sensors are carefully calibrated before starting as explained above.

The problem of magnetic variations due to ferromagnetic materials in the vicinity of a magnetic sensor raises additional modelling considerations. Equation (68) shows that the reference magnetic vector is not necessarily the earth’s magnetic field h_earth. The presence of any constant field vector h_ext superimposed on h_earth does not preclude the possibility of constructing the horizontal reference needed for orientation estimation, provided that h_ext is accurately known. This is in contrast with the need to perform ambulatory measurements, without prior knowledge of existence and location of disturbances. Moreover, we have to consider dynamic effects that are related to either ferromagnetic objects moving in the vicinity of the sensor or to movements of the body-fixed sensor relative to static ferromagnetic objects. Because of these dynamic effects, h_ext turns out to be time-variant and unknown. A strategy to tackle this problem is to assume that a time-variant bias error ^hb is present in the magnetic sensor output. In the dynamic model the kinematics equations are therefore augmented with additional equations yielding the model of the behaviour in time of h_ext by “absorbing” it into a mathematical model of the magnetic drift bias, e.g., random-walk or first-order Gauss-Markov models (state augmentation). In other terms, the horizontal reference is built and maintained by the EKF itself (auto-calibration) [21,22].

The role of aiding sensors is played by accelerometers, taken alone or in combination with magnetic sensors. Sometimes, and depending on the requirements of a specific application, the complexity of the measurement hardware setup, and the sophistication of the filtering algorithms as well, can be reduced to some extent. Simplifying assumptions are quite common in applications to gait analysis [65]. For instance, in the case that motion outside the sagittal plane is assumed not to take place, accelerometers can be used without magnetic sensors, and, since the acceleration sensitivity axes can be embedded in the sagittal plane, simpler bi-axial configurations may suffice. Analogously, uni-axial gyros are enough to capture angular velocities when rotations are approximately about a single axis, oriented in the medio-lateral direction (orthogonal to the sagittal plane).

If motion occurs in the 3D space, heading estimation requires a horizontal reference, for which construction an IMMU is necessary. Few possibilities exist as for the choice of the measurement model. Since we prefer to take the measured angular velocity as an input to the filter, we have to decide how to handle acceleration and magnetic measurements. They can be fused together directly using any deterministic attitude estimation algorithm, e.g., TRIAD, QUEST, FQA, GN optimizer. An advantage of single-frame deterministic algorithms that deliver the quaternion at their output is that the measurement equations are linear; with the exception of the GN optimizer, see (57)–(58) in this regard, the unit-norm property of the measured quaternion is also preserved. The FQA is a potentially good choice, because of the decoupling of magnetic and acceleration data in estimating heading and inclination. A difficulty with this approach is in constructing the expression of the measurement noise covariance, a problem apparently dismissed in [41]. Analytical expressions of the covariance matrix for the TRIAD algorithm and QUEST are derived in [40]: it is proven that the noise in the quaternion measurements presents quaternion-dependent covariance matrices. The dependence on the quaternion is not a problem per se, since the predicted state can be used in place of the true unknown state. An element of complication is that the noisy reference magnetic vector estimated by the EKF must be used in the process of vector matching.

Alternatively, each reference vector component is given a specific measurement equation, which helps moving its representation from the earth-fixed to the body-fixed frame via either (24) or the rule of composition (33): (69) [ a k h k ] = [ E B C   ( q ¯ k ) 0 0 E B C   ( q ¯ k ) ]   [ g h ] + [ a v k h v k ]as done, e.g., in [22,24]. The measurement equations are nonlinear, which forces to compute their Jacobian matrices when carrying out the linearization process; however, the computations are neither algebraically difficult nor computationally demanding. The derivation of the measurement noise covariance matrix is not difficult as well, since it can be expressed directly in terms of the statistics of the measurement noise affecting each sensor.

Another important issue in the EKF design deals with its behaviour when anomalous measurements are received. It takes relatively long for an EKF to recover from false measurements when they are given the opportunity to contribute to the estimate of the state vector [57]. The best approach to deal with this problem would consist of preventing the filter from processing data whose reliability is suspected to be low [22,24].

As for the acceleration, in order to answer the question whether the body-fixed measured acceleration vector is suitable for measuring gravity, we would compare its norm with the known value of gravity; better yet, we may decide to work directly with the norm of the difference between the measured acceleration vector, resolved in the earth-fixed frame, and the gravity. If the deviation exceeds some properly chosen threshold value, a sensor glitch, or a contamination due to body motion would be suspected. Different actions can be taken: the measured vector is discarded, and the filter update is only based on magnetic measurements, unless they too are considered unreliable. This is equivalent to temporarily set the acceleration measurement noise variance to some large value, which can be considered, to all practical purposes, infinite: (70) R σ a 2 = { σ a 2 , | a k − E B C   ( q ¯ k − 1 − )   g | < ɛ a ∞ , otherwise ,where ɛ_a is a suitably chosen threshold. An alternative approach consists of defining a suitable law for relating the increase of the acceleration measurement noise variance to the actual deviation of the measured acceleration vector from the gravity. According to our experience, the norm-based adaptive algorithm (70) works adequately in most practical conditions. Note that the quaternion predicted by the filter at time t_k−1 is used in (70), in place of the unknown true quaternion at the time instant t_k.

A similar approach can be pursued as for the magnetic measurements. In this case we have to work with the difference between the measured magnetic vector, resolved in the earth-fixed frame, and the local magnetic reference vector. Sometimes, computing the dip angle is suggested to help improving the process; the dip angle is the angle between the magnetic field and the horizontal plane, which would be constant for given latitude and longitude. Unfortunately, the dip angle varies very erratically, especially within indoor environments, and we find its use somewhat critical. An effective norm-based adaptive algorithm applied to the magnetic sensor measurement noise variance is as follows: (71) R σ h 2 = { σ h 2 , | h k − B E C   ( q ¯ k − 1 − )   h earth − h b k − 1 − | < ɛ h ∞ , otherwise ,where ɛ_h is a suitably chosen threshold. The problem with this approach is that we must assume that the local magnetic reference vector is known, which is not the case unless the magnetic bias auto-calibration feature is implemented in the EKF. Note that the magnetic bias predicted by the filter at time t_k−1 is used in (71), in place of its unknown true value at time t_k.

A final comment concerns the applications in gait analysis, in the particularly important case that the IMMU is placed on the lower limbs, e.g., on the foot instep [65]. Rather than fusing them in an EKF, accelerometer data are used just to align the IMMU when the foot is at rest (stance phase of the gait cycle), followed by gyro integration during the swing phase of the gait cycle. During the swing phase the acceleration signals are indeed dominated by the inertial component, which means that the condition implied by (70) is almost never fulfilled. The stride-by-stride gyro integration reset allows mitigating the random-walk errors associated with the integration of gyro wideband measurement noise. To perform stance detection, we need a variant of the norm-based adaptive algorithm (70), or possibly a similar algorithm applied to gyro data: (72) | a k − E B C   ( q ¯ i − 1   − )   g | < ɛ a   ∪     | ω k | < ɛ g   for   i ∈ [ k − K , k − 1 ] ,where ɛ_g is a suitably chosen threshold, and KT_s is the given amount of time to detect when the foot is at rest.

The parameters to be tuned in an EKF concern the statistical properties assumed for the process noise and the measurement noise. Since they are modelled as Gaussian noise, we need to specify the possibly time-variant covariance matrices R_k and Q_k [57,69].

The measurement noise covariance matrix R_k is usually built from an isotropic model of sensor behaviour: the sensing elements that form each triad are characterized by the same measurement noise variance, hence R_k is proportional to a identity matrix, or it presents a block-diagonal structure. The measurement noise variances are estimated by taking samples from the sensors at a stationary location. The estimated measurement noise variances can be slightly increased over the on-bench calibration values, to help the EKF stability. In this way, for instance, tremulous motions that an accelerometer can be subject to or minute magnetic variations that can be observed for even very small sensor displacements can be accounted for as noise components in the filter. The EKF is generally quite robust to mismodelling errors in the measurement noise covariance matrix.

Once the structure of the process noise covariance matrix Q_k is determined, it can be convenient to use scaling parameters that are applied to specific blocks of Q_k. The values of the scaling parameters can be determined using a non-linear optimization routine for values that optimize the filter behaviour in given operating conditions [59]. Some considerations in parameter tuning must be directed to the belief attached to the validity of the process model. In assessing the model validity, for instance, we have to consider the many gyro error sources besides bias that are not modelled using (68), and the assumption that the angular velocity is constant in the time interval between consecutives updates by the EKF. In particular, the latter assumption can be criticized either for high dynamic motions, low sampling frequencies or both. In any case, it is known that increasing the value of the scaling parameters tends to make the filter response more prompt, at the expense of some degradation in the accuracy of the state vector estimates when the system’s behaviour is more benign. The filter response can also be adjusted by on-line adaptation of Q_k. This can be done according to different means, e.g., by detecting statistically significant changes in the innovation produced by the filter—fading memory algorithm, [70]. We do not introduce any Q-adaptation in the exemplary EKF presented in this paper. In general, it would be said that the EKF is less robust to mismodelling errors in Q_k than in R_k.

A study concerning the EKF performance assessment can be performed on experimental or synthetic motion data. With the former approach a series of experiments is performed, where monitored subjects are asked to perform paradigmatic motions. The main problem is that the true motion signals are unknown, because of the noise present in the measurement data. It is possible to smooth the data to some extent and use them as a reference signal, but the risk is to introduce false signal features, while removing true ones. With the latter approach, the main problem is whether the synthetic signals really capture the exact characteristics of the paradigmatic motions or not. If not, it is difficult to predict filter performance when applied to real-life tasks. As neither of these approaches is perfect, both analyses are usually performed and attempts are made to relate them to each other.

Mostly, in order to construct a dataset according to the approach based on experimental motion data, the motion is recorded using a gold standard optical tracker. The truth-reference unit quaternion q̄_true can be built from the 3D-position coordinates of a minimum of three markers, using, e.g., the Horn algorithm [71]. Quaternion smoothing can be performed by using norm-preserving orientation filters [72] or by independently filtering the quaternion components with any standard low-pass filter, followed by “brute force” normalization [22]. In order to implement a simulation environment for Monte Carlo simulation studies, standard conversion formulas can be applied to construct the orientation vector from the unit quaternion [30]. The orientation vector and its time derivative are then used to synthesize the angular velocity vector that generates the specified orientation [73]. The sensed gravity and magnetic field are computed from resolving gravity and magnetic field into the body frame using (32); additive white Gaussian noises with null mean and assigned variance are added to simulate the sensor measurements during the motion. Any given disturbance, e.g., body acceleration and magnetic variation can also be added into the simulated sensor signals before resolving them in the body-fixed frame.

The performance metrics can be based on computing Δ q ¯ = q ¯ true   − 1 ⊗ q ¯, where q̄_true and q̄ are the true and the estimated quaternions, respectively. The quaternion Δq̄ represents therefore the rotation that brings the estimated body frame onto the true body frame. The orientation error Δθ is obtained from the scalar component of Δq̄ according to the equation Δθ = 2 arccos (Δq₄). The performance metrics are expressed in terms of the root-mean-square-value of the orientation error (RMSE_θ), 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 can be computed from q̄_true 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. An obvious advantage of working with synthetic motion 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. Of course, this possibility is precluded when working with experimental motion signals.

The dynamic model equation is as follows: (73) [ q ¯ k h b k ] = [ exp   [ Ω   ( ω B   ( t k − 1 ) ) T s ] 0 0 I 3 ]   [ q ¯ k − 1 h b k − 1 ] + [ q w k − 1 h w k − 1 ]   .

The dynamic model is linear and time-variant. Implicit in the formulation (73) is that the gyro biases are negligible. The auto-calibration feature implemented in the filter is limited to handling the problem of magnetic variations, by modelling the magnetic bias as a random-walk process driven by the zero-mean white Gaussian noise vector ^hw_k.

The validity of the model part that describes the time-evolution of the unit quaternion depends on the assumption that the angular velocity is constant in the time interval [t_k−1 t_k]. The process noise component is: (74) q w k − 1 = − T s 2   Ξ   ( q ¯ k − 1 ) g   v k − 1 = − T s 2 [ [ q k − 1 × ] + q 4   ( k − 1 ) I 3 − q k − 1 T ]   .^qw_k−1 describes how the gyro noise enters the state model through a quaternion-dependent linear transformation. The process noise component ^hw_k−1 is assumed to have covariance matrix b Σ = T s   σ b 2   I 3. The variance term σ b 2 reflects the a priori belief about the severity of the magnetic variations in the given environment.

Because of the assumption that ^qw_k−1 and ^hw_k−1 are uncorrelated, the process noise covariance matrix Q_k has the following block-diagonal structure: (75) Q k − 1 = [ ( T s 2 ) 2 Ξ k − 1 g Σ Ξ k − 1 T 0 0 b Σ ]   .

Equation (69) describes the measurement model. The measurement noise covariance matrix is: (76) R k = [ a R k 0 0 h R k ]   ,with: (77) { a R k = R σ a 2   I 3   h R k = R σ h 2   I 3 .where the variance terms are determined using (70)–(71) (R-adaptation). Useless to say, when both variance terms are set to some extremely large values, i.e., no aiding comes to the filter, the kinematic equations are integrated based only on the gyro data.

The block diagram of the filter is sketched in Figure 6. The block “project ahead” computes the a priori state estimate and error covariance matrix, using (A2)–(A3). The estimation of the rotation matrix is carried within this block; this estimate is also used to compute the Jacobian matrix of the measurement Equation (69), using (A10), and the Kalman Gain, using (A5)–(A6). The block “update” computes the a posteriori state estimate and error covariance matrix, using (A7)–(A8). Remind the need for normalizing the updated quaternion at this level, in preparation for the next “project ahead” step. The measurement validation tests implement (70)–(71), which is followed by the R-adaptation step, via (76)–(77). The iterative nature of the filter allows exploiting the statistics available at any time step to start the computations at the next time-step, when a new set of measurements from the sensors become available.

The dataset for the experiment described in this Section was obtained by collecting inertial/magnetic sensor data from the MTx orientation tracker by Xsens Technologies B.V., Enschede, The Netherlands. These data were delivered through the USB interface to a host computer at a rate of 100 Hz together with the unit quaternion time functions estimated by the native Xsens EKF that runs in its default setting. The device was placed on top of a 10 cm × 10 cm plate that was screwed on a cyclist helmet, and fastened using double-side adhesive tape. The plate orientation was recorded using a six-camera Vicon optical tracker with a sampling rate of 100 Hz. A trigger signal was generated by the host computer and enabled time-synchronization of MTx and Vicon data streams. The system measured the position of four reflective markers (diameter: 25 mm), placed at the corners of the plate.

The MTx sensors were calibrated before starting the experimental session and their scale factor and bias errors were therefore zeroed for all practical purposes. The initial orientation of the sensor frame relative to the reference frame was found by asking the subject to stand still for few seconds at the beginning of the trial. The calibration quaternion needed to estimate the reference magnetic vector was built during the still time. The subject was then asked to freely move and turn his helmet-capped head during the trial, while standing on the spot. The head motion trial lasted slightly more than half a minute. The Euler angles time functions delivered by the Vicon system were considered the truth reference for the purpose of error estimation.

The EKF was implemented in Matlab for off-line data processing on a MacBook Air computer. Using the virtualization technology from Parallels Desktop 4.0 for Mac, the cycle time for a single iteration turned out to be about 2.0 ms, without any particular programming effort made to optimize the computational efficiency of the filter. The optimally tuned parameter setting for the EKF is reported in Table 1.

The error statistics are reported in Table 2.

The first and last column report the errors incurred by the EKF and the Xsens EKF, respectively. The columns 3 to 5 give the errors incurred when the gyro is aided by: accelerometer only (column 3); magnetic sensor only (column 4); no sensors (column 5). Suppose that (71) is implemented with ɛ_h = 0 The measurements from the magnetic sensor cannot be incorporated in the measurement update stage of the EKF; the same occurs when ɛ_a = 0, as for the accelerometer, in (70). When both thresholds are zero, the gyro-free aiding sensor system is inhibited, and the orientation solution is obtained entirely from gyro measurement data. It is apparent that the accelerometer and the magnetic sensor are helpful in inclination and heading stabilization, respectively. Without aiding sensors, random-walk integration of gyro wideband measurement noise yields seriously degraded performance. These results clearly indicate the importance of sensor fusion in improving the accuracy of orientation estimates by inertial/magnetic sensing.

Finally, Figures 7–9 show the time functions of the Euler angles as they are measured from the Vicon system; superimposed on them the time functions of the estimation errors incurred by the EKF.