Kalman Filtering for Attitude Estimation with Quaternions and Concepts from Manifold Theory

Abstract

The problem of attitude estimation is broadly addressed using the Kalman filter formalism and unit quaternions to represent attitudes. This paper is also included in this framework, but introduces a new viewpoint from which the notions of “multiplicative update” and “covariance correction step” are conceived in a natural way. Concepts from manifold theory are used to define the moments of a distribution in a manifold. In particular, the mean and the covariance matrix of a distribution of unit quaternions are defined. Non-linear versions of the Kalman filter are developed applying these definitions. A simulation is designed to test the accuracy of the developed algorithms. The results of the simulation are analyzed and the best attitude estimator is selected according to the adopted performance metric.

1. Introduction

Mechanical state estimation of a vehicle is a field of interest. A vehicle is considered a rigid body, and its state of motion is represented by 4 mathematical objects: two of them represent its position and velocity, and the other two represent its orientation, and angular velocity. This paper is focused on the estimation of the angular state, composed of orientation, and angular velocity.

Although there are other mathematical tools used for estimation [1], the Kalman Filter [2] has become the algorithm par excellence in this area. Because of its simplicity, the rigor and elegance in its mathematical derivation, and its recursive nature it is very attractive for many practical applications. Its non-linear versions have been widely used in orientation estimation: the Extended Kalman Filter (EKF), and the Unscented Kalman Filter (UKF) [3]. However, there are problems arising from the used parametrization to represent the orientation.

The orientation of a system is represented by the rotation transformation that relates two reference frames: a reference frame anchored to that system, and an external reference frame. A thorough survey of attitude representations is provided in Reference [4]. The parametrization used to represent the rotation transformation could be singular, or present discontinuities among others. Table 1 summarizes the main characteristics of the most used parametrizations.

Having in mind that the special orthogonal group SO(3) has dimension three, ideally we would seek for a continuous and non-singular representation expressed by 3 parameters. However, since 1964 we know that “...it is topologically impossible to have a global 3-dimensional parametrization without singular points for the rotation group” [5]. Knowing this, we would not be wrong to say that unit quaternions are the most convenient representation we have, and that we will have for orientations. In Reference [6] the literature on attitude estimation is reviewed until 1982, when other parametrizations like Euler angles were common, and founds the basis of modern quaternion-based attitude estimation, in which this paper is supported. After that work, many others have explored this viewpoint, and have demonstrated its superiority [7,8,9,10,11,12].

Quaternions are 4-dimensional entities, but only those having unit norm represent a rotation transformation. This fact implies a problem in applying the ordinary Kalman Filter, so different approaches have emerged. Since a quaternion is of dimension 4, one tends to think at first on a4×4 covariance matrix, and in the direct application of the Kalman Filter [13]. Given that all predictions are contained in the surface defined by the unit constraint, the covariance matrix shrinks in the orthogonal direction to this surface, which leads to a singular covariance matrix after several updates. A second perspective was firstly approached in Reference [6] and was after named as “Multiplicative Extended Kalman Filter” [8,11,12]. In this second approach we define an “error-quaternion” that is transformed to a 3-vector. We use this vector to build the covariance matrix, and we talk about a “3×3 representation of the quaternion covariance matrix”. However, there are still details in this adaptation that are currently being developed. Namely, the “covariance correction step” [14].

This paper presents a new viewpoint on the problem of attitude estimation using Kalman filters when the orientation is represented by unit quaternions. Noticing that unit quaternions live in a manifold (the unit sphere in^R4), we use basic concepts from manifold theory to define the mean and covariance matrix of a distribution of unit quaternions. With these definitions we develop two estimators based on the Kalman filter (one EKF-based and another UKF-based) arriving at the concepts of “multiplicative update” and “covariance correction step” in a natural and satisfying way. The inartificial emergence of these ideas establishes a solid foundation for the development of general navigation algorithms. Lastly, we also analyze the accuracy in the estimations of these two estimators using simulations.

The organization of this paper is as follows. In Section 2 we review quaternion basics. We also expose the new viewpoint on the definition of the quaternion mean and covariance matrix. In Section 3 we present the developed estimation algorithms. In Section 4 we define the performance metric, describe the simulation scheme, and present the results of the simulations. We also discuss the results. Finally, Section 5 concludes the paper.

2. Quaternions Describing Orientations 2.1. Quaternions

Quaternions are hypercomplex numbers composed of a real part and an imaginary part. The imaginary part is expressed using three different imaginary units{i,j,k}satisfying the Hamilton axiom:

ⁱ²=^j2=^k2=i∗j∗k=−1.

A quaternionqcan be represented with 4 real numbers, and using several notations:

q=_q0+_q1i+_q2j+_q3k≡

≡^{_q0,_q1,_q2,_q3T}≡

≡^_q0,qT.

We will denote quaternions with bold italic symbols (q), while vectors will be denoted with bold upright symbols (q). Vectors will be written in matrix form, and the transposed of a matrixMwill be denoted as^MT.

Quaternion product is defined by Equation (1) which produces the multiplication rule

p∗q=_p0q0−p·q_p0q+_q0p+p×q,

where(·)represents the usual dot product, and(×) represents the 3-vector cross product. Note that the quaternion product (*) is different from the product denoted by (⊗) in reference [4]. Given this multiplication rule, the inverse of a quaternionq(the one for whichq∗^q−1=^q−1∗q=1) is given by

^q−1=1^∥q∥2q∗=1^{∥q∥2_q0,−qT},

where^q∗represents the complex conjugate quaternion. Note that ifqis a unit quaternion (a quaternion with∥q∥=1), then^q−1=^q∗.

2.2. Quaternions Representing Rotations

Each rotation transformation is mapped with a rotation matrixRand with two unit quaternionsqand−qall of them related through

R(q)=1−2_q22−2_q322(_q1q2−_q3q0)2(_q1q3+_q2q0)2(_q1q2+_q3q0)1−2_q12−2_q322(_q2q3−_q1q0)2(_q1q3−_q2q0)2(_q2q3+_q1q0)1−2_q12−2_q22.

Note thatR(q)=R(−q).

Quaternions representing rotations have the form

q=^{cos(θ/2),q^sin(θ/2)T},

whereq^denotes the unit vector that defines the rotation axis, andθthe angle of rotation. Having this form, they satisfy the restriction

^∥q∥2=_q02+_q12+_q22+_q32=1.

This means that quaternions describing rotations live in the unit sphere of^R4,^S3. This space is a manifold, and some concepts regarding these mathematical objects are useful in our context. In particular, the concept of chart is of special interest.

2.3. Distributions of Unit Quaternions

When dealing with the Kalman filter, the distribution of a random variablexis encoded by its meanx¯=E[x]and its covariance matrixPdefined as

P=Ex−x¯^x−x¯T.

This definition makes sense when our random variables are defined in the Euclidean space. But how do we define the covariance matrix of a random variable living in a manifold like ours? How can we define the covariance for unit quaternions ifq−q¯does not represent a rotation? (Unit quaternions form a group under multiplication, but not under addition. This means that the addition of two unit quaternions may not result in another unit quaternion. Therefore, the addition of two unit quaternions may not represent a rotation.) What would be the covariance matrix if each quaternion was equiprobable in the unit sphere? We cannot redefine the covariance matrix, because the Kalman filter uses this precise form in its derivations, but we can take advantage of the properties of a manifold. Let us retrieve some important definitions:

Definition 1 (Homeomorphism).

A homeomorphism is a functionf:X→Ybetween two topological spaces X and Y satisfying the next properties:

1. f is a bijection,

2. f is continuous,

3. its inverse function^f−1is continuous.

If such a function exists, we say that X and Y are homeomorphic.

Definition 2 (Manifold).

A n-manifold ^Mnis a topological space in which each point is locally homeomorphic to the Euclidean space^Rn. This is, each pointx∈^Mnhas a neighborhoodN⊂^Mnfor which we can define a homeomorphismf:N→_Bnwith_Bnthe unit ball of^Rn.

Definition 3 (Chart).

A chart for a manifold^Mnis a homeomorphism φ from an open subsetU⊂^Mnto an open subset of the Euclidean spaceV⊂^Rn. This is, a chart is a function

φ:U⊂^Mn→V⊂^Rn,

with φ a homeomorphism. Traditionally, a chart is expressed as the pair(U,φ).

Given these definitions we can continue our reasoning.

In Reference [8] it talks about four “attitude error representations”. Namely, the one we will call Orthographic (O), the Rodrigues Parameters (RP), the Modified Rodrigues Parameters (MRP), and the Rotation Vector (RV). The first three are what we know as stereographic projections (and are called Orthographic, Gnomonic, and Stereographic respectively). The last one is a projection called Equidistant. But all four are charts defining a homeomorphism from the manifold^S3to the Euclidean space^R3. This is, they map a pointqin the manifold with a pointein^R3 . Table 2 arranges these chart definitions, together with their domain and image. We must ensure the charts to be bijections so that they properly define a homeomorphism, and that they do not mapqand−qwith different points of^R3since they represent the same rotation. We achieve this by the given definitions of the domain and image for each chart.

Figure 1 shows how points in the sphere^S2(subspace of the sphere^S3where quaternions live) are mapped to points in^R2(subspace of^R3 where the images of the charts are contained) through each one of the named charts. Since our charts are homeomorphisms, it is possible to invert the functions. Figure 2 shows how points from^R2 are mapped to points in the manifold through the inverted charts. As pointed in Reference [8], all four charts provide the same second-order approximation for a pointe∈^R3near the origin, to a quaternionq∈^S3

^φ−1(e)≈^{1−∥e∥28,e2T}.

We should notice that having^R3and^S3different metrics, a chartφwill inevitably produce a deformation of the space. However, for quaternions in the neighborhood of the identity quaternion (top of the sphere), our charts behave like the identity transformation between the imaginary part of these quaternions, and the points near the origin in^R3 as suggested by (10). This is a desirable property, as this means that the space around the identity quaternion closely resembles the Euclidean space, which is the space for which the Kalman filter is designed. But this just happens in the neighborhood of the identity quaternion. However, we can extend this property for any quaternionq¯∈^S3noting that any quaternionq∈^S3can be expressed as a “deviation” from the first one through the quaternion product:

q=q¯∗^δq¯,

where^δq¯represents such a deviation. (This definition is arbitrary: we could have chosen to relate the quaternions throughq=^δq¯∗q¯ , but it is important to establish one of these definitions, and then be consequent with it. However, (11) entails a computational advantage for the computation of (37).) Then, we define a chart_φq¯for each quaternionq¯∈^S3as

^eq¯=_φq¯(q)=φ^δq¯,

where^δq¯=^q¯∗∗qand where we have denoted the point of the Euclidean space mapped with the quaternionq∈^S3through the chart_φq¯as^eq¯. Then, we will have a set of charts_φq¯q¯, each one resembling the Euclidean space around a quaternionq¯∈^S3, and mapping this last quaternion to the origin of^R3. We will refer to the Euclidean space associated with the chart_φq¯as theq¯-centered chart. Thus, the homeomorphism_φq¯−1takes a point^eq¯in theq¯-centered chart and maps it to a pointqin the manifold through

q=_φq¯−1^eq¯=q¯∗^φ−1eq¯.

After reviewing these concepts, we can define the covariance matrix of a distribution of unit quaternions.

Given a unit quaternionq¯and a chartφ, we will define the expected value of a distribution of unit quaternions in theq¯-centered chart as

^e¯q¯=E^eq¯,

and its covariance matrix as

^Pq¯=E^eq¯−^{e¯q¯eq¯−e¯q¯T},

and the probability density of each unit quaternionqwould be defined through the homeomorphismq=_φq¯−1(^eq¯). Then, a distribution of unit quaternions needs of four mathematical objects to be encoded:φ,q¯,^e¯q¯,^Pq¯. Although a distribution of unit quaternions is unique, given this definition, its expected value^e¯q¯and its covariance matrix^Pq¯may take different values depending on the chosen quaternionq¯and chartφ. However, knowing that the Kalman filter is designed for the Euclidean space, it will be convenient to choose a unit quaternionq¯central in the distribution, in order that the manifold space around it closely resembles the most significant region for the covariance matrix in theq¯-centered chart. It is particularly convenient to choose a quaternionq¯such that^e¯q¯=0so that the covariance matrix is centered in the origin of theq¯-centered chart.

2.4. Transition Maps

At some step of the Kalman filter, we will have a distribution of unit quaternions defined in aq¯-centered chart, and we will be interested in expressing our distribution in anotherp¯-centered chart. The concept of transition map is relevant for this purpose.

Definition 4 (Transition map).

Given two charts(_Uα,_φα)and(_Uβ,_φβ)for a manifold M, with_Uαβ=_Uα∩_Uβ≠∅, we can define a function_φαβ:_φα(_Uαβ)→_φβ(_Uαβ)as

_φαβ(x)=_φβφα−1(x),

withx∈_φα(_Uαβ). The function_φαβis called a transition map. Being that_φαand_φβare homeomorphisms, so is_φαβ.

For the present case, let us consider two unit quaternionsp¯andq¯both related through

p¯=q¯∗δ¯.

These two quaternions define the charts_φp¯and_φq¯. We build the transition map that relates a point^eq¯expressed in theq¯-centered chart with a point^ep¯expressed in thep¯-centered chart doing

^ep¯=_{φp¯φq¯−1}^eq¯=

=φ^p¯∗∗q¯∗^φ−1eq¯=

=φ^δ¯∗∗^φ−1eq¯.

That is to say, first we take the point^eq¯in theq¯-centered chart, and we obtain its associated quaternionqin the manifold using_φq¯−1. Then, we transform this quaternionqto a point^ep¯in thep¯-centered chart. Nevertheless, knowing the quaternionδ¯we do not need to explicitly computeq. In fact, being able to express the same quaternionqas two different deviations,

q=q¯∗^δq¯q=p¯∗^δp¯⇒^δp¯=^p¯∗∗q¯︸^δ¯∗∗^δq¯.

Note the equivalence of expressions (18c) and (19).

Table 3 displays the transition maps for the charts studied. The detailed derivations of these transition maps can be found in Appendix A. Figure 3 attempts to provide some insight into how points are transformed through the transition map of each chart.

3. Manifold Kalman Filters

In this section we present the models adopted for the Manifold Kalman Filters (MKF), and we display the resulting algorithms.

The state of the system at a time t is defined by an orientation, encoded with a unit quaternion_qtand by an angular velocity_ωt′. We will consider them to be random variables, and we will try to estimate their value using a Kalman filter.

Our unit quaternions_qt∈H:∥_qt∥=1will define the rotation transformation that relates a vector_vt′expressed in a reference frame^S′attached to the solid whose state we want to describe, with the same vector_vtexpressed in an external reference frameS

_vt=R(_qt)_vt′≡_vt=_qt∗_vt′∗_qt∗.

For example, if we measure an acceleration_at′in reference frame^S′the acceleration in the inertial reference frameSwould be given by_at=R(_qt)_at′. This acceleration would be the one that we would have to integrate to obtain the position estimated by an accelerometer.

The vector_ωt′will define the angular velocity of the solid measured in^S′. Note that we do not include the bias of the sensors in the state of our system. We will assume that our sensors are calibrated, so the biases are zero.

We can predict the value of the random variables that describe the state of our system through the following motion equations:

d^ω′(t)dt=^qω(t),

dq(t)dt=12q(t)∗^ω′(t)=12q(t)∗0^ω′(t),

where^qω(t)is a random variable that represents the process noise, and is associated with the torque acting on the system, and with its inertia tensor. Its expected value at a given time t will be denoted as_q¯tωand its covariance matrix will be denoted as_Qtω.

We will assume that we have sensors giving measurements of angular velocity_ωtm(which provide information about the relative change in orientation), and of a vector_vtmwhose value_vtexpressed in the external reference frameSis known (this provides information about absolute orientation). Examples of such sensors could be a gyroscope giving angular velocity measurements, an accelerometer measuring the gravity vector near the Earth surface (_vt:=−g), or a magnetometer measuring the Earth magnetic field (_vt:=B). The measurement model relates these measurements with the variables that describe the state of the system:

_vtm=^RT(_qt)_qtv+_vt+_rtv,

_ωtm=_ωt′+_rtω,

where_rtωand_rtvare random variables with zero mean and covariance matrices_Rtωand_Rtvrespectively that represent the measurement noises, and_qtvis another random variable with mean_q¯tvand covariance matrix_Qtvrepresenting external disturbances in the measurement of the vector_vt. For example, it could represent accelerations others than gravity for an accelerometer, or magnetic disturbances produced by moving irons for a magnetometer.

We will assume that the measurements arrive at discrete times_{tn}n. The format_xt|tnwill be used to denote a variable x at a time t, having included measurements up to a time_tnwitht>_tn. For the n-th time stamp, in which a measurement arrives, we will write_xt|nfor the sake of simplicity. Then, our knowledge about the state at a time t, having included measurements up to a time_tnwitht>_tnis described by a distribution encoded in the collection of mathematical objectsφ,p¯,_x¯t|np¯,_Pt|np¯ as described in Section 2.3. For the present case,_x¯t|np¯=_e¯t|np¯,_ω¯t|n′^Tis the expected value of the distribution, and_Pt|np¯is its6×6covariance matrix, both expressing the quaternion distribution in thep¯-centered chart. Preferably,p¯will be a unit quaternion central in the distribution, so that the mapping of points from thep¯-centered chart to the manifold causes minimal deformation in such distribution. The unit quaternion_q¯t|n=_{φp¯−1e¯t|np¯}will be our best estimation of the real quaternion_qtthat defines the orientation of the system with respect to the external reference frameSat time t.

The following subsections present the developed Kalman filters: one version based on the EKF and another version based on the UKF. The EKF is based on the linearization of the non-linear models to calculate the predicted covariance matrices. That is, the EKF approximates non-linear functions using their Jacobian matrices. To apply the EKF, our functions must be differentiable. On the other hand, the UKF is based on a deterministic sampling to approximate the distribution of our random variables. We select a minimal set of samples whose mean and covariance matrix are those of the state distribution. Then, they are transformed by the non-linear models, and the resulting set of points is used to compute the means and covariance matrices necessary to perform the Kalman update. This second approach does not need the functions to be differentiable.

3.1. Manifold Extended Kalman Filter

In this section we present the EKF-based estimator: the Manifold Extended Kalman Filter (MEKF). We offer here the main results of the more detailed derivation given in Appendix B.

A measurement

_zn=_vnmωnm

arrives at time_tn. Our knowledge about the orientation at a previous time_tn−1is described by a distribution expressed in the_{q¯n−1|n−1}-centered chart. We assume that this distribution has mean

_{x¯n−1|n−1q¯n−1|n−1} =_{e¯n−1|n−1q¯n−1|n−1} =0_{ω¯n−1|n−1′},

and covariance matrix_{Pn−1|n−1q¯n−1|n−1} . This is, we have an initial four

φ,_{q¯n−1|n−1},_{ω¯n−1|n−1′},_{Pn−1|n−1q¯n−1|n−1} .

The state prediction at time_tngiven all the information up to_tn−1is computed through

_{ω¯n|n−1′}=_{ω¯n−1|n−1′},

_δnω=cos∥_{ω¯n|n−1′}∥Δ_tn2_{ω¯n|n−1′}∥_{ω¯n|n−1′}∥sin∥_{ω¯n|n−1′}∥Δ_tn2,

_q¯n|n−1=_{q¯n−1|n−1}∗_δnω,

_Fn=^RT(_δnω)IΔ_tn0I,

_{Pn|n−1q¯n|n−1} =_{FnPn−1|n−1q¯n−1|n−1} +_QnFnT,

with

_Qn=_Qnω^(Δ_tn)33−_Qnω^(Δ_tn)22−_Qnω^(Δ_tn)22_QnωΔ_tn.

The measurement prediction at the same time is given by

_v¯n|n−1m=^RT_{q¯n|n−1q¯nv}+_vn,

_ω¯n|n−1m=_{ω¯n|n−1′},

_z¯n|n−1=_{v¯n|n−1mω¯n|n−1m},

_Hn=_{v¯n|n−1m×}00I,

_Sn|n−1=_{HnPn|n−1q¯n|n−1 HnT}+^RT_{q¯n|n−1Qnv}R_q¯n|n−1+_Rnv00_Rnω,

where_[v]×stands for

_[v]×=0−_v3v2v30−_v1−_v2v10.

At this point, we compute the Kalman gain_Knand use it to obtain the optimal estimation of the state:

_Kn=_{Pn|n−1q¯n|n−1 HnTSn|n−1−1},

_{x¯n|nq¯n|n−1} =_{x¯n|n−1q¯n|n−1} +_Knzn−_z¯n|n−1,

_{Pn|nq¯n|n−1} =I−_{KnHnPn|n−1q¯n|n−1} ,

where_{x¯n|n−1q¯n|n−1} =_{e¯n|n−1q¯n|n−1} =0,_{ω¯n|n−1′}^T. Finally, we need to obtain the updated unit quaternion,_q¯n|nand compute the mean and the covariance matrix in the_q¯n|n-centered chart, so that the distribution is expressed in the same conditions as at the beginning of the iteration. The point_{e¯n|nq¯n|n−1} that results from (41), and that is defined in the_q¯n|n−1-centered chart, correspond to a unit quaternion in the manifold. This is the updated unit quaternion_q¯n|nwhich we are looking for:

_q¯n|n=_{φq¯n|n−1−1e¯n|nq¯n|n−1} =

=_q¯n|n−1∗^φ−1_{e¯n|nq¯n|n−1} =

=_q¯n|n−1∗_δ¯n.

Knowing that the Kalman update (41) could produce any point in the_q¯n|n−1-centered chart we will need to “saturate” to the closest point contained in the image of each chart. The point_{e¯n|nq¯n|n−1} in the_q¯n|n−1-centered chart is the origin in the_q¯n|n-centered chart. Then, the expected value of the state in this new chart will be given by_x¯n|nq¯n|n =_e¯n|nq¯n|n =0,_ω¯n|n′^Tas at the beginning of the iteration.

To update the covariance matrix we need to consider its definition (15). We want to compute^P_q¯n|n having^P_q¯n|n−1 and knowing the relation^ep¯eq¯ provided by the transition maps in Table 3. Continuing with the EKF philosophy, the update for the covariance matrix will be found by linearizing^ep¯eq¯around the point where the majority of information is comprised (in our case, the point^e¯q¯=_{e¯n|nq¯n|n−1} ):

_eip¯^eq¯=_eip¯^e¯q¯+∑j_{∂eip¯^eq¯∂ejq¯^eq¯=^e¯q¯ejq¯}−_e¯jq¯+O∥^eq¯−^{e¯q¯ ∥2},

where we have used the big O notation to describe the limiting behavior of the error term of the approximation as^eq¯→^e¯q¯. In particular, if we define

_(T)ij=_{∂eip¯^eq¯∂ejq¯^eq¯=^e¯q¯},

then,

^ep¯−^e¯p¯≈^ep¯eq¯−^ep¯e¯q¯≈T^eq¯−^e¯q¯,

and the final update for the covariance matrix will be computed through

_Pn|nq¯n|n =E(_xn|nq¯n|n −_x¯n|nq¯n|n )^{(_xn|nq¯n|n −_x¯n|nq¯n|n )T}≈

≈T(_δ¯n)00I_{Pn|nq¯n|n−1} ^{T(_δ¯n)00IT}.

Table 4 summarizes the resultingT-matrix for each chart, along with their application domain. A detailed derivation of theseT -matrices can be found in Appendix C.

After the final computation we obtain the four

φ,_q¯n|n,_ω¯n|n′,_Pn|nq¯n|n ,

that is a condition equivalent to (27) in which we started the iteration.

3.2. Manifold Unscented Kalman Filter

In this section we present the UKF-based estimator: the Manifold Unscented Kalman Filter (MUKF).

A measurement_znarrives at time_tn. Our knowledge about the orientation at a previous time_tn−1is described by a distribution expressed in the_{q¯n−1|n−2}-centered chart. This distribution is encoded in the four

φ,_{q¯n−1|n−2},_{x¯n−1|n−1q¯n−1|n−2} ,_{Pn−1|n−1q¯n−1|n−2} .

The first step in the UKF is to create the augmentedN×1mean_x˜nandN×Ncovariance matrix_P˜n. Since the measurement equations are linear for the random variables_rtωand_rtvwe can leave their covariance matrices out of the augmented one and add them later:

_x˜n=_{x¯n−1|n−1q¯n−1|n−2 q¯nωq¯nv},

_P˜n=_{Pn−1|n−1q¯n−1|n−2} 000_Qnω000_Qnv.

Then, we obtain the matrix_Lnwhich satisfies_LnLnT=_P˜nand we use it to generate the2N+1sigma points_{Xj}j=02N as described in Ref. [15]:

_Xi,0=_(x˜n)i,

_Xi,j=_(x˜n)i+_Lnij2_Wjforj=1,…,N,

_Xi,j+N=_(x˜n)i−_Lnij2_Wjforj=1,…,N,

being_Wj=(1−_W0)/(2N)forj≠0where_W0regulates the importance given to the sigma point_X0in the computation of the mean. These sigma points_{Xj}jare expressed in the_{q¯n−1|n−2}-centered chart. We need to express them in the manifold before applying the evolution equations and the measurement equations:

_Xjq=_{φq¯n−1|n−2−1Xje}=_{q¯n−1|n−2}∗^φ−1_Xje,

_Yjω=_Xjω+_Xj^qω Δ_tn,

_Yjq=_Xjq∗cos∥_Yjω∥Δ_tn2_Y^jωsin∥_Yjω∥Δ_tn2,

_Zjv=^RT_XjqXjv+_vt,

_Zjω=_Yjω,

where for the j-th sigma point,_Xjeis its chart point part and_Xjqis the quaternion with which it is mapped,_Xjωis its angular velocity part,_Xj^qω is its angular velocity noise part,_Yjωis its angular velocity prediction,_Yjqis the quaternion part of its prediction (we have assumed that the angular velocity_Yjωis constant in the time interval[_tn−1,_tn) so that we can use (A20)),_Xjvis the vector process noise part,_Zjvis its vector measurement prediction,_Zjωis its angular velocity measurement prediction, andΔ_tn=_tn−_tn−1. Note that when applying the inverse chart^φ−1we will need to “saturate”_Xjeto the closest point in the image ofφ. Having these new sigma points, we can obtain the means and covariance matrices of the distributions present in the UKF. First, defining_Zj:=_Zjv,_Zjω^Tthe means are computed through

_q¯n|n−1=_∑jWjYjq∥_∑jWjYjq∥,

_{ω¯n|n−1′}=∑j_WjYjω,

_{x¯n|n−1q¯n|n−1} =_{φq¯n|n−1 q¯n|n−1}=0_{ω¯n|n−1′},

_z¯n|n−1=∑j_WjZj.

where we have used a variation of the result provided in Ref. [16]. Namely,

q¯≈_{∑j qj}∥_{∑j qj}∥,

with_qj·_qk>0forj,k=0,…,2N. This result is shown to minimize the fourth order approximation of the distance defined as the sum of squared angles between the rotation transformation represented by each quaternion_qjand the one represented byq¯. This approach to compute the mean quaternion is extremely efficient, and its derivation is elegant and simple. In order to ensure that_qj·_qk>0it is useful to remember the property that bothqand−qrepresent the same rotation. This property is also useful for introducing the quaternions in the domain ofφto execute the next step of the filter.

After this, we use the obtained mean quaternion_q¯n|n−1to express each sigma point in the_q¯n|n−1-centered chart, and compute the covariance matrices:

_Yje=_{φq¯n|n−1 Yjq}=φ_{q¯n|n−1∗}∗_Yjq,

_{Pn|n−1q¯n|n−1} =∑j_WjYjYjT,

Pn|n−1yz=∑jWjYj Zj−z¯n|n−1T,

Sn|n−1=∑jWjZj−z¯n|n−1Zj−z¯n|n−1T+Rnv00Rnω,

where we have denoted_Yj:=_Yje,_Yjω−_{ω¯n|n−1′}^T. Finally, we compute the UKF version of the Kalman gain_Knand we use it to obtain the optimal estimation of the state:

_Kn=_{Pn|n−1yzSn|n−1−1},

_{x¯n|nq¯n|n−1} =_{x¯n|n−1q¯n|n−1} +_Knzn−_z¯n|n−1,

_{Pn|nq¯n|n−1} =_{Pn|n−1q¯n|n−1} −_{KnSn|n−1KnT},

arriving at the same conditions in which we began the iteration, with a distribution expressed in the_q¯n|n−1-centered chart, and encoded by the four

φ,_q¯n|n−1,_{x¯n|nq¯n|n−1} ,_{Pn|nq¯n|n−1} .

Our best estimation for the orientation at this time is

_q¯n|n=_{φq¯n|n−1−1e¯n|nq¯n|n−1} =_q¯n|n−1∗^φ−1_{e¯n|nq¯n|n−1} ,

being_{e¯n|nq¯n|n−1} the part of the mean_{x¯n|nq¯n|n−1} that represent the quaternion in the_q¯n|n−1-centered chart.

Note that setting_{q¯n−1|n−2}:=_{q¯n−1|n−1}and_{e¯n−1|n−1q¯n−1|n−2} :=0at the beginning of each iteration yields the traditional version of the algorithm, where a “reset operation” is performed instead of the covariance matrix update.

4. Simulation Results

This section presents the results of the simulations used to measure the accuracy of each estimator. Simulations are chosen instead of real experiments because a real system entails an uncertainty in the measurement of the true attitude: the attitude that is used to compare with that estimated by the algorithms. There are sources of error ranging from a miscalibration of the measurement system to a possible bias in the “true attitude” produced by another attitude estimator, which makes it problematic to define an adequate metric to measure the accuracy of the algorithms. For this reason, the authors consider that using a simulation is more reliable to avoid possible biases in the results due to said sources of error. Others have performed similar types of tests [7,17]. However, the results do not seem to be statistically conclusive: only the estimations of some orientation trajectories are shown.

We perform our comparison through a simulation in which we do have an absolute knowledge of the attitude of the system: a true oracle exists in a simulation. Therefore, we can compare the real orientation with the attitude estimated by the algorithms having fed them only with simulated measurements that we obtain from such known orientations. We will extract our performance metrics from a wide set of orientation trajectories in order to obtain statistically conclusive results.

We try to answer three questions with the simulation test. The first question is, is there a chart for which we get a greater accuracy in attitude estimation? The second one is, what algorithm produces the most accurate attitude estimation, the MEKF or the MUKF? The last question stems from the fact that previous algorithms on attitude estimation, such as the Multiplicative Extended Kalman Filter, did not contemplate updating the distribution from one chart to another as done at (47b) in the MEKF. However, their estimators performed well [6,7,12]. Then the third question is, does this “chart update” imply an improvement in the accuracy of the attitude estimation?

Although a simulation has been used to compare our algorithms, these have also been tested with a real IMU. In the Supplementary Materials one can find a demonstration video, the source code used in the video, the source code used to generate the simulations, and the source code used to obtain the computational cost of the algorithms in each platform.

4.1. Performance Metric

We have already described a quaternionqas a deviation from another quaternionq¯asq=q¯∗δ. Now we define the instantaneous error between an estimated attitude, represented by a unit quaternionq¯and the real attitude, represented by the unit quaternionq⭑as the angle we have to rotate one of them to transform it into the other. This is, the angle of the rotation transformation defined by the quaternion_δesuch thatq⭑=q¯∗_δe . Recalling (6), this angle can be computed as:

_θe=2arccos_{^q¯∗∗q⭑0}=

=2arccosq¯·q⭑,

having previously ensured thatq¯·q⭑≥0using the fact that bothqand−qrepresent the same rotation transformation.

Angle_θewill vary along an orientation trajectory. Then, we will define the mean error in orientation estimation for a given trajectory starting at timet=0and ending at timet=Tas

_eθ=1T_{∫0T θe}(t)dt.

Finally,_eθwill depend on the followed trajectory, and on the set of taken measurements. We will need to generate several orientation trajectories to obtain the mean value_e¯θand the variance_σe¯θ2that characterize the distribution of the error in orientation estimation_eθfor each algorithm. We will define the confidence interval for the computed_e¯θas

_e¯θ−3_σe¯θ /_Ns,_e¯θ+3_σe¯θ /_Ns,

where_Nsis the number of samples taken for the_e¯θcomputation, so that_σe¯θ2/_Nsis the variance of the sample mean distribution.

Being that the lower the better, the value of_e¯θgives us a measure of how well an algorithm estimates the orientation. We will consider that the performance of an algorithm A is better than the performance of other algorithm B if_e¯θ(A)<_e¯θ(B)and their confidence intervals do not overlap.

4.2. Simulation Scheme

To compute the performance metrics we will need to generate a large number of simulations. Each independent simulation will consist of three steps: initialization, convergence, and estimation.

In the initialization step we set up the initial conditions accordingly to the chosen simulation parameters. This includes generating the initial unit quaternion_q⭑0from a uniform distribution in^S3setting the initial angular velocityω⭑_0′to zero, setting the update frequency_fupdategenerating the variances of the process noises_σω2and_σv2from a uniform distribution in the intervals(0,_Qmaxω]and(0,_Qmaxv]respectively, and initializing the estimation algorithm. The initialization of the MEKF includes setting_q¯0|0=1_ω¯0|0′=0rad/sand_P0|0q¯0|0 =¹⁰²I. On the other hand, the initialization of the MUKF includes setting_q¯0|−1=1_{e0|0q¯0|−1} =0_ω¯0|0′=^(1,1,1)Trad/sand_{P0|0q¯0|−1} =¹⁰²I. The angular velocity is not initialized to0in the MUKF because it has been observed that it is sometimes necessary to “break the symmetry” for the algorithm to converge; especially when we do not apply the chart update (when we perform the “reset operation”) for the RV chart. The covariance matrices that appear in both algorithms are initialized as_Qnω=I^rads2/^s4_Qnv=¹⁰⁻²Ip.d.u.(“p.d.u.” stands for “Procedure Defined Unit”. In the present case it depends on the definition of the vectorv),_Rnω=^RωI^rads2/^s2,_Rnv=^RvIp.d.u., where^Rωand^Rvare the variances of the measurement noise that will be used in the simulation. We give this information about the measurement noise to the algorithms because it can be obtained offline, while the information about the process noise cannot. Given that a priori we cannot know how the system will behave, the values of_Qnωand_Qnvhave been chosen according to what we understand could be normal. Choosing these values we are assuming that after a second it is normal for the angular velocity to have changed by1rad/sand also that it is normal to find external noises added to the vector_vtof magnitude¹⁰⁻¹p.d.u.. For the mean values we set_q¯nω=0rads/^s2and^q¯v=0p.d.u..

In the convergence step we keep the system in the initial orientation_q⭑0 . Simulated measurements are generated using (23) and (24). For each measurement, a different_vtis sampled from a uniform distribution in the unit sphere of^R3. The values for each component of_qtvrtvand_rtωare obtained from normal distributions with zero mean and variances_σv2^Rvand^Rωrespectively. The term^RT(_qt) in (23) is obtained from the true attitude_q⭑t, which in the convergence step takes the value of_q⭑t=_q⭑0. The term_ωt′in (24) is the true angular velocity, which in the convergence step takes the valueω⭑_t′=0. The tested algorithm updates its state estimation until the inequality_θe(t)<_θe0is satisfied, where_θe(t)is the value of the error (72), and_θe0is a parameter in the simulation. The convergence step could have been replaced by an initialization of the attitude estimated by the algorithm_q¯tto the real value_q⭑tbut then it would have also been necessary to fix a certain covariance matrix. Since the metric of the space generated by each chart is different, it is difficult to set a covariance matrix that provides the same information for each chart. It seemed more natural to the authors to allow the algorithm to find the true attitude by its own means, and for the covariance matrix to converge to a value in each case.

Finally, in the estimation step we generate a random but continuous orientation sequence using a Wiener process for the angular velocity:

ω⭑_t′=ω⭑_t−δt′+_ntδt,

_q⭑t=_q⭑t−δt∗cos∥ω⭑_t′∥δt2ω⭑_t′∥ω⭑_t′∥sin∥ω⭑_t′∥δt2,

where_ntis a random vector whose components are sampled from a normal distribution with zero mean and variance_σω2andδtis the simulation time step that is related to the algorithm time step Δt troughdtdtsimδt=Δtbeingdtdtsiman integer parameter that determines the simulation updates per algorithm update. Note that we multiply_ntbyδtand not byδt. We do it this way so that the covariance matrix after k steps does not depend on the simulation time stepδt. In fact, after a timeT=kδtthe covariance matrix of the angular velocity will have grown byΔ^Pω=kI_σω2δt=I_σω2Tand not by^(ΔPω)′=kI_σω2^(δt)2=I_σω2Tδt. After eachdtdtsimsimulation updates, a simulated measurement is generated in the same way it was done in the convergence step, and the algorithm is updated with it. The simulation will run for a timeTsim=^k′Δtwhere^k′ is an integer number. This way we will perform the last algorithm update at the end of the simulation. The error (72) will be evaluated after each algorithm update, and it will be added up through the simulation to obtain the averaged error (73). After each simulation, we will obtain a sample for the computation of_e¯θand_σe¯θ2. We will perform_Ns of these simulations to obtain the confidence interval (74).

4.3. Results

In this section we present the results of the simulations. The algorithms are tested for update frequencies_fupdate=1/Δtin the interval[2,1000]Hz. This range has been chosen thinking about the possible limitations of a real system. For example, the maximum data rate of a low cost IMU is around1000Hz . On the other hand, the update frequency may be limited by processing. The computational cost of each estimator has been evaluated in two platforms: an Arduino MEGA 2560, and a Raspberry Pi 3 Model B. The code has been written in c++. The resulting maximum update frequencies are presented in Figure 4, which indicates that the MEKF can be executed approximately 3 times faster than the MUKF.

Although the algorithms have been developed allowing a differentΔ_tn for each update, the simulations are performed using a constant Δt, and the simulation parameters depicted in Table 5.

The parameters_θe0 Tsim, dtdtsim, and_Nshave been chosen trying to reach a compromise between the precision of the results, and the execution time of the simulation. The values for_Qmaxωand_Qmaxvhave been chosen in such a way that the estimation algorithms face both normal situations (_Qnω≈_σω2Iand_Qnv≈_σv2I) and situations that were not foreseen (_Qnω≠_σω2Ior_Qnv≠_σv2I). A typical low cost IMU has^Rω≈^10−4rad2/^s2and^Rv≈^10−4g2. The values chosen for R represent an imprecise sensor (¹⁰⁻²), a normal sensor (¹⁰⁻⁴), and a precise sensor (¹⁰⁻⁶). The value of_W0has been chosen so that all sigma points have the same importance, but very similar results, if not identical, have been obtained for other selections of_W0.

4.3.1. Chart Choice

The results of the simulation are presented in Figure 5. The average of the performance metric is shown along with its confidence interval for each of the selected update frequencies. The results of the MEKF and the MUKF are shown in different graphs, but drawn in the same one are the results for each chart and for a given MKF. In this way we are able to distinguish if a chart has an advantage over the others.

We observe that there is no chart that is especially advantageous. All things being equal, we would opt for the RP chart. For this chart it is not necessary to worry about the domain since it mapsqand−qwith the same point of^R3and with the sameT-matrix; or of the image since it is all^R3. In addition, the expressions of^φ−1and theT-matrix for the MEKF are simpler for the RP chart. These computational advantages make us prefer the RP chart over the others.

4.3.2. MEKF vs. MUKF

Figure 6 also presents the results of the simulations. This time, we display on the same graph the resulting performance metrics for the MUKF and the MEKF when the RP chart is used. In this way, we can distinguish if one MKF has an advantage over the other.

We note that the MEKF performs the same or better than the MUKF. This differs from the usual experience, in which the UKF outperforms the EKF in traditional non-linear estimation applications. The fact that the charts resemble the Euclidean space near the origin (see Section 2.3) might be favoring the MEKF, since the Jacobian matrices, used to approximate the non-linear functions, are defined at that point. However, the sigma points generated for the MUKF are sampled far from the origin of the chart, where the non-linearities become notorious. We are facing a very particular scenario in which the model is approximately linear for the MEKF, while for the MUKF it is not. In addition, due to the difference in computational cost (see Figure 4), the MUKF update frequencies will generally be lower than those of the MEKF, which will imply worse accuracy in its estimations. Then, the MEKF with the RP chart seems to be our best option.

4.3.3. Chart Update vs. No Chart Update

Figure 7 presents the results of each MKF with each chart in a different graph, but displayed in the same one are the results using the “chart update” and the results without using it.

We can observe that there is almost no difference between using the “chart update” and not using it. The concepts used in this paper have helped us to understand the mechanisms of the MKF, and ultimately to arrive to the concepts of “multiplicative update”, and of “covariance correction step” with theT-matrix definition. However, it is not necessary to apply the latest update (47b) in practice: we will obtain essentially the same accuracy in our estimations.

5. Conclusions

We have used concepts from manifold theory to define the expected value and the covariance matrix of a distribution in a manifold. In particular, we have defined the expected value and covariance matrix of a distribution of unit quaternions in^S3, the unit sphere in^R4, using the concept of chart. These definitions have helped us to develop Kalman filters for orientation estimation, where the attitude has been represented by a unit quaternion. They have also helped us solve the problem of the “covariance correction step”. Two estimators have been developed: one based on the EKF (the MEKF), and another based on the UKF (the MUKF). The MEKF and the MUKF have been tested in simulations, and some results have been obtained. The conclusions of the simulations are:

1. There is no chart that presents a clear advantage over the others, but the RP chart has some characteristics that motivate us to prefer it.

2. The MEKF is preferable to the MUKF due to its lower computational cost and its greater accuracy in orientation estimation.

3. The “chart update” is not necessary for the MKF in practice.

Then, the MEKF with the RP chart and without applying the “chart update” is our best attitude estimator according to the adopted performance metric. This algorithm resembles the conventional “Multiplicative Extended Kalman Filter”, but we have obtained the MEKF without having to redefine any aspect of the classic Kalman filter.

[Image omitted. See PDF.]

[Image omitted. See PDF.]

[Image omitted. See PDF.]

[Image omitted. See PDF.]

[Image omitted. See PDF.]

[Image omitted. See PDF.]

[Image omitted. See PDF.]

Representation	Parameters	Continuous	Non-Singular	Linear Evolution Equation
Euler angles	3	✗	✗	✗
Axis-angle	3–4	✗	✗	✗
Rotation matrix	9	✓	✓	✓
Unit quaternion	4	✓	✓	✓

Chart	Domain	Image	e=φ(q)	q=φ−1(e)
O	{q∈S3:q0≥0}	{e∈R3:∥e∥≤2}	2q	1−∥e∥24e/2
RP	{q∈S3:q0>0}	R3	2qq0	14+∥e∥22e
MRP	{q∈S3:q0≥0}	{e∈R3:∥e∥≤4}	4q1+q0	116+∥e∥216−∥e∥28e
RV	{q∈S3:q0≥0}	{e∈R3:∥e∥≤π}	2q^arcsin∥q∥	cos∥e∥2e^sin∥e∥2

Chart	Transition Mapep¯eq¯
O	δ¯0eq¯−4−∥eq¯ ∥2δ¯−δ¯×eq¯
RP	2δ¯0eq¯−2δ¯−δ¯×eq¯2δ¯0+δ¯·eq¯
MRP	48δ¯0eq¯−(16−∥eq¯∥2)δ¯−8δ¯×eq¯16+∥eq¯ ∥2+δ¯0(16−∥eq¯∥2)+8δ¯·eq¯
RV	2δp¯∥δp¯∥arcsin∥δp¯∥, withδp¯=δ¯0e^q¯sin∥eq¯∥2−cos∥eq¯∥2δ¯−δ¯×e^q¯sin∥eq¯∥2

Chart	T(δ¯)Matrix	Domain
O	δ¯0I−δ¯×+δ¯δ¯Tδ¯0	{δ¯∈S3:δ¯0>0}
RP	δ¯0δ¯0I−δ¯×	{δ¯∈S3:δ¯0≠0}
MRP	121+δ¯0δ¯0I−δ¯×+δ¯δ¯T	{δ¯∈S3:δ¯0≥0}
RV	δ¯0I−δ¯^δ¯^T−δ¯×∥δ¯∥arcsin∥δ¯∥+δ¯^δ¯^T	{δ¯∈S3:δ¯0≥0,∥δ¯∥≠0}

Parameter	Value
θe0	1∘
Tsim	10s
dtdtsim	100
Ns	1000
Qmaxω	102rads2/s3
Qmaxv	1p.d.u.
R	{10−2,10−4,10−6}
Rω	Rrads2/s2
Rv	Rp.d.u.
W0	1/25

Supplementary Materials

The following are available online at https://www.mdpi.com/1424-8220/19/1/149/s1: SupplementaryMaterials.zip.

Author Contributions

Conceptualization, P.B.-P.; methodology, P.B.-P.; software, P.B.-P.; validation, P.B.-P. and H.M.-B.; formal analysis, P.B.-P.; investigation, P.B.-P.; resources, H.M.-B.; data curation, P.B.-P.; writing–original draft preparation, P.B.-P.; writing–review and editing, P.B.-P. and H.M.-B.; visualization, P.B.-P.; supervision, H.M.-B.; project administration, P.B.-P. and H.M.-B.

Funding

This research received no external funding.

Conflicts of Interest

The authors declare no conflict of interest.

Abbreviations

The following abbreviations are used in this manuscript:

EKF Extended Kalman Filter

UKF Unscented Kalman Filter

MKF Manifold Kalman Filter

MEKF Manifold Extended Kalman Filter

MUKF Manifold Unscented Kalman Filter

O Orthographic

RP Rodrigues Parameters

MRP Modified Rodrigues Parameters

RV Rotation Vector

Appendix A. Derivation of Transition Maps

This appendix contains the derivation of the transition map for each chart.

Appendix A.1. Orthographic

Using the inverse of the transformation that defines the chart,^φ−1,

^δq¯=1−∥^{eq¯ ∥2}4^eq¯/2.

Introducing (A1) into (19),

^δp¯=^δ¯∗∗^δq¯=_δ¯01−∥^{eq¯ ∥2}4+δ¯·^eq¯2_δ¯0^eq¯2−1−∥^{eq¯ ∥2}4δ¯−δ¯×^eq¯2.

Finally, applying the chart definition,

^ep¯=2^δp¯=_δ¯0^eq¯−4−∥^{eq¯ ∥2}δ¯−δ¯×^eq¯.

Appendix A.2. Rodrigues Parameters

Using the inverse of the transformation that defines the chart,^φ−1,

^δq¯=14+∥^{eq¯ ∥2}2^eq¯.

Introducing (A4) into (19),

^δp¯=^δ¯∗∗^δq¯=14+∥^{eq¯ ∥2}2_δ¯0+δ¯·^eq¯_δ¯0^eq¯−2δ¯−δ¯×^eq¯.

Finally, applying the chart definition,

^ep¯=2^δp¯_δ0p¯=2_δ¯0^eq¯−2δ¯−δ¯×^eq¯2_δ¯0+δ¯·^eq¯.

Appendix A.3. Modified Rodrigues Parameters

Using the inverse of the transformation that defines the chart,^φ−1,

^δq¯=116+∥^{eq¯ ∥2}16−∥^{eq¯ ∥2}8^eq¯.

Introducing (A7) into (19),

^δp¯=^δ¯∗∗^δq¯=116+∥^{eq¯ ∥2}_δ¯016−∥^{eq¯ ∥2}+8δ¯·^eq¯8_δ¯0^eq¯−16−∥^{eq¯ ∥2}δ¯−8δ¯×^eq¯.

Finally, applying the chart definition,

^ep¯=4^δp¯1+_δ0p¯=48_δ¯0^eq¯−16−∥^{eq¯ ∥2}δ¯−8δ¯×^eq¯16+∥^{eq¯ ∥2}+_δ¯016−∥^{eq¯ ∥2}+8δ¯·^eq¯.

Appendix A.4. Rotation Vector

Using the inverse of the transformation that defines the chart,^φ−1,

^δq¯=cos∥^eq¯∥2^{e^q¯}sin∥^eq¯∥2.

Introducing (A10) into (19),

^δp¯=^δ¯∗∗^δq¯=_δ¯0cos∥^eq¯∥2+δ¯·^{e^q¯}sin∥^eq¯∥2_δ¯0^{e^q¯}sin∥^eq¯∥2−cos∥^eq¯∥2δ¯−δ¯×^{e^q¯}sin∥^eq¯∥2.

Finally, applying the chart definition,

^ep¯=2^δp¯∥^δp¯∥arcsin∥^δp¯∥,

with

^δp¯=_δ¯0^{e^q¯}sin∥^eq¯∥2−cos∥^eq¯∥2δ¯−δ¯×^{e^q¯}sin∥^eq¯∥2.

Note that all transition maps are expressed using theδ¯quaternion. Given thate¯=φδ¯we could also have expressed them usinge¯which is what we get after applying the Kalman update (41). However, our choice makes transition maps to take a simpler form. In addition, having to compute the quaternionδ¯to perform (43c), this choice does not imply a computational overhead.

Appendix B. Details in the Derivation of the MEKF

This appendix contains the details in the derivation of the Manifold Extended Kalman Filter used in this study.

Appendix B.1. State Prediction

This subsection contains the derivation of the equations for the state prediction.

Appendix B.1.1. Evolution of the Expected Value of the State

Taking expected values in Equation (21) we obtain

d^ω¯′dt=^q¯ω⇒^ω¯′(t)=_ω¯0′+_q¯tωt,

with_q¯tωthe expected value of the random variable^qωat time t. Doing_ω¯0′=_{ω¯n−1|n−1′}we arrive at

_{ω¯t|n−1′}=_{ω¯n−1|n−1′}+_q¯tωt.

On the other hand, approximating (22) with its Taylor series up to first order around the current state(q¯,^ω¯′), and taking its expected value we obtain

Edq(t)dt≈12q¯(t)∗^ω¯′(t).

This differential equation has no general closed solution. But if we assume that the expected value of the process noise^q¯ω(t)is zero whent∈(_tn−1,_tn), so that^ω¯′(t)is constant in that interval, then we will have the matrix differential equation

dq¯(t)dt=_Ωˇnq¯(t),

with

_Ωˇn:=12_{(0−ω¯1′−ω¯2′−ω¯3′ω¯1′0ω¯3′−ω¯2′ω¯2′−ω¯3′0ω¯1′ω¯3′ω¯2′−ω¯1′0)n|n−1}.

This differential equation has the solution

q¯(t)=^eΩˇt_q¯0,

where_q¯0represents the initial conditions. After taking_q¯0=_{q¯n−1|n−1}, we obtain the prediction_q¯t|n−1, that can be expressed using the quaternion product as

_q¯t|n−1=_{q¯n−1|n−1}∗_δtω=_{q¯n−1|n−1}∗cos∥_{ω¯t|n−1′}∥Δt2_{ω¯t|n−1′}∥_{ω¯t|n−1′}∥sin∥_{ω¯t|n−1′}∥Δt2,

withΔt=t−_tn−1.

Appendix B.1.2. Evolution of the State Covariance Matrix

For a continuous nonlinear system of the form

dxdt=f(x,t)+g(q),

we know [18] that the covariance matrix satisfies the following differential equation:

dPdt=FP+P^FT+GQ^GT,

whereF=∂f∂x, andG=∂g∂q. This is so because the evolution equation forΔx=x−x¯is approximately given by:

dxdt≈dx¯dt+_{∂f∂xx=x¯}x−x¯+_{∂g∂qq=q¯}q−q¯,

andPis defined asP=EΔx^(Δx)T. However, we have a different definition forP:

P=E^eq¯−^e¯q¯ω′−^{ω¯′eq¯−e¯q¯ω′−ω¯′T}.

Then we need to find the evolution equation for^eq¯. Recall that we are assuming^e¯q¯=0at the beginning of the iteration. Knowing that any quaternion in the unit sphere can be expressed as a deviation from a central quaternionq¯asq=q¯∗δ , and using the differential Equations (22) and (A16), we can find a differential equation for the quaternionδ:

q=q¯∗δ⇒

⇒q˙=q¯˙∗δ+q¯∗δ˙⇒

⇒12q∗^ω′≈12q¯∗^ω¯′∗δ+q¯∗δ˙,

where a dot over a symbol represents time derivative, and we have obviated the time dependence. Isolating the time derivativeδ˙,

δ˙≈12^q¯∗∗q︷δ∗^ω′−12^q¯∗∗q¯︷1∗^ω¯′∗δ=

=12δ∗^ω′−^ω¯′∗δ=

=12_δ0δ∗0^ω′−0^ω¯′∗_δ0δ=

=12−^ω′−^ω¯′·δ_δ0^ω′−^ω¯′−^ω′+^ω¯′×δ=

=12−Δ^ω′·δ_δ0Δ^ω′−2^ω¯′+Δ^ω′×δ.

Knowing that, for each of our charts, theδ quaternion can be approximated by (10) ase→0, then we can obtain an approximate differential equation for a pointeexpressed in theq¯-centered chart. Note that we have not explicitly denoted^eq¯or^δq¯. This will be assumed implicitly, since these quantities will always be expressed in theq¯-centered chart in this appendix. Using the chain rule for a time derivative and expression (A26e),

_e˙i=∑j∂_ei∂_δj︷2_δij≈2_δij∂_δj∂t︷_δ˙j≡

≡e˙≈_δ0Δ^ω′−2^ω¯′+Δ^ω′×δ≈

≈1−^∥e∥28Δ^ω′−2^ω¯′+Δ^ω′×e2.

Then, the first order approximation to differential Equation (A27c) would be

e˙≈Δ^ω′−^ω¯′×e.

On the other hand, combining Equations (21) and (A14) we obtain

dΔ^ω′dt=d(^ω′−^ω¯′)dt=^qω−^q¯ω=^qω.

Summarizing,

ddteΔ^ω′≈−_^ω¯′×I00eΔ^ω′+0Δ^qω,

therefore matricesF,G, andQ in (A22) are in our case

F=−_^ω¯′×I00,

G=I,

Q=000E[Δ^qω(Δqω)T].

We are now in a position to solve the differential Equation (A22). Let us consider its homogeneous version first:

d_PHdt=F_PH+_PH^FT,

which has as solution

_PH=^eFt_C0^eFTt.

Taking into account the definition of matrix exponential, and after computing the powers ofFwe obtain

^eFt=∑n=0∞^(−Ω)ntnn!∑n=1∞^{(−Ω)n−1tn}n!0I≈^RT(^δω)It0I,

where we have denotedΩ=_^ω¯′×, and^δω=cos∥^ω¯′∥t2,^ω¯′∥^ω¯′∥sin∥^ω¯′∥t2. We also have assumed that t takes small values so we can approximate the infinite sums truncating in the first term. To find the solution of the non-homogeneous differential equation we use the variation of constants method:

P=^eFtC(t)^eFTt⇒

⇒dPdt=F^eFtC(t)^eFTt+^eFtC(t)^eFTtFT+^eFtdC(t)dt^eFTt=

=FP+P^FT+^eFtdC(t)dt^eFTt.

Identifying terms with (A22) we obtain that

^eFtdC(t)dt^eFTt=GQ^GT⇒

⇒dC(t)dt=^e−FtQ^e−FTt=∑n=0∞∑m=0∞^(−F)ntnn!Q^(−FT)mtmm!⇒

⇒C(t)=_C0+∑n=0∞∑m=0∞^(−F)nn!Q^(−FT)mm!^tn+m+1n+m+1.

Finally, truncating the summation in (A42) at the first non-zero elements, and inserting the result into (A37), we obtain (32) where we have identified_C0=P(0)through the initial conditions.

Appendix B.2. Measurement Prediction

This subsection contains the derivation of the equations for the measurement prediction.

Appendix B.2.1. Expected Value of the Measurement Prediction

Taking expected values on (24), and assuming_r¯tω=0 we arrive at (35). On the other hand, approximating (23) with its Taylor series up to first order around the current estimation of the state(q¯,^ω¯′), taking its expected value, and assuming_r¯tv=0 we obtain (34).

Appendix B.2.2. Covariance Matrix of the Measurement Prediction

In order to find the covariance matrix of the measurement prediction we need the linear approximation of the vector measurement around the point_x0:=e=0,^qv=^q¯v,^rv=^r¯v:

^vm≈^v¯m+_∂^vm∂ex0 e+_{∂^vm∂^qvx0} ^qv−^q¯v+_{∂^vm∂^rvx0} ^rv−^r¯v.

It is direct to identify

_{∂^vm∂^qvx0} =^RT(q¯),

_{∂^vm∂^rvx0} =I.

On the other hand, rewriting (23) as

^vm=^δ∗∗^q¯∗∗^qv+v∗q¯∗δ+^rv≡

≡^vm=^RT(δ)^RT(q¯)^qv+v+^rv,

and noting that settinge=0is equivalent to doδ=1,

_{∂vim∂ejx0} =∑k_{∂vim∂δkx0 ∂δk∂eje=0}=∑kl_{∂RilT(δ)∂δkδ=1 ^RT(q¯)^q¯v+vl ∂δk∂eje=0}.

Now, recalling (5) we have

_{∂^RT(δ)∂δkδ=1}=20_δ3k−_δ2k−_δ3k0_δ1kδ2k−_δ1k0≡−2∑n_εinlδnk,

with_εinlthe Levi-Civita symbol, and_δnk the Kronecker delta. Recalling (10) we also have

∂δ∂_ej=_{−ej/4δ1j/2δ2j/2δ3j/2e=0}≡(1−_δ0k)_δkj/2.

Then, introducing in (A48) Equations (A49), (34), and (A50),

_{∂vim∂ejx0} ≈−∑kln_{εinlδnkv¯lm}(1−_δ0k)_δkj=∑l_εiljv¯lm≡_^v¯m×,

where we have used_εijl=−_εilj. Finally, assuming the independence of the random variables_xt=^{(_et,_ωt′)T},_qtv,_rtv, and_rtω, and computing the covariance matrix_St=E(_zt−_z¯t)^{(_zt−_z¯t)T}with_zt=^{(_vtm,_ωtm)T}and (A43), we arrive at (37) and (38).

Appendix C. Derivation of the T-matrices

This appendix contains the derivation of theT-matrix for each chart.

Appendix C.1. Orthographic

Our transition map (A3) can be written as

_eip¯(^eq¯)=_δ¯0eiq¯−4−_∑k ^(_ekq¯)2_δ¯i−∑lm_{εilmδ¯lemq¯},

being_εilm the Levi-Civita symbol. Finding (45) for (A52) we obtain

_(T)ij=_{δ¯0δij−−∑k ekq¯δkj4−∑k ^(ekq¯)2δ¯i−∑lmεilmδ¯lδmj^eq¯=^e¯q¯}=

=_δ¯0δij+_e¯jq¯4−_∑k ^(_e¯kq¯)2_δ¯i−∑l_εiljδ¯l,

This expression can be rewritten in matrix form as

T=_δ¯0I+δ¯^(e¯q¯)T4−∥^{e¯q¯ ∥2}−_δ¯×.

Finally, recalling that for this chart_δ¯0=1−∥^{e¯q¯ ∥2}/4andδ¯=^e¯q¯/2, we arrive at the final expression

T=_δ¯0I+δ¯^δ¯T_δ¯0−_δ¯×.

Appendix C.2. Rodrigues Parameters

First, let us denote the numerator of (A6) asN(^eq¯), and its denominator asD(^eq¯):

N^eq¯:=_δ¯0^eq¯−2δ¯−δ¯×^eq¯,

D^eq¯:=2_δ¯0+δ¯·^eq¯.

Now let us evaluate (A56) at^e¯q¯:

N^e¯q¯=_δ¯0^e¯q¯︸2δ¯/_δ¯0−2δ¯︷0−δ¯×^e¯q¯︷=(δ¯∥^e¯q¯)=0.

Then, the approximation ofN^eq¯does not have terms of orderO(1). This means that we will only need to approximateD^eq¯to the zeroth order. Any further approximation would produce, after multiplying by the linear approximation ofN^eq¯, a higher order term. Let us then calculate each approximation.

We can rewrite (A56) as

_Ni^eq¯=_δ¯0eiq¯−2_δ¯i−∑kl_{εiklδ¯kelq¯},

with_εikl the Levi-Civita symbol. Applying (44) to (A59),

_Ni^eq¯≈∑j_{δ¯0δij−∑klεiklδ¯kδlj^eq¯=^e¯q¯ejq¯}−_e¯jq¯=

=∑j_δ¯0δij−∑k_{εikjδ¯kejq¯}−_e¯jq¯,

being_δijthe Kronecker delta. Returning to matrix notation, the linear approximation ofN^eq¯is

N^eq¯=_δ¯0I−_δ¯×^eq¯−^e¯q¯+O∥^eq¯−^{e¯q¯ ∥2}.

On the other hand, evaluating (A57) at^e¯q¯we obtain the zeroth order approximation:

D^e¯q¯=2_δ¯0+δ¯·2δ¯_δ¯0+O∥^eq¯−^e¯q¯∥=

=2_δ¯0δ¯02+^∥δ¯∥2︸1+O∥^eq¯−^e¯q¯∥=

=2_δ¯0+O∥^eq¯−^e¯q¯∥.

Finally, combining (A61) and (A62c) we can compute the linear approximation of (A6):

^ep¯eq¯=2_δ¯0I−_δ¯×^eq¯−^e¯q¯+O∥^eq¯−^{e¯q¯ ∥2}_δ¯02+O∥^eq¯−^e¯q¯∥=

=_δ¯0δ¯0I−_δ¯×^eq¯−^e¯q¯+O∥^eq¯−^{e¯q¯ ∥2}.

Appendix C.3. Modified Rodrigues Parameters

First, let us denote the numerator of (A9) asN(^eq¯), and its denominator asD(^eq¯):

N^eq¯=8_δ¯0^eq¯−16−∥^{eq¯ ∥2}δ¯−8δ¯×^eq¯,

D^eq¯=16+∥^{eq¯ ∥2}+_δ¯016−∥^{eq¯ ∥2}+8δ¯·^eq¯.

Now let us evaluate (A64) at^e¯q¯:

N^e¯q¯=8_δ¯0^e¯q¯︷4δ¯/(1+_δ¯0)−16−∥^{e¯q¯ ∥2}︷16∥δ¯^∥2/^(1+_δ¯0)2δ¯−8δ¯×^e¯q¯︷0(^e¯q¯∥δ¯)=0(^e¯q¯∥δ¯)=

=16δ¯1+_δ¯02_δ¯0−1+_δ¯0−∥δ¯^∥2︷1−_δ¯021+_δ¯0=0.

Then, as with the RP chart, the approximation ofN^eq¯does not have terms of orderO(1), and we will only need to approximateD^eq¯to the zeroth order.

We can write (A64) as

_Ni^eq¯=8_δ¯0eiq¯−16−∑k^(_ekq¯)2_δ¯i−8∑lm_{εilmδ¯lemq¯},

with_εilm the Levi-Civita symbol. Applying (44) to (A67),

_Ni^eq¯≈∑j_{8δ¯0δij+∑k2ekq¯δkjδ¯i−8∑lmεilmδ¯lδmj^eq¯=^e¯q¯ejq¯}−_e¯jq¯=

=∑j8_δ¯0δij+2_e¯jq¯δ¯i−8∑l_{εiljδ¯lejq¯}−_e¯jq¯,

being_δijthe Kronecker delta. Returning to matrix notation, the linear approximation ofN^eq¯is

N^eq¯=8_δ¯0I+2δ¯^(e¯q¯)T−8_δ¯×^eq¯−^e¯q¯+O∥^eq¯−^{e¯q¯ ∥2}=

=8_δ¯0I+δ¯^δ¯T1+_δ¯0−_δ¯×^eq¯−^e¯q¯+O∥^eq¯−^{e¯q¯ ∥2}.

On the other hand, evaluating (A65) at^e¯q¯we obtain the zeroth order approximation:

D^e¯q¯≈16+16∥δ¯^{∥2(1+_δ¯0)2}+_δ¯016−16∥δ¯^{∥2(1+_δ¯0)2}+8δ¯·4δ¯1+_δ¯0=

=161+_δ¯0(1+_δ¯0)+∥δ¯^∥21+_δ¯0+_δ¯0(1+_δ¯0)−∥δ¯^∥21+_δ¯0+2^∥δ¯∥2=

=161+_δ¯02+_δ¯02_δ¯0+21−_δ¯02=641+_δ¯0,

where we have used the equality∥δ¯^∥2=1−_δ¯02 for unit quaternions. Finally, combining (A69b) and (A70c) we can compute the linear approximation of (A9):

^ep¯eq¯=48_δ¯0I+δ¯^δ¯T1+_δ¯0−_δ¯×^eq¯−^e¯q¯+O∥^eq¯−^{e¯q¯ ∥2}1+_δ¯064+O∥^eq¯−^e¯q¯∥=

=1+_δ¯02_δ¯0I+δ¯^δ¯T1+_δ¯0−_δ¯×^eq¯−^e¯q¯+O∥^eq¯−^{e¯q¯ ∥2}=

=121+_δ¯0δ¯0I−_δ¯×+δ¯^δ¯Teq¯−^e¯q¯+O∥^eq¯−^{e¯q¯ ∥2}.

Appendix C.4. Rotation Vector

Let us start evaluating the vector^δp¯ in (A12) and (A13) at the point^e¯q¯:

^δp¯e¯q¯=_δ¯0^{e¯^q¯}sin∥^e¯q¯∥2︸δ¯−cos∥^e¯q¯∥2︸_δ¯0δ¯︷0=0−δ¯×^{e¯^q¯}sin∥^e¯q¯∥2︸δ¯︷0=0=0.

Then, the first order approximation of^δp¯around^e¯q¯will have the form

^δp¯=T˜^eq¯−^e¯q¯+O∥^eq¯−^{e¯q¯ ∥2},

and∥^δp¯∥→0as^eq¯→^e¯q¯. Taking the Taylor series of thearcsinx,

arcsin∥^δp¯∥∥^δp¯∥=∥^δp¯∥+O∥^{δp¯ ∥3}∥^δp¯∥=1+O∥^{δp¯ ∥2}=1+O∥^eq¯−^{e¯q¯ ∥2},

so that (A12) is linearized as

2^δp¯∥^δp¯∥arcsin∥^δp¯∥=2T˜^eq¯−^e¯q¯+O∥^eq¯−^{e¯q¯ ∥2}.

We only lack theT˜matrix. We will need the linear approximations ofcos∥^eq¯∥/2and^{e^q¯}sin∥^eq¯∥/2around^e¯q¯. To this end we will first obtain the linear approximation of∥x∥:

∥x∥=_{∑k xk2}=

=∥x¯∥+∑j_{∑k xkδkj∑k xk2x=x¯xj}−_x¯j+O∥x−x¯^∥2=

=∥x¯∥+∑j_{x¯j∑k x¯k2xj}−_x¯j+O∥x−x¯^∥2=

=∥x¯∥+^{x¯^T}x−x¯+O∥x−x¯^∥2.

Noticing that

∂∥x∥∂x=^{x¯^T}+O∥x−x¯∥,

our computations are straightforward:

cos∥x∥2=cos∥x¯∥2−_{sin∥x∥212^{x¯^T}+O∥x−x¯∥x=x¯}x−x¯+O∥x−x¯^∥2=

=cos∥x¯∥2−12sin∥x¯∥2^{x¯^T}x−x¯+O∥x−x¯^∥2.

For our particular case,

cos∥^eq¯∥2=_δ¯0−12^δ¯Teq¯−^e¯q¯+O∥^eq¯−^{e¯q¯ ∥2}.

On the other hand,

sin∥x∥2∥x∥=

=sin∥x¯∥2∥x¯∥+_{cos∥x∥2∥x∥12−sin∥x∥2^{∥x∥2x¯^T}+O∥x−x¯∥x=x¯}x−x¯+O∥x−x¯^∥2=

=sin∥x¯∥2∥x¯∥+cos∥x¯∥2∥x¯∥12−sin∥x¯∥2∥x¯^{∥2x¯^T}x−x¯+O∥x−x¯^∥2.

Now, takingx=x¯+x−x¯we arrive at

x∥x∥sin∥x∥2=x¯+x−x¯sin∥x∥2∥x∥=

=x¯^sin∥x¯∥2+sin∥x¯∥2∥x¯∥+12cos∥x¯∥2x¯^^{x¯^T}−sin∥x¯∥2∥x¯∥x¯^^{x¯^T}x−x¯+O∥x−x¯^∥2.

For our particular case,

^eq¯∥^eq¯∥sin∥^eq¯∥2=δ¯+∥δ¯∥2arcsin∥δ¯∥+_δ¯02δ¯^^{δ¯^T}−∥δ¯∥2arcsin∥δ¯∥δ¯^^{δ¯^Teq¯}−^e¯q¯+O∥^eq¯−^{e¯q¯ ∥2}=

=δ¯+12I−δ¯^^{δ¯^T}∥δ¯∥arcsin∥δ¯∥+_δ¯0δ¯^^{δ¯^Teq¯}−^e¯q¯+O∥^eq¯−^{e¯q¯ ∥2}.

Finally, we just have to replace (A79) and (A82b) in (A13) to obtain the required linear approximation. Returning to the original notation we have

2^δp¯∥^δp¯∥arcsin∥^δp¯∥=2^δp¯+O∥^eq¯−^{e¯q¯ ∥2}=

=2_δ¯0^{e^q¯}sin∥^eq¯∥2−2cos∥^eq¯∥2δ¯−2δ¯×^{e^q¯}sin∥^eq¯∥2+O∥^eq¯−^{e¯q¯ ∥2}==2_δ¯0δ¯+_δ¯0I−δ¯^^{δ¯^T}∥δ¯∥arcsin∥δ¯∥+_δ¯0δ¯^^{δ¯^Teq¯}−^e¯q¯−2_δ¯0−^δ¯Teq¯−^e¯q¯δ¯+

−δ¯×2δ¯+I−δ¯^^{δ¯^T}∥δ¯∥arcsin∥δ¯∥+_δ¯0δ¯^^{δ¯^Teq¯}−^e¯q¯+O∥^eq¯−^{e¯q¯ ∥2}=

=_δ¯0I−δ¯^^{δ¯^T}−_δ¯×∥δ¯∥arcsin∥δ¯∥+_δ¯02δ¯^^{δ¯^T}+δ¯^δ¯Teq¯−^e¯q¯+O∥^eq¯−^{e¯q¯ ∥2}=

=_δ¯0I−δ¯^^{δ¯^T}−_δ¯×∥δ¯∥arcsin∥δ¯∥+δ¯^^{δ¯^Teq¯}−^e¯q¯+O∥^eq¯−^{e¯q¯ ∥2}.

Note that the linear approximations of our transition maps are valid for^eq¯near of^e¯q¯. However, we have not made any assumption about theδ¯quaternion. This means that our linear approximations are exact for anyδ¯=^φ−1(^e¯q¯)in the domain of eachT-matrix, provided that^eq¯is close enough to^e¯q¯.

References

1. Crassidis, J.L.; Markley, F.L.; Cheng, Y. Survey of nonlinear attitude estimation methods. J. Guid. Control Dyn. 2007, 30, 12–28.

2. Kalman, R.E. A new approach to linear filtering and prediction problems. J. Basic Eng. 1960, 82, 35–45.

3. Julier, S.J.; Uhlmann, J.K. New Extension of the Kalman Filter to Nonlinear Systems; AeroSense’97; International Society for Optics and Photonics: Bellingham, WA, USA, 1997; pp. 182–193.

4. Shuster, M.D. A survey of attitude representations. Navigation 1993, 8, 439–517.

5. Stuelpnagel, J. On the parametrization of the three-dimensional rotation group. SIAM Rev. 1964, 6, 422–430.

6. Lefferts, E.J.; Markley, F.L.; Shuster, M.D. Kalman filtering for spacecraft attitude estimation. J. Guid. Control Dyn. 1982, 5, 417–429.

7. Crassidis, J.L.; Markley, F.L. Unscented filtering for spacecraft attitude estimation. J. Guid. Control Dyn. 2003, 26, 536–542.

8. Markley, F.L. Attitude error representations for Kalman filtering. J. Guid. Control Dyn. 2003, 26, 311–317.

9. Hall, J.K.; Knoebel, N.B.; McLain, T.W. Quaternion attitude estimation for miniature air vehicles using a multiplicative extended Kalman filter. In Proceedings of the 2008 IEEE/ION Position, Location and Navigation Symposium, Monterey, CA, USA, 5–8 May 2008; IEEE: Piscataway, NJ, USA; pp. 1230–1237.

10. VanDyke, M.C.; Schwartz, J.L.; Hall, C.D. Unscented Kalman filtering for spacecraft attitude state and parameter estimation. Adv. Astronaut. Sci. 2004, 118, 217–228.

11. Markley, F.L. Multiplicative vs. additive filtering for spacecraft attitude determination. Dyn. Control Syst. Struct. Space 2004, 6, 311–317.

12. Crassidis, J.L.; Markley, F.L. Attitude Estimation Using Modified Rodrigues Parameters. Available online: https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19960035754.pdf (accessed on 1 January 2019).

13. Bar-Itzhack, I.; Oshman, Y. Attitude determination from vector observations: Quaternion estimation. IEEE Trans. Aerosp. Electr. Syst. 1985, AES-21, 128–136.

14. Mueller, M.W.; Hehn, M.; D’Andrea, R. Covariance correction step for kalman filtering with an attitude. J. Guid. Control Dyn. 2016, 40, 2301–2306.

15. Julier, S.J.; Uhlmann, J.K. Unscented filtering and nonlinear estimation. Proc. IEEE 2004, 92, 401–422.

16. Gramkow, C. On averaging rotations. J. Math. Imag. Vision 2001, 15, 7–16.

17. LaViola, J.J. A comparison of unscented and extended Kalman filtering for estimating quaternion motion. In Proceedings of the American Control Conference, Denver, CO, USA, 4–6 June 2003; IEEE: Piscataway, NJ, USA, 2003; Volume 3, pp. 2435–2440.

18. Xie, L.; Popa, D.; Lewis, F.L. Optimal and Robust Estimation: With an Introduction to Stochastic Control Theory; CRC Press: Boca Raton, FL, USA, 2007.