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. Main characteristics of the most used parametrizations to represent an orientation.

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

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 a $4\times 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\times 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 ${\mathbb{R}}^4$), 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 $\{\mathit{i},\mathit{j},\mathit{k}\}$ satisfying the Hamilton axiom:

$${\mathit{i}}^2={\mathit{j}}^2={\mathit{k}}^2=\mathit{i}\ast \mathit{j}\ast \mathit{k}=-1.$$

(1)

A quaternion $\mathit{q}$ can be represented with 4 real numbers, and using several notations:

$$\begin{array}{cc}\hfill \mathit{q}& =q_0+q_1\mathit{i}+q_2\mathit{j}+q_3\mathit{k}\equiv \hfill \end{array}$$

(2a)

$$\begin{array}{c}\equiv {\left(q_0,q_1,q_2,q_3\right)}^T\equiv \hfill \end{array}$$

(2b)

$$\begin{array}{c}\equiv {\left(q_0,\mathbf{q}\right)}^T.\hfill \end{array}$$

(2c)

We will denote quaternions with bold italic symbols ($\mathit{q}$), while vectors will be denoted with bold upright symbols ($\mathbf{q}$). Vectors will be written in matrix form, and the transposed of a matrix $\mathbf{M}$ will be denoted as ${\mathbf{M}}^T$.

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

$$\mathit{p}\ast \mathit{q}=\left(\begin{array}{c}{p}_0{q}_0-\mathbf{p}·\mathbf{q}\\ p_0\mathbf{q}+q_0\mathbf{p}+\mathbf{p}\times \mathbf{q}\end{array}\right),$$

(3)

where $(·)$ represents the usual dot product, and $(\times )$ 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 quaternion $\mathit{q}$ (the one for which $\mathit{q}\ast {\mathit{q}}^{-1}={\mathit{q}}^{-1}\ast \mathit{q}=1$) is given by

$${\mathit{q}}^{-1}=\frac{1}{{\parallel \mathit{q}\parallel }^2}{\mathit{q}}^{\ast }=\frac{1}{{\parallel \mathit{q}\parallel }^2}{\left(q_0,-\mathbf{q}\right)}^T,$$

(4)

where ${\mathit{q}}^{\ast }$ represents the complex conjugate quaternion. Note that if $\mathit{q}$ is a unit quaternion (a quaternion with $\parallel \mathit{q}\parallel =1$), then ${\mathit{q}}^{-1}={\mathit{q}}^{\ast }$.

2.2. Quaternions Representing Rotations

Each rotation transformation is mapped with a rotation matrix $\mathbf{R}$ and with two unit quaternions $\mathit{q}$ and $-\mathit{q}$ all of them related through

$$\mathbf{R}(\mathit{q})=\left(\begin{array}{ccc}1-2q_2^2-2q_3^2& 2(q_1{q}_2-q_3{q}_0)& 2(q_1{q}_3+q_2{q}_0)\\ 2(q_1{q}_2+q_3{q}_0)& 1-2q_1^2-2q_3^2& 2(q_2{q}_3-q_1{q}_0)\\ 2(q_1{q}_3-q_2{q}_0)& 2(q_2{q}_3+q_1{q}_0)& 1-2q_1^2-2q_2^2\end{array}\right).$$

(5)

Note that $\mathbf{R}(\mathit{q})=\mathbf{R}(-\mathit{q})$.

Quaternions representing rotations have the form

$$\mathit{q}={\left(cos(\theta /2),\widehat{\mathbf{q}}sin(\theta /2)\right)}^T,$$

(6)

where $\widehat{\mathbf{q}}$ denotes the unit vector that defines the rotation axis, and $\theta$ the angle of rotation. Having this form, they satisfy the restriction

$${\parallel \mathit{q}\parallel }^2=q_0^2+q_1^2+q_2^2+q_3^2=1.$$

(7)

This means that quaternions describing rotations live in the unit sphere of ${\mathbb{R}}^4$, $S^3$. 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 variable $\mathbf{x}$ is encoded by its mean $\overline{\mathbf{x}}=\mathrm{E}[\mathbf{x}]$ and its covariance matrix $\mathbf{P}$ defined as

$$\mathbf{P}=\mathrm{E}\left[\left(\mathbf{x}-\overline{\mathbf{x}}\right){\left(\mathbf{x}-\overline{\mathbf{x}}\right)}^T\right].$$

(8)

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 if $\mathit{q}-\overline{\mathit{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 function $f:X\to Y$ between two topological spaces X and Y satisfying the next properties:

• f is a bijection,
• f is continuous,
• its inverse function $f^{-1}$ is continuous.

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

Definition 2 (Manifold).

A n-manifold $M^n$ is a topological space in which each point is locally homeomorphic to the Euclidean space ${\mathbb{R}}^n$. This is, each point $x\in M^n$ has a neighborhood $N\subset M^n$ for which we can define a homeomorphism $f:N\to B_n$ with $B_n$ the unit ball of ${\mathbb{R}}^n$.

Definition 3 (Chart).

A chart for a manifold $M^n$ is a homeomorphism φ from an open subset $U\subset M^n$ to an open subset of the Euclidean space $V\subset {\mathbb{R}}^n$. This is, a chart is a function

$$\phi :U\subset M^n\to V\subset {\mathbb{R}}^n,$$

(9)

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

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 $S^3$ to the Euclidean space ${\mathbb{R}}^3$. This is, they map a point $\mathit{q}$ in the manifold with a point $\mathbf{e}$ in ${\mathbb{R}}^3$.

Table 2. Main characteristics of the charts studied.

Chart	Domain	Image	$\mathbf{e}=\mathit{\phi }(\mathit{q})$	$\mathit{q}={\mathit{\phi }}^{-1}(\mathbf{e})$
O	$\{\mathit{q}\in S^3:q_0\ge 0\}$	$\{\mathbf{e}\in {\mathbb{R}}^3:\parallel \mathbf{e}\parallel \le 2\}$	$2\mathbf{q}$	$\left(\begin{array}{c}\sqrt{1-\frac{{\parallel \mathbf{e}\parallel }^2}{4}}\\ \mathbf{e}/2\end{array}\right)$
RP	$\{\mathit{q}\in S^3:q_0>0\}$	${\mathbb{R}}^3$	${\displaystyle 2\frac{\mathbf{q}}{q_0}}$	${\displaystyle \frac{1}{\sqrt{4+{\parallel \mathbf{e}\parallel }^2}}\left(\begin{array}{c}2\\ \mathbf{e}\end{array}\right)}$
MRP	$\{\mathit{q}\in S^3:q_0\ge 0\}$	$\{\mathbf{e}\in {\mathbb{R}}^3:\parallel \mathbf{e}\parallel \le 4\}$	${\displaystyle 4\frac{\mathbf{q}}{1+q_0}}$	${\displaystyle \frac{1}{16+{\parallel \mathbf{e}\parallel }^2}\left(\begin{array}{c}16-{\parallel \mathbf{e}\parallel }^2\\ 8\mathbf{e}\end{array}\right)}$
RV	$\{\mathit{q}\in S^3:q_0\ge 0\}$	$\{\mathbf{e}\in {\mathbb{R}}^3:\parallel \mathbf{e}\parallel \le \pi \}$	$2\widehat{\mathbf{q}}arcsin\left(\parallel \mathbf{q}\parallel \right)$	$\left(\begin{array}{c}cos\left(\frac{\parallel \mathbf{e}\parallel }{2}\right)\\ \widehat{\mathbf{e}}sin\left(\frac{\parallel \mathbf{e}\parallel }{2}\right)\end{array}\right)$

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 map $\mathit{q}$ and $-\mathit{q}$ with different points of ${\mathbb{R}}^3$ since 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 $S^2$ (subspace of the sphere $S^3$ where quaternions live) are mapped to points in ${\mathbb{R}}^2$ (subspace of ${\mathbb{R}}^3$ 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 ${\mathbb{R}}^2$ 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 point $\mathbf{e}\in {\mathbb{R}}^3$ near the origin, to a quaternion $\mathit{q}\in S^3$

$${\phi }^{-1}(\mathbf{e})\approx {\left(1-\frac{{\parallel \mathbf{e}\parallel }^2}{8},\frac{\mathbf{e}}{2}\right)}^T.$$

(10)

We should notice that having ${\mathbb{R}}^3$ and $S^3$ different metrics, a chart $\phi$ 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 ${\mathbb{R}}^3$ 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 quaternion $\overline{\mathit{q}}\in S^3$ noting that any quaternion $\mathit{q}\in S^3$ can be expressed as a “deviation” from the first one through the quaternion product:

$$\mathit{q}=\overline{\mathit{q}}\ast {\mathit{\delta }}^{\overline{\mathit{q}}},$$

(11)

where ${\mathit{\delta }}^{\overline{\mathit{q}}}$ represents such a deviation. (This definition is arbitrary: we could have chosen to relate the quaternions through $\mathit{q}={\mathit{\delta }}^{\overline{\mathit{q}}}\ast \overline{\mathit{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 ${\phi }_{\overline{\mathit{q}}}$ for each quaternion $\overline{\mathit{q}}\in S^3$ as

$${\mathbf{e}}^{\overline{\mathit{q}}}={\phi }_{\overline{\mathit{q}}}(\mathit{q})=\phi \left({\mathit{\delta }}^{\overline{\mathit{q}}}\right),$$

(12)

where ${\mathit{\delta }}^{\overline{\mathit{q}}}={\overline{\mathit{q}}}^{\ast }\ast \mathit{q}$ and where we have denoted the point of the Euclidean space mapped with the quaternion $\mathit{q}\in S^3$ through the chart ${\phi }_{\overline{\mathit{q}}}$ as ${\mathbf{e}}^{\overline{\mathit{q}}}$. Then, we will have a set of charts ${\left\{{\phi }_{\overline{\mathit{q}}}\right\}}_{\overline{\mathit{q}}}$, each one resembling the Euclidean space around a quaternion $\overline{\mathit{q}}\in S^3$, and mapping this last quaternion to the origin of ${\mathbb{R}}^3$. We will refer to the Euclidean space associated with the chart ${\phi }_{\overline{\mathit{q}}}$ as the $\overline{\mathit{q}}$-centered chart. Thus, the homeomorphism ${\phi }_{\overline{\mathit{q}}}^{-1}$ takes a point ${\mathbf{e}}^{\overline{\mathit{q}}}$ in the $\overline{\mathit{q}}$-centered chart and maps it to a point $\mathit{q}$ in the manifold through

$$\mathit{q}={\phi }_{\overline{\mathit{q}}}^{-1}\left({\mathbf{e}}^{\overline{\mathit{q}}}\right)=\overline{\mathit{q}}\ast {\phi }^{-1}\left({\mathbf{e}}^{\overline{\mathit{q}}}\right).$$

(13)

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

Given a unit quaternion $\overline{\mathit{q}}$ and a chart $\phi$, we will define the expected value of a distribution of unit quaternions in the $\overline{\mathit{q}}$-centered chart as

$${\overline{\mathbf{e}}}^{\overline{\mathit{q}}}=\mathrm{E}\left[{\mathbf{e}}^{\overline{\mathit{q}}}\right],$$

(14)

and its covariance matrix as

$${\mathbf{P}}^{\overline{\mathit{q}}}=\mathrm{E}\left[\left({\mathbf{e}}^{\overline{\mathit{q}}}-{\overline{\mathbf{e}}}^{\overline{\mathit{q}}}\right){\left({\mathbf{e}}^{\overline{\mathit{q}}}-{\overline{\mathbf{e}}}^{\overline{\mathit{q}}}\right)}^T\right],$$

(15)

and the probability density of each unit quaternion $\mathit{q}$ would be defined through the homeomorphism $\mathit{q}={\phi }_{\overline{\mathit{q}}}^{-1}({\mathbf{e}}^{\overline{\mathit{q}}})$. Then, a distribution of unit quaternions needs of four mathematical objects to be encoded: $\left(\phi ,\overline{\mathit{q}},{\overline{\mathbf{e}}}^{\overline{\mathit{q}}},{\mathbf{P}}^{\overline{\mathit{q}}}\right)$. Although a distribution of unit quaternions is unique, given this definition, its expected value ${\overline{\mathbf{e}}}^{\overline{\mathit{q}}}$ and its covariance matrix ${\mathbf{P}}^{\overline{\mathit{q}}}$ may take different values depending on the chosen quaternion $\overline{\mathit{q}}$ and chart $\phi$. However, knowing that the Kalman filter is designed for the Euclidean space, it will be convenient to choose a unit quaternion $\overline{\mathit{q}}$ central in the distribution, in order that the manifold space around it closely resembles the most significant region for the covariance matrix in the $\overline{\mathit{q}}$-centered chart. It is particularly convenient to choose a quaternion $\overline{\mathit{q}}$ such that ${\overline{\mathbf{e}}}^{\overline{\mathit{q}}}=\mathbf{0}$ so that the covariance matrix is centered in the origin of the $\overline{\mathit{q}}$-centered chart.

2.4. Transition Maps

At some step of the Kalman filter, we will have a distribution of unit quaternions defined in a $\overline{\mathit{q}}$-centered chart, and we will be interested in expressing our distribution in another $\overline{\mathit{p}}$-centered chart. The concept of transition map is relevant for this purpose.

Definition 4 (Transition map).

Given two charts $(U_{\alpha },{\phi }_{\alpha })$ and $(U_{\beta },{\phi }_{\beta })$ for a manifold M, with $U_{\alpha \beta }=U_{\alpha }\cap U_{\beta }\ne \varnothing$, we can define a function ${\phi }_{\alpha \beta }:{\phi }_{\alpha }(U_{\alpha \beta })\to {\phi }_{\beta }(U_{\alpha \beta })$ as

$${\phi }_{\alpha \beta }(x)={\phi }_{\beta }\left({\phi }_{\alpha }^{-1}(x)\right),$$

(16)

with $x\in {\phi }_{\alpha }(U_{\alpha \beta })$. The function ${\phi }_{\alpha \beta }$ is called a transition map. Being that ${\phi }_{\alpha }$ and ${\phi }_{\beta }$ are homeomorphisms, so is ${\phi }_{\alpha \beta }$.

For the present case, let us consider two unit quaternions $\overline{\mathit{p}}$ and $\overline{\mathit{q}}$ both related through

$$\overline{\mathit{p}}=\overline{\mathit{q}}\ast \overline{\mathit{\delta }}.$$

(17)

These two quaternions define the charts ${\phi }_{\overline{\mathit{p}}}$ and ${\phi }_{\overline{\mathit{q}}}$. We build the transition map that relates a point ${\mathbf{e}}^{\overline{\mathit{q}}}$ expressed in the $\overline{\mathit{q}}$-centered chart with a point ${\mathbf{e}}^{\overline{\mathit{p}}}$ expressed in the $\overline{\mathit{p}}$-centered chart doing

$$\begin{array}{cc}\hfill {\mathbf{e}}^{\overline{\mathit{p}}}& ={\phi }_{\overline{\mathit{p}}}\left({\phi }_{\overline{\mathit{q}}}^{-1}\left({\mathbf{e}}^{\overline{\mathit{q}}}\right)\right)=\hfill \end{array}$$

(18a)

$$\begin{array}{c}=\phi \left({\overline{\mathit{p}}}^{\ast }\ast \overline{\mathit{q}}\ast {\phi }^{-1}\left({\mathbf{e}}^{\overline{\mathit{q}}}\right)\right)=\hfill \end{array}$$

(18b)

$$\begin{array}{c}=\phi \left({\overline{\mathit{\delta }}}^{\ast }\ast {\phi }^{-1}\left({\mathbf{e}}^{\overline{\mathit{q}}}\right)\right).\hfill \end{array}$$

(18c)

That is to say, first we take the point ${\mathbf{e}}^{\overline{\mathit{q}}}$ in the $\overline{\mathit{q}}$-centered chart, and we obtain its associated quaternion $\mathit{q}$ in the manifold using ${\phi }_{\overline{\mathit{q}}}^{-1}$. Then, we transform this quaternion $\mathit{q}$ to a point ${\mathbf{e}}^{\overline{\mathit{p}}}$ in the $\overline{\mathit{p}}$-centered chart. Nevertheless, knowing the quaternion $\overline{\mathit{\delta }}$ we do not need to explicitly compute $\mathit{q}$. In fact, being able to express the same quaternion $\mathit{q}$ as two different deviations,

$$\left.\begin{array}{c}\mathit{q}=\overline{\mathit{q}}\ast {\mathit{\delta }}^{\overline{\mathit{q}}}\hfill \\ \mathit{q}=\overline{\mathit{p}}\ast {\mathit{\delta }}^{\overline{\mathit{p}}}\hfill \end{array}\right\}\Rightarrow {\mathit{\delta }}^{\overline{\mathit{p}}}=\underset{{\overline{\delta }}^{\ast }}{\underbrace{{\overline{\mathit{p}}}^{\ast }\ast \overline{\mathit{q}}}}\ast {\mathit{\delta }}^{\overline{\mathit{q}}}.$$

(19)

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

Table 3. Transition maps for the charts studied.

Chart	Transition Map ${\mathbf{e}}^{\overline{\mathit{p}}}\left({\mathbf{e}}^{\overline{\mathit{q}}}\right)$
O	${\displaystyle {\overline{\delta }}_0{\mathbf{e}}^{\overline{\mathit{q}}}-\sqrt{4-\parallel {\mathbf{e}}^{\overline{\mathit{q}}}{\parallel }^2}\overline{\mathsf{\delta }}-\overline{\mathsf{\delta }}\times {\mathbf{e}}^{\overline{\mathit{q}}}}$
RP	${\displaystyle 2\frac{{\overline{\delta }}_0{\mathbf{e}}^{\overline{\mathit{q}}}-2\overline{\mathsf{\delta }}-\overline{\mathsf{\delta }}\times {\mathbf{e}}^{\overline{\mathit{q}}}}{2{\overline{\delta }}_0+\overline{\mathsf{\delta }}·{\mathbf{e}}^{\overline{\mathit{q}}}}}$
MRP	${\displaystyle 4\frac{8{\overline{\delta }}_0{\mathbf{e}}^{\overline{\mathit{q}}}-(16-\parallel {\mathbf{e}}^{\overline{\mathit{q}}}{\parallel }^2)\overline{\mathsf{\delta }}-8\overline{\mathsf{\delta }}\times {\mathbf{e}}^{\overline{\mathit{q}}}}{16+\parallel {\mathbf{e}}^{\overline{\mathit{q}}}{\parallel }^2+{\overline{\delta }}_0(16-\parallel {\mathbf{e}}^{\overline{\mathit{q}}}{\parallel }^2)+8\overline{\mathsf{\delta }}·{\mathbf{e}}^{\overline{\mathit{q}}}}}$
RV	${\displaystyle 2\frac{{\mathsf{\delta }}^{\overline{\mathit{p}}}}{\parallel {\mathsf{\delta }}^{\overline{\mathit{p}}}\parallel }arcsin\parallel {\mathsf{\delta }}^{\overline{\mathit{p}}}\parallel }$,   with ${\mathsf{\delta }}^{\overline{\mathit{p}}}={\overline{\delta }}_0{\widehat{\mathbf{e}}}^{\overline{\mathit{q}}}sin\left(\frac{\parallel {\mathbf{e}}^{\overline{\mathit{q}}}\parallel }{2}\right)-cos\left(\frac{\parallel {\mathbf{e}}^{\overline{\mathit{q}}}\parallel }{2}\right)\overline{\mathsf{\delta }}-\overline{\mathsf{\delta }}\times {\widehat{\mathbf{e}}}^{\overline{\mathit{q}}}sin\left(\frac{\parallel {\mathbf{e}}^{\overline{\mathit{q}}}\parallel }{2}\right)$

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 ${\mathit{q}}_t$ and by an angular velocity ${\mathsf{\omega }}_t^{\prime }$. We will consider them to be random variables, and we will try to estimate their value using a Kalman filter.

Our unit quaternions $\left\{{\mathit{q}}_t\in \mathbb{H}:\parallel {\mathit{q}}_t\parallel =1\right\}$ will define the rotation transformation that relates a vector ${\mathbf{v}}_t^{\prime }$ expressed in a reference frame ${\mathcal{S}}^{\prime }$ attached to the solid whose state we want to describe, with the same vector ${\mathbf{v}}_t$ expressed in an external reference frame $\mathcal{S}$

$${\mathbf{v}}_t=\mathbf{R}({\mathit{q}}_t){\mathbf{v}}_t^{\prime }\equiv {\mathit{v}}_t={\mathit{q}}_t\ast {\mathit{v}}_t^{\prime }\ast {\mathit{q}}_t^{\ast }.$$

(20)

For example, if we measure an acceleration ${\mathbf{a}}_t^{\prime }$ in reference frame ${\mathcal{S}}^{\prime }$ the acceleration in the inertial reference frame $\mathcal{S}$ would be given by ${\mathbf{a}}_t=\mathbf{R}({\mathit{q}}_t){\mathbf{a}}_t^{\prime }$. This acceleration would be the one that we would have to integrate to obtain the position estimated by an accelerometer.

The vector ${\mathsf{\omega }}_t^{\prime }$ will define the angular velocity of the solid measured in ${\mathcal{S}}^{\prime }$. 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:

$$\begin{array}{cc}\hfill \frac{d{\mathsf{\omega }}^{\prime }(t)}{dt}& ={\mathbf{q}}^{\mathsf{\omega }}(t),\hfill \end{array}$$

(21)

$$\begin{array}{cc}\hfill \frac{d\mathit{q}(t)}{dt}& =\frac{1}{2}\mathit{q}(t)\ast {\mathit{\omega }}^{\prime }(t)=\frac{1}{2}\mathit{q}(t)\ast \left(\begin{array}{c}0\\ {\mathsf{\omega }}^{\prime }(t)\end{array}\right),\hfill \end{array}$$

(22)

where ${\mathbf{q}}^{\mathsf{\omega }}(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 ${\overline{\mathbf{q}}}_t^{\mathsf{\omega }}$ and its covariance matrix will be denoted as ${\mathbf{Q}}_t^{\mathsf{\omega }}$.

We will assume that we have sensors giving measurements of angular velocity ${\mathsf{\omega }}_t^m$ (which provide information about the relative change in orientation), and of a vector ${\mathbf{v}}_t^m$ whose value ${\mathbf{v}}_t$ expressed in the external reference frame $\mathcal{S}$ is 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 (${\mathbf{v}}_t:=-\mathbf{g}$), or a magnetometer measuring the Earth magnetic field (${\mathbf{v}}_t:=\mathbf{B}$). The measurement model relates these measurements with the variables that describe the state of the system:

$$\begin{array}{cc}\hfill {\mathbf{v}}_t^m& ={\mathbf{R}}^T({\mathit{q}}_t)\left({\mathbf{q}}_t^{\mathbf{v}}+{\mathbf{v}}_t\right)+{\mathbf{r}}_t^{\mathbf{v}},\hfill \end{array}$$

(23)

$$\begin{array}{cc}\hfill {\mathsf{\omega }}_t^m& ={\mathsf{\omega }}_t^{\prime }+{\mathbf{r}}_t^{\mathsf{\omega }},\hfill \end{array}$$

(24)

where ${\mathbf{r}}_t^{\mathsf{\omega }}$ and ${\mathbf{r}}_t^{\mathbf{v}}$ are random variables with zero mean and covariance matrices ${\mathbf{R}}_t^{\mathsf{\omega }}$ and ${\mathbf{R}}_t^{\mathbf{v}}$ respectively that represent the measurement noises, and ${\mathbf{q}}_t^{\mathbf{v}}$ is another random variable with mean ${\overline{\mathbf{q}}}_t^{\mathbf{v}}$ and covariance matrix ${\mathbf{Q}}_t^{\mathbf{v}}$ representing external disturbances in the measurement of the vector ${\mathbf{v}}_t$. 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 ${\{t_n\}}_n$. The format $x_{t|t_n}$ will be used to denote a variable x at a time t, having included measurements up to a time $t_n$ with $t>t_n$. For the n-th time stamp, in which a measurement arrives, we will write $x_{t|n}$ for the sake of simplicity. Then, our knowledge about the state at a time t, having included measurements up to a time $t_n$ with $t>t_n$ is described by a distribution encoded in the collection of mathematical objects $\left(\phi ,\overline{\mathit{p}},{\overline{\mathbf{x}}}_{t|n}^{\overline{\mathit{p}}},{\mathbf{P}}_{t|n}^{\overline{\mathit{p}}}\right)$ as described in Section 2.3. For the present case, ${\overline{\mathbf{x}}}_{t|n}^{\overline{\mathit{p}}}=\left({\overline{\mathbf{e}}}_{t|n}^{\overline{\mathit{p}}},{\overline{\mathsf{\omega }}}_{t|n}^{\prime }\right){}^T$ is the expected value of the distribution, and ${\mathbf{P}}_{t|n}^{\overline{\mathit{p}}}$ is its $6\times 6$ covariance matrix, both expressing the quaternion distribution in the $\overline{\mathit{p}}$-centered chart. Preferably, $\overline{\mathit{p}}$ will be a unit quaternion central in the distribution, so that the mapping of points from the $\overline{\mathit{p}}$-centered chart to the manifold causes minimal deformation in such distribution. The unit quaternion ${\overline{\mathit{q}}}_{t|n}={\phi }_{\overline{\mathit{p}}}^{-1}\left({\overline{\mathbf{e}}}_{t|n}^{\overline{\mathit{p}}}\right)$ will be our best estimation of the real quaternion ${\mathit{q}}_t$ that defines the orientation of the system with respect to the external reference frame $\mathcal{S}$ at 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

$${\mathbf{z}}_n=\left(\begin{array}{c}{\mathbf{v}}_n^m\\ {\mathsf{\omega }}_n^m\end{array}\right)$$

(25)

arrives at time $t_n$. Our knowledge about the orientation at a previous time $t_{n-1}$ is described by a distribution expressed in the ${\overline{\mathit{q}}}_{n-1|n-1}$-centered chart. We assume that this distribution has mean

$${\overline{\mathbf{x}}}_{n-1|n-1}^{{\overline{\mathit{q}}}_{n-1|n-1}}=\left(\begin{array}{c}{\overline{\mathbf{e}}}_{n-1|n-1}^{{\overline{\mathit{q}}}_{n-1|n-1}}=\mathbf{0}\\ {\overline{\mathsf{\omega }}}_{n-1|n-1}^{\prime }\end{array}\right),$$

(26)

and covariance matrix ${\mathbf{P}}_{n-1|n-1}^{{\overline{\mathit{q}}}_{n-1|n-1}}$. This is, we have an initial four

$$\left(\phi ,{\overline{\mathit{q}}}_{n-1|n-1},{\overline{\mathsf{\omega }}}_{n-1|n-1}^{\prime },{\mathbf{P}}_{n-1|n-1}^{{\overline{\mathit{q}}}_{n-1|n-1}}\right).$$

(27)

The state prediction at time $t_n$ given all the information up to $t_{n-1}$ is computed through

$$\begin{array}{cc}\hfill {\overline{\mathsf{\omega }}}_{n|n-1}^{\prime }& ={\overline{\mathsf{\omega }}}_{n-1|n-1}^{\prime },\hfill \end{array}$$

(28)

$$\begin{array}{cc}\hfill {\mathit{\delta }}_n^{\mathsf{\omega }}& =\left(\begin{array}{c}cos\left(\frac{\parallel {\overline{\mathsf{\omega }}}_{n|n-1}^{\prime }\parallel \Delta t_n}{2}\right)\\ \frac{{\overline{\mathsf{\omega }}}_{n|n-1}^{\prime }}{\parallel {\overline{\mathsf{\omega }}}_{n|n-1}^{\prime }\parallel }sin\left(\frac{\parallel {\overline{\mathsf{\omega }}}_{n|n-1}^{\prime }\parallel \Delta t_n}{2}\right)\end{array}\right),\hfill \end{array}$$

(29)

$$\begin{array}{cc}\hfill {\overline{\mathit{q}}}_{n|n-1}& ={\overline{\mathit{q}}}_{n-1|n-1}\ast {\mathit{\delta }}_n^{\mathsf{\omega }},\hfill \end{array}$$

(30)

$$\begin{array}{cc}\hfill {\mathbf{F}}_n& =\left(\begin{array}{cc}{\mathbf{R}}^T({\mathit{\delta }}_n^{\mathsf{\omega }})& \mathbf{I}\Delta t_n\\ \mathbf{0}& \mathbf{I}\end{array}\right),\hfill \end{array}$$

(31)

$$\begin{array}{cc}\hfill {\mathbf{P}}_{n|n-1}^{{\overline{\mathit{q}}}_{n|n-1}}& ={\mathbf{F}}_n\left[{\mathbf{P}}_{n-1|n-1}^{{\overline{\mathit{q}}}_{n-1|n-1}}+{\mathbf{Q}}_n\right]{\mathbf{F}}_n^T,\hfill \end{array}$$

(32)

with

$${\mathbf{Q}}_n=\left(\begin{array}{cc}{\mathbf{Q}}_n^{\mathsf{\omega }}\frac{{(\Delta t_n)}^3}{3}& -{\mathbf{Q}}_n^{\mathsf{\omega }}\frac{{(\Delta t_n)}^2}{2}\\ -{\mathbf{Q}}_n^{\mathsf{\omega }}\frac{{(\Delta t_n)}^2}{2}& {\mathbf{Q}}_n^{\mathsf{\omega }}\Delta t_n\end{array}\right).$$

(33)

The measurement prediction at the same time is given by

$$\begin{array}{cc}\hfill {\overline{\mathbf{v}}}_{n|n-1}^m& ={\mathbf{R}}^T\left({\overline{\mathit{q}}}_{n|n-1}\right)\left({\overline{\mathbf{q}}}_n^{\mathbf{v}}+{\mathbf{v}}_n\right),\hfill \end{array}$$

(34)

$$\begin{array}{cc}\hfill {\overline{\mathsf{\omega }}}_{n|n-1}^m& ={\overline{\mathsf{\omega }}}_{n|n-1}^{\prime },\hfill \end{array}$$

(35)

$$\begin{array}{cc}\hfill {\overline{\mathbf{z}}}_{n|n-1}& =\left(\begin{array}{c}{\overline{\mathbf{v}}}_{n|n-1}^m\\ {\overline{\mathsf{\omega }}}_{n|n-1}^m\end{array}\right),\hfill \end{array}$$

(36)

$$\begin{array}{cc}\hfill {\mathbf{H}}_n& =\left(\begin{array}{cc}{\left[{\overline{\mathbf{v}}}_{n|n-1}^m\right]}_{\times }& \mathbf{0}\\ \mathbf{0}& \mathbf{I}\end{array}\right),\hfill \end{array}$$

(37)

$$\begin{array}{cc}\hfill {\mathbf{S}}_{n|n-1}& ={\mathbf{H}}_n{\mathbf{P}}_{n|n-1}^{{\overline{\mathit{q}}}_{n|n-1}}{\mathbf{H}}_n^T+\left(\begin{array}{cc}{\mathbf{R}}^T\left({\overline{\mathit{q}}}_{n|n-1}\right){\mathbf{Q}}_n^{\mathbf{v}}\mathbf{R}\left({\overline{\mathit{q}}}_{n|n-1}\right)+{\mathbf{R}}_n^{\mathbf{v}}& \mathbf{0}\\ \mathbf{0}& {\mathbf{R}}_n^{\mathsf{\omega }}\end{array}\right),\hfill \end{array}$$

(38)

where ${[\mathbf{v}]}_{\times }$ stands for

$${[\mathbf{v}]}_{\times }=\left(\begin{array}{ccc}0& -v_3& v_2\\ v_3& 0& -v_1\\ -v_2& v_1& 0\end{array}\right).$$

(39)

At this point, we compute the Kalman gain ${\mathbf{K}}_n$ and use it to obtain the optimal estimation of the state:

$$\begin{array}{cc}\hfill {\mathbf{K}}_n& ={\mathbf{P}}_{n|n-1}^{{\overline{\mathit{q}}}_{n|n-1}}{\mathbf{H}}_n^T{\mathbf{S}}_{n|n-1}^{-1},\hfill \end{array}$$

(40)

$$\begin{array}{cc}\hfill {\overline{\mathbf{x}}}_{n|n}^{{\overline{\mathit{q}}}_{n|n-1}}& ={\overline{\mathbf{x}}}_{n|n-1}^{{\overline{\mathit{q}}}_{n|n-1}}+{\mathbf{K}}_n\left({\mathbf{z}}_n-{\overline{\mathbf{z}}}_{n|n-1}\right),\hfill \end{array}$$

(41)

$$\begin{array}{cc}\hfill {\mathbf{P}}_{n|n}^{{\overline{\mathit{q}}}_{n|n-1}}& =\left(\mathbf{I}-{\mathbf{K}}_n{\mathbf{H}}_n\right){\mathbf{P}}_{n|n-1}^{{\overline{\mathit{q}}}_{n|n-1}},\hfill \end{array}$$

(42)

where ${\overline{\mathbf{x}}}_{n|n-1}^{{\overline{\mathit{q}}}_{n|n-1}}=\left({\overline{\mathbf{e}}}_{n|n-1}^{{\overline{\mathit{q}}}_{n|n-1}}=\mathbf{0},{\overline{\mathsf{\omega }}}_{n|n-1}^{\prime }\right){}^T$. Finally, we need to obtain the updated unit quaternion, ${\overline{\mathit{q}}}_{n|n}$ and compute the mean and the covariance matrix in the ${\overline{\mathit{q}}}_{n|n}$-centered chart, so that the distribution is expressed in the same conditions as at the beginning of the iteration. The point ${\overline{\mathbf{e}}}_{n|n}^{{\overline{\mathit{q}}}_{n|n-1}}$ that results from (41), and that is defined in the ${\overline{\mathit{q}}}_{n|n-1}$-centered chart, correspond to a unit quaternion in the manifold. This is the updated unit quaternion ${\overline{\mathit{q}}}_{n|n}$ which we are looking for:

$$\begin{array}{cc}\hfill {\overline{\mathit{q}}}_{n|n}& ={\phi }_{{\overline{\mathit{q}}}_{n|n-1}}^{-1}\left({\overline{\mathbf{e}}}_{n|n}^{{\overline{\mathit{q}}}_{n|n-1}}\right)=\hfill \end{array}$$

(43a)

$$\begin{array}{c}={\overline{\mathit{q}}}_{n|n-1}\ast {\phi }^{-1}\left({\overline{\mathbf{e}}}_{n|n}^{{\overline{\mathit{q}}}_{n|n-1}}\right)=\hfill \end{array}$$

(43b)

$$\begin{array}{c}={\overline{\mathit{q}}}_{n|n-1}\ast {\overline{\delta }}_n.\hfill \end{array}$$

(43c)

Knowing that the Kalman update (41) could produce any point in the ${\overline{\mathit{q}}}_{n|n-1}$-centered chart we will need to “saturate” to the closest point contained in the image of each chart. The point ${\overline{\mathbf{e}}}_{n|n}^{{\overline{\mathit{q}}}_{n|n-1}}$ in the ${\overline{\mathit{q}}}_{n|n-1}$-centered chart is the origin in the ${\overline{\mathit{q}}}_{n|n}$-centered chart. Then, the expected value of the state in this new chart will be given by ${\overline{\mathbf{x}}}_{n|n}^{{\overline{\mathit{q}}}_{n|n}}=\left({\overline{\mathbf{e}}}_{n|n}^{{\overline{\mathit{q}}}_{n|n}}=\mathbf{0},{\overline{\mathsf{\omega }}}_{n|n}^{\prime }\right){}^T$ as at the beginning of the iteration.

To update the covariance matrix we need to consider its definition (15). We want to compute ${\mathbf{P}}^{{\overline{\mathit{q}}}_{n|n}}$ having ${\mathbf{P}}^{{\overline{\mathit{q}}}_{n|n-1}}$ and knowing the relation ${\mathbf{e}}^{\overline{\mathit{p}}}\left({\mathbf{e}}^{\overline{\mathit{q}}}\right)$ provided by the transition maps in Table 3. Continuing with the EKF philosophy, the update for the covariance matrix will be found by linearizing ${\mathbf{e}}^{\overline{\mathit{p}}}\left({\mathbf{e}}^{\overline{\mathit{q}}}\right)$ around the point where the majority of information is comprised (in our case, the point ${\overline{\mathbf{e}}}^{\overline{\mathit{q}}}={\overline{\mathbf{e}}}_{n|n}^{{\overline{\mathit{q}}}_{n|n-1}}$):

$$\begin{array}{cc}\hfill e_i^{\overline{\mathit{p}}}\left({\mathbf{e}}^{\overline{\mathit{q}}}\right)& =e_i^{\overline{\mathit{p}}}\left({\overline{\mathbf{e}}}^{\overline{\mathit{q}}}\right)+\sum _j{\left.\frac{\partial e_i^{\overline{\mathit{p}}}\left({\mathbf{e}}^{\overline{\mathit{q}}}\right)}{\partial e_j^{\overline{\mathit{q}}}}\right|}_{{\mathbf{e}}^{\overline{\mathit{q}}}={\overline{\mathbf{e}}}^{\overline{\mathit{q}}}}\left(e_j^{\overline{\mathit{q}}}-{\overline{e}}_j^{\overline{\mathit{q}}}\right)+\mathcal{O}\left(\parallel {\mathbf{e}}^{\overline{\mathit{q}}}-{\overline{\mathbf{e}}}^{\overline{\mathit{q}}}{\parallel }^2\right),\hfill \end{array}$$

(44)

where we have used the big O notation to describe the limiting behavior of the error term of the approximation as ${\mathbf{e}}^{\overline{\mathit{q}}}\to {\overline{\mathbf{e}}}^{\overline{\mathit{q}}}$. In particular, if we define

$${(\mathbf{T})}_{ij}={\left.\frac{\partial e_i^{\overline{\mathit{p}}}\left({\mathbf{e}}^{\overline{\mathit{q}}}\right)}{\partial e_j^{\overline{\mathit{q}}}}\right|}_{{\mathbf{e}}^{\overline{\mathit{q}}}={\overline{\mathbf{e}}}^{\overline{\mathit{q}}}},$$

(45)

then,

$${\mathbf{e}}^{\overline{\mathit{p}}}-{\overline{\mathbf{e}}}^{\overline{\mathit{p}}}\approx {\mathbf{e}}^{\overline{\mathit{p}}}\left({\mathbf{e}}^{\overline{\mathit{q}}}\right)-{\mathbf{e}}^{\overline{\mathit{p}}}\left({\overline{\mathbf{e}}}^{\overline{\mathit{q}}}\right)\approx \mathbf{T}\left({\mathbf{e}}^{\overline{\mathit{q}}}-{\overline{\mathbf{e}}}^{\overline{\mathit{q}}}\right),$$

(46)

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

$$\begin{array}{cc}\hfill {\mathbf{P}}_{n|n}^{{\overline{\mathit{q}}}_{n|n}}& =\mathrm{E}\left[({\mathbf{x}}_{n|n}^{{\overline{\mathit{q}}}_{n|n}}-{\overline{\mathbf{x}}}_{n|n}^{{\overline{\mathit{q}}}_{n|n}}){({\mathbf{x}}_{n|n}^{{\overline{\mathit{q}}}_{n|n}}-{\overline{\mathbf{x}}}_{n|n}^{{\overline{\mathit{q}}}_{n|n}})}^T\right]\approx \hfill \end{array}$$

(47a)

$$\begin{array}{c}\approx \left(\begin{array}{cc}\mathbf{T}({\overline{\delta }}_n)& \mathbf{0}\\ \mathbf{0}& \mathbf{I}\end{array}\right){\mathbf{P}}_{n|n}^{{\overline{\mathit{q}}}_{n|n-1}}{\left(\begin{array}{cc}\mathbf{T}({\overline{\delta }}_n)& \mathbf{0}\\ \mathbf{0}& \mathbf{I}\end{array}\right)}^T.\hfill \end{array}$$

(47b)

Table 4. $\mathbf{T}$-matrices for the transition maps of the charts studied.

Chart	$\mathbf{T}(\overline{\mathit{\delta }})$ Matrix	Domain
O	${\displaystyle {\overline{\delta }}_0\mathbf{I}-{\left[\overline{\mathsf{\delta }}\right]}_{\times }+\frac{\overline{\mathsf{\delta }}{\overline{\mathsf{\delta }}}^T}{{\overline{\delta }}_0}}$	$\{\overline{\delta }\in S^3:{\overline{\delta }}_0>0\}$
RP	${\displaystyle {\overline{\delta }}_0\left({\overline{\delta }}_0\mathbf{I}-{\left[\overline{\mathsf{\delta }}\right]}_{\times }\right)}$	$\{\overline{\delta }\in S^3:{\overline{\delta }}_0\ne 0\}$
MRP	${\displaystyle \frac{1}{2}\left[\left(1+{\overline{\delta }}_0\right)\left({\overline{\delta }}_0\mathbf{I}-{\left[\overline{\mathsf{\delta }}\right]}_{\times }\right)+\overline{\mathsf{\delta }}{\overline{\mathsf{\delta }}}^T\right]}$	$\{\overline{\delta }\in S^3:{\overline{\delta }}_0\ge 0\}$
RV	${\displaystyle \left[{\overline{\delta }}_0\left(\mathbf{I}-\widehat{\overline{\mathsf{\delta }}}{\widehat{\overline{\mathsf{\delta }}}}^T\right)-{\left[\overline{\mathsf{\delta }}\right]}_{\times }\right]\frac{\parallel \overline{\mathsf{\delta }}\parallel }{arcsin\parallel \overline{\mathsf{\delta }}\parallel }+\widehat{\overline{\mathsf{\delta }}}{\widehat{\overline{\mathsf{\delta }}}}^T}$	$\{\overline{\delta }\in S^3:{\overline{\delta }}_0\ge 0,\parallel \overline{\mathsf{\delta }}\parallel \ne 0\}$

summarizes the resulting $\mathbf{T}$-matrix for each chart, along with their application domain. A detailed derivation of these $\mathbf{T}$-matrices can be found in Appendix C.

After the final computation we obtain the four

$$\left(\phi ,{\overline{\mathit{q}}}_{n|n},{\overline{\mathsf{\omega }}}_{n|n}^{\prime },{\mathbf{P}}_{n|n}^{{\overline{\mathit{q}}}_{n|n}}\right),$$

(48)

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 ${\mathbf{z}}_n$ arrives at time $t_n$. Our knowledge about the orientation at a previous time $t_{n-1}$ is described by a distribution expressed in the ${\overline{\mathit{q}}}_{n-1|n-2}$-centered chart. This distribution is encoded in the four

$$\left(\phi ,{\overline{\mathit{q}}}_{n-1|n-2},{\overline{\mathbf{x}}}_{n-1|n-1}^{{\overline{\mathit{q}}}_{n-1|n-2}},{\mathbf{P}}_{n-1|n-1}^{{\overline{\mathit{q}}}_{n-1|n-2}}\right).$$

(49)

The first step in the UKF is to create the augmented $N\times 1$ mean ${\tilde{\mathbf{x}}}_n$ and $N\times N$ covariance matrix ${\tilde{\mathbf{P}}}_n$. Since the measurement equations are linear for the random variables ${\mathbf{r}}_t^{\mathsf{\omega }}$ and ${\mathbf{r}}_t^{\mathbf{v}}$ we can leave their covariance matrices out of the augmented one and add them later:

$$\begin{array}{cc}\hfill {\tilde{\mathbf{x}}}_n& =\left(\begin{array}{c}{\overline{\mathbf{x}}}_{n-1|n-1}^{{\overline{\mathit{q}}}_{n-1|n-2}}\\ {\overline{\mathbf{q}}}_n^{\mathsf{\omega }}\\ {\overline{\mathbf{q}}}_n^{\mathbf{v}}\end{array}\right),\hfill \end{array}$$

(50)

$$\begin{array}{cc}\hfill {\tilde{\mathbf{P}}}_n& =\left(\begin{array}{ccc}{\mathbf{P}}_{n-1|n-1}^{{\overline{\mathit{q}}}_{n-1|n-2}}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& {\mathbf{Q}}_n^{\mathsf{\omega }}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& {\mathbf{Q}}_n^{\mathbf{v}}\end{array}\right).\hfill \end{array}$$

(51)

Then, we obtain the matrix ${\mathbf{L}}_n$ which satisfies ${\mathbf{L}}_n{\mathbf{L}}_n^T={\tilde{\mathbf{P}}}_n$ and we use it to generate the $2N+1$ sigma points ${\{{\mathcal{X}}_j\}}_{j=0}^{2N}$ as described in Ref. [15]:

$$\begin{array}{cc}\hfill {\mathcal{X}}_{i,0}& ={({\tilde{\mathbf{x}}}_n)}_i,\hfill \end{array}$$

(52a)

$$\begin{array}{cc}\hfill {\mathcal{X}}_{i,j}& ={({\tilde{\mathbf{x}}}_n)}_i+\frac{{\left({\mathbf{L}}_n\right)}_{ij}}{\sqrt{2W_j}}\mathrm{for}j=1,\dots ,N,\hfill \end{array}$$

(52b)

$$\begin{array}{cc}\hfill {\mathcal{X}}_{i,j+N}& ={({\tilde{\mathbf{x}}}_n)}_i-\frac{{\left({\mathbf{L}}_n\right)}_{ij}}{\sqrt{2W_j}}\mathrm{for}j=1,\dots ,N,\hfill \end{array}$$

(52c)

being $W_j=(1-W_0)/(2N)$ for $j\ne 0$ where $W_0$ regulates the importance given to the sigma point ${\mathcal{X}}_0$ in the computation of the mean. These sigma points ${\{{\mathcal{X}}_j\}}_j$ are expressed in the ${\overline{\mathit{q}}}_{n-1|n-2}$-centered chart. We need to express them in the manifold before applying the evolution equations and the measurement equations:

$$\begin{array}{cc}\hfill {\mathcal{X}}_j^{\mathit{q}}& ={\phi }_{{\overline{\mathit{q}}}_{n-1|n-2}}^{-1}\left({\mathcal{X}}_j^{\mathbf{e}}\right)={\overline{\mathit{q}}}_{n-1|n-2}\ast {\phi }^{-1}\left({\mathcal{X}}_j^{\mathbf{e}}\right),\hfill \end{array}$$

(53)

$$\begin{array}{cc}\hfill {\mathcal{Y}}_j^{\mathsf{\omega }}& ={\mathcal{X}}_j^{\mathsf{\omega }}+{\mathcal{X}}_j^{{\mathbf{q}}^{\mathsf{\omega }}}\Delta t_n,\hfill \end{array}$$

(54)

$$\begin{array}{cc}\hfill {\mathcal{Y}}_j^{\mathit{q}}& ={\mathcal{X}}_j^{\mathit{q}}\ast \left(\begin{array}{c}cos\left(\frac{\parallel {\mathcal{Y}}_j^{\mathsf{\omega }}\parallel \Delta t_n}{2}\right)\\ {\widehat{\mathcal{Y}}}_j^{\mathsf{\omega }}sin\left(\frac{\parallel {\mathcal{Y}}_j^{\mathsf{\omega }}\parallel \Delta t_n}{2}\right)\end{array}\right),\hfill \end{array}$$

(55)

$$\begin{array}{cc}\hfill {\mathcal{Z}}_j^{\mathbf{v}}& ={\mathbf{R}}^T\left({\mathcal{X}}_j^{\mathit{q}}\right)\left({\mathcal{X}}_j^{\mathbf{v}}+{\mathbf{v}}_t\right),\hfill \end{array}$$

(56)

$$\begin{array}{cc}\hfill {\mathcal{Z}}_j^{\mathsf{\omega }}& ={\mathcal{Y}}_j^{\mathsf{\omega }},\hfill \end{array}$$

(57)

where for the j-th sigma point, ${\mathcal{X}}_j^{\mathbf{e}}$ is its chart point part and ${\mathcal{X}}_j^{\mathit{q}}$ is the quaternion with which it is mapped, ${\mathcal{X}}_j^{\mathsf{\omega }}$ is its angular velocity part, ${\mathcal{X}}_j^{{\mathit{q}}^{\mathsf{\omega }}}$ is its angular velocity noise part, ${\mathcal{Y}}_j^{\mathsf{\omega }}$ is its angular velocity prediction, ${\mathcal{Y}}_j^{\mathit{q}}$ is the quaternion part of its prediction (we have assumed that the angular velocity ${\mathcal{Y}}_j^{\mathsf{\omega }}$ is constant in the time interval $[t_{n-1},t_n)$ so that we can use (A20)), ${\mathcal{X}}_j^{\mathbf{v}}$ is the vector process noise part, ${\mathcal{Z}}_j^{\mathbf{v}}$ is its vector measurement prediction, ${\mathcal{Z}}_j^{\mathsf{\omega }}$ is its angular velocity measurement prediction, and $\Delta t_n=t_n-t_{n-1}$. Note that when applying the inverse chart ${\phi }^{-1}$ we will need to “saturate” ${\mathcal{X}}_j^{\mathbf{e}}$ to the closest point in the image of $\phi$. Having these new sigma points, we can obtain the means and covariance matrices of the distributions present in the UKF. First, defining ${\mathcal{Z}}_j:=\left({\mathcal{Z}}_j^{\mathbf{v}},{\mathcal{Z}}_j^{\mathsf{\omega }}\right){}^T$ the means are computed through

$$\begin{array}{cc}\hfill {\overline{\mathit{q}}}_{n|n-1}& =\frac{{\sum }_j{W}_j{\mathcal{Y}}_j^{\mathit{q}}}{\parallel {\sum }_j{W}_j{\mathcal{Y}}_j^{\mathit{q}}\parallel },\hfill \end{array}$$

(58)

$$\begin{array}{cc}\hfill {\overline{\mathsf{\omega }}}_{n|n-1}^{\prime }& =\sum _j{W}_j{\mathcal{Y}}_j^{\mathsf{\omega }},\hfill \end{array}$$

(59)

$$\begin{array}{cc}\hfill {\overline{\mathbf{x}}}_{n|n-1}^{{\overline{\mathit{q}}}_{n|n-1}}& =\left(\begin{array}{c}{\phi }_{{\overline{\mathit{q}}}_{n|n-1}}\left({\overline{\mathit{q}}}_{n|n-1}\right)=\mathbf{0}\\ {\overline{\mathsf{\omega }}}_{n|n-1}^{\prime }\end{array}\right),\hfill \end{array}$$

(60)

$$\begin{array}{cc}\hfill {\overline{\mathbf{z}}}_{n|n-1}& =\sum _j{W}_j{\mathcal{Z}}_j.\hfill \end{array}$$

(61)

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

$$\overline{\mathit{q}}\approx \frac{{\sum }_j{\mathit{q}}_j}{\parallel {\sum }_j{\mathit{q}}_j\parallel },$$

(62)

with ${\mathit{q}}_j·{\mathit{q}}_k>0$ for $j,k=0,\dots ,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 ${\mathit{q}}_j$ and the one represented by $\overline{\mathit{q}}$. This approach to compute the mean quaternion is extremely efficient, and its derivation is elegant and simple. In order to ensure that ${\mathit{q}}_j·{\mathit{q}}_k>0$ it is useful to remember the property that both $\mathit{q}$ and $-\mathit{q}$ represent the same rotation. This property is also useful for introducing the quaternions in the domain of $\phi$ to execute the next step of the filter.

After this, we use the obtained mean quaternion ${\overline{\mathit{q}}}_{n|n-1}$ to express each sigma point in the ${\overline{\mathit{q}}}_{n|n-1}$-centered chart, and compute the covariance matrices:

$$\begin{array}{cc}\hfill {\mathcal{Y}}_j^{\mathbf{e}}& ={\phi }_{{\overline{\mathit{q}}}_{n|n-1}}\left({\mathcal{Y}}_j^{\mathit{q}}\right)=\phi \left({\overline{\mathit{q}}}_{n|n-1}^{\ast }\ast {\mathcal{Y}}_j^{\mathit{q}}\right),\hfill \end{array}$$

(63)

$$\begin{array}{cc}\hfill {\mathbf{P}}_{n|n-1}^{{\overline{\mathit{q}}}_{n|n-1}}& =\sum _j{W}_j{\mathcal{Y}}_j{\mathcal{Y}}_j^T,\hfill \end{array}$$

(64)

$$\begin{array}{cc}\hfill {\mathbf{P}}_{n|n-1}^{yz}& =\sum _j{W}_j{\mathcal{Y}}_j{\left({\mathcal{Z}}_j-{\overline{\mathbf{z}}}_{n|n-1}\right)}^T,\hfill \end{array}$$

(65)

$$\begin{array}{cc}\hfill {\mathbf{S}}_{n|n-1}& =\sum _j{W}_j\left({\mathcal{Z}}_j-{\overline{\mathbf{z}}}_{n|n-1}\right){\left({\mathcal{Z}}_j-{\overline{\mathbf{z}}}_{n|n-1}\right)}^T+\left(\begin{array}{cc}{\mathbf{R}}_n^{\mathbf{v}}& \mathbf{0}\\ \mathbf{0}& {\mathbf{R}}_n^{\mathsf{\omega }}\end{array}\right),\hfill \end{array}$$

(66)

where we have denoted ${\mathcal{Y}}_j:=\left({\mathcal{Y}}_j^{\mathbf{e}},{\mathcal{Y}}_j^{\mathsf{\omega }}-{\overline{\mathsf{\omega }}}_{n|n-1}^{\prime }\right){}^T$. Finally, we compute the UKF version of the Kalman gain ${\mathbf{K}}_n$ and we use it to obtain the optimal estimation of the state:

$$\begin{array}{cc}\hfill {\mathbf{K}}_n& ={\mathbf{P}}_{n|n-1}^{yz}{\mathbf{S}}_{n|n-1}^{-1},\hfill \end{array}$$

(67)

$$\begin{array}{cc}\hfill {\overline{\mathbf{x}}}_{n|n}^{{\overline{\mathit{q}}}_{n|n-1}}& ={\overline{\mathbf{x}}}_{n|n-1}^{{\overline{\mathit{q}}}_{n|n-1}}+{\mathbf{K}}_n\left({\mathbf{z}}_n-{\overline{\mathbf{z}}}_{n|n-1}\right),\hfill \end{array}$$

(68)

$$\begin{array}{cc}\hfill {\mathbf{P}}_{n|n}^{{\overline{\mathit{q}}}_{n|n-1}}& ={\mathbf{P}}_{n|n-1}^{{\overline{\mathit{q}}}_{n|n-1}}-{\mathbf{K}}_n{\mathbf{S}}_{n|n-1}{\mathbf{K}}_n^T,\hfill \end{array}$$

(69)

arriving at the same conditions in which we began the iteration, with a distribution expressed in the ${\overline{\mathit{q}}}_{n|n-1}$-centered chart, and encoded by the four

$$\left(\phi ,{\overline{\mathit{q}}}_{n|n-1},{\overline{\mathbf{x}}}_{n|n}^{{\overline{\mathit{q}}}_{n|n-1}},{\mathbf{P}}_{n|n}^{{\overline{\mathit{q}}}_{n|n-1}}\right).$$

(70)

Our best estimation for the orientation at this time is

$${\overline{\mathit{q}}}_{n|n}={\phi }_{{\overline{\mathit{q}}}_{n|n-1}}^{-1}\left({\overline{\mathbf{e}}}_{n|n}^{{\overline{\mathit{q}}}_{n|n-1}}\right)={\overline{\mathit{q}}}_{n|n-1}\ast {\phi }^{-1}\left({\overline{\mathbf{e}}}_{n|n}^{{\overline{\mathit{q}}}_{n|n-1}}\right),$$

(71)

being ${\overline{\mathbf{e}}}_{n|n}^{{\overline{\mathit{q}}}_{n|n-1}}$ the part of the mean ${\overline{\mathbf{x}}}_{n|n}^{{\overline{\mathit{q}}}_{n|n-1}}$ that represent the quaternion in the ${\overline{\mathit{q}}}_{n|n-1}$-centered chart.

Note that setting ${\overline{\mathit{q}}}_{n-1|n-2}:={\overline{\mathit{q}}}_{n-1|n-1}$ and ${\overline{\mathbf{e}}}_{n-1|n-1}^{{\overline{\mathit{q}}}_{n-1|n-2}}:=\mathbf{0}$ at 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 quaternion $\mathit{q}$ as a deviation from another quaternion $\overline{\mathit{q}}$ as $\mathit{q}=\overline{\mathit{q}}\ast \mathit{\delta }$. Now we define the instantaneous error between an estimated attitude, represented by a unit quaternion $\overline{\mathit{q}}$ and the real attitude, represented by the unit quaternion $\stackrel{⭑}{\mathit{q}}$ 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 ${\mathit{\delta }}_e$ such that $\stackrel{⭑}{\mathit{q}}=\overline{\mathit{q}}\ast {\mathit{\delta }}_e$. Recalling (6), this angle can be computed as:

$$\begin{array}{cc}\hfill {\theta }_e& =2arccos\left[{\left({\overline{\mathit{q}}}^{\ast }\ast \stackrel{⭑}{\mathit{q}}\right)}_0\right]=\hfill \end{array}$$

(72a)

$$\begin{array}{c}=2arccos\left(\overline{\mathit{q}}·\stackrel{⭑}{\mathit{q}}\right),\hfill \end{array}$$

(72b)

having previously ensured that $\overline{\mathit{q}}·\stackrel{⭑}{\mathit{q}}\ge 0$ using the fact that both $\mathit{q}$ and $-\mathit{q}$ represent the same rotation transformation.

Angle ${\theta }_e$ will vary along an orientation trajectory. Then, we will define the mean error in orientation estimation for a given trajectory starting at time $t=0$ and ending at time $t=T$ as

$$e_{\theta }=\frac{1}{T}{\int }_0^T{\theta }_e(t)dt.$$

(73)

Finally, $e_{\theta }$ 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 ${\overline{e}}_{\theta }$ and the variance ${\sigma }_{{\overline{e}}_{\theta }}^2$ that characterize the distribution of the error in orientation estimation $e_{\theta }$ for each algorithm. We will define the confidence interval for the computed ${\overline{e}}_{\theta }$ as

$$\left[{\overline{e}}_{\theta }-3{\sigma }_{{\overline{e}}_{\theta }}/\sqrt{N_s},{\overline{e}}_{\theta }+3{\sigma }_{{\overline{e}}_{\theta }}/\sqrt{N_s}\right],$$

(74)

where $N_s$ is the number of samples taken for the ${\overline{e}}_{\theta }$ computation, so that ${\sigma }_{{\overline{e}}_{\theta }}^2/N_s$ is the variance of the sample mean distribution.

Being that the lower the better, the value of ${\overline{e}}_{\theta }$ 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 ${\overline{e}}_{\theta }(\mathrm{A})<{\overline{e}}_{\theta }(\mathrm{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 ${\stackrel{⭑}{\mathit{q}}}_0$ from a uniform distribution in $S^3$ setting the initial angular velocity $\stackrel{⭑}{\mathsf{\omega }}{}_0^{\prime }$ to zero, setting the update frequency $f_{\mathrm{update}}$ generating the variances of the process noises ${\sigma }_{\mathsf{\omega }}^2$ and ${\sigma }_{\mathbf{v}}^2$ from a uniform distribution in the intervals $(0,Q_{\max }^{\mathsf{\omega }}]$ and $(0,Q_{\max }^{\mathbf{v}}]$ respectively, and initializing the estimation algorithm. The initialization of the MEKF includes setting ${\overline{\mathit{q}}}_{0|0}=1$ ${\overline{\mathsf{\omega }}}_{0|0}^{\prime }=\mathbf{0}\mathrm{rad}/\mathrm{s}$ and ${\mathbf{P}}_{0|0}^{{\overline{\mathit{q}}}_{0|0}}={10}^2\mathbf{I}$. On the other hand, the initialization of the MUKF includes setting ${\overline{\mathit{q}}}_{0|-1}=1$ ${\mathbf{e}}_{0|0}^{{\overline{\mathit{q}}}_{0|-1}}=\mathbf{0}$ ${\overline{\mathsf{\omega }}}_{0|0}^{\prime }={(1,1,1)}^T\mathrm{rad}/\mathrm{s}$ and ${\mathbf{P}}_{0|0}^{{\overline{\mathit{q}}}_{0|-1}}={10}^2\mathbf{I}$. The angular velocity is not initialized to $\mathbf{0}$ in 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 ${\mathbf{Q}}_n^{\mathsf{\omega }}=\mathbf{I}{\mathrm{rads}}^2/{\mathrm{s}}^4$ ${\mathbf{Q}}_n^{\mathbf{v}}={10}^{-2}\mathbf{I}\mathrm{p}.\mathrm{d}.\mathrm{u}.$ (“p.d.u.” stands for “Procedure Defined Unit”. In the present case it depends on the definition of the vector $\mathbf{v}$), ${\mathbf{R}}_n^{\mathsf{\omega }}=R^{\mathsf{\omega }}\mathbf{I}{\mathrm{rads}}^2/{\mathrm{s}}^2$, ${\mathbf{R}}_n^{\mathbf{v}}=R^{\mathbf{v}}\mathbf{I}\mathrm{p}.\mathrm{d}.\mathrm{u}.$, where $R^{\mathsf{\omega }}$ and $R^{\mathbf{v}}$ are 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 ${\mathbf{Q}}_n^{\mathsf{\omega }}$ and ${\mathbf{Q}}_n^{\mathbf{v}}$ have 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 by $1\mathrm{rad}/\mathrm{s}$ and also that it is normal to find external noises added to the vector ${\mathbf{v}}_t$ of magnitude ${10}^{-1}\mathrm{p}.\mathrm{d}.\mathrm{u}.$. For the mean values we set ${\overline{\mathbf{q}}}_n^{\mathsf{\omega }}=\mathbf{0}\mathrm{rads}/{\mathrm{s}}^2$ and ${\overline{\mathbf{q}}}^{\mathbf{v}}=\mathbf{0}\mathrm{p}.\mathrm{d}.\mathrm{u}.$.

In the convergence step we keep the system in the initial orientation ${\stackrel{⭑}{\mathit{q}}}_0$. Simulated measurements are generated using (23) and (24). For each measurement, a different ${\mathbf{v}}_t$ is sampled from a uniform distribution in the unit sphere of ${\mathbb{R}}^3$. The values for each component of ${\mathbf{q}}_t^{\mathbf{v}}$ ${\mathbf{r}}_t^{\mathbf{v}}$ and ${\mathbf{r}}_t^{\mathsf{\omega }}$ are obtained from normal distributions with zero mean and variances ${\sigma }_{\mathbf{v}}^2$ $R^{\mathbf{v}}$ and $R^{\mathsf{\omega }}$ respectively. The term ${\mathbf{R}}^T({\mathit{q}}_t)$ in (23) is obtained from the true attitude ${\stackrel{⭑}{\mathit{q}}}_t$, which in the convergence step takes the value of ${\stackrel{⭑}{\mathit{q}}}_t={\stackrel{⭑}{\mathit{q}}}_0$. The term ${\mathsf{\omega }}_t^{\prime }$ in (24) is the true angular velocity, which in the convergence step takes the value $\stackrel{⭑}{\mathsf{\omega }}{}_t^{\prime }=\mathbf{0}$. The tested algorithm updates its state estimation until the inequality ${\theta }_e(t)<{\theta }_e^0$ is satisfied, where ${\theta }_e(t)$ is the value of the error (72), and ${\theta }_e^0$ is a parameter in the simulation. The convergence step could have been replaced by an initialization of the attitude estimated by the algorithm ${\overline{\mathit{q}}}_t$ to the real value ${\stackrel{⭑}{\mathit{q}}}_t$ but 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:

$$\begin{array}{cc}\hfill \stackrel{⭑}{\mathsf{\omega }}{}_t^{\prime }& =\stackrel{⭑}{\mathsf{\omega }}{}_{t-\delta t}^{\prime }+{\mathbf{n}}_t\sqrt{\delta t},\hfill \end{array}$$

(75)

$$\begin{array}{cc}\hfill {\stackrel{⭑}{\mathit{q}}}_t& ={\stackrel{⭑}{\mathit{q}}}_{t-\delta t}\ast \left(\begin{array}{c}cos\left(\frac{\parallel \stackrel{⭑}{\mathsf{\omega }}{}_t^{\prime }\parallel \delta t}{2}\right)\\ \frac{\stackrel{⭑}{\mathsf{\omega }}{}_t^{\prime }}{\parallel \stackrel{⭑}{\mathsf{\omega }}{}_t^{\prime }\parallel }sin\left(\frac{\parallel \stackrel{⭑}{\mathsf{\omega }}{}_t^{\prime }\parallel \delta t}{2}\right)\end{array}\right),\hfill \end{array}$$

(76)

where ${\mathbf{n}}_t$ is a random vector whose components are sampled from a normal distribution with zero mean and variance ${\sigma }_{\mathsf{\omega }}^2$ and $\delta t$ is the simulation time step that is related to the algorithm time step Δt trough $\mathtt{dtdtsim}\delta t=\Delta t$ being $\mathtt{dtdtsim}$ an integer parameter that determines the simulation updates per algorithm update. Note that we multiply ${\mathbf{n}}_t$ by $\sqrt{\delta t}$ and not by $\delta t$. We do it this way so that the covariance matrix after k steps does not depend on the simulation time step $\delta t$. In fact, after a time $T=k\delta t$ the covariance matrix of the angular velocity will have grown by $\Delta {\mathbf{P}}^{\mathsf{\omega }}=k\mathbf{I}{\sigma }_{\mathsf{\omega }}^2\delta t=\mathbf{I}{\sigma }_{\mathsf{\omega }}^{2}T$ and not by ${(\Delta {\mathbf{P}}^{\mathsf{\omega }})}^{\prime }=k\mathbf{I}{\sigma }_{\mathsf{\omega }}^2{(\delta t)}^2=\mathbf{I}{\sigma }_{\mathsf{\omega }}^{2}T\delta t$. After each $\mathtt{dtdtsim}$ simulation 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 time $\mathtt{Tsim}=k^{\prime }\Delta t$ where $k^{\prime }$ 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 ${\overline{e}}_{\theta }$ and ${\sigma }_{{\overline{e}}_{\theta }}^2$. We will perform $N_s$ 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 $f_{\mathrm{update}}=1/\Delta t$ in the interval $[2,1000]\mathrm{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 around $1000\mathrm{Hz}$. 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 $\Delta t_n$ for each update, the simulations are performed using a constant Δt, and the simulation parameters depicted in

Table 5. Parameters used in the simulations.

Parameter	Value
${\theta }_e^0$	$1^{\circ }$
Tsim	$10\mathrm{s}$
dtdtsim	100
$N_s$	1000
$Q_{\max }^{\mathsf{\omega }}$	${10}^2{\mathrm{rads}}^2/{\mathrm{s}}^3$
$Q_{\max }^{\mathbf{v}}$	$1\mathrm{p}.\mathrm{d}.\mathrm{u}.$
R	$\{{10}^{-2},{10}^{-4},{10}^{-6}\}$
$R^{\mathsf{\omega }}$	$R{\mathrm{rads}}^2/{\mathrm{s}}^2$
$R^{\mathbf{v}}$	$R\mathrm{p}.\mathrm{d}.\mathrm{u}.$
$W_0$	$1/25$

.

The parameters ${\theta }_e^0$ Tsim, dtdtsim, and $N_s$ have been chosen trying to reach a compromise between the precision of the results, and the execution time of the simulation. The values for $Q_{\max }^{\mathsf{\omega }}$ and $Q_{\max }^{\mathbf{v}}$ have been chosen in such a way that the estimation algorithms face both normal situations (${\mathbf{Q}}_n^{\mathsf{\omega }}\approx {\sigma }_{\mathsf{\omega }}^2\mathbf{I}$ and ${\mathbf{Q}}_n^{\mathbf{v}}\approx {\sigma }_{\mathbf{v}}^2\mathbf{I}$) and situations that were not foreseen (${\mathbf{Q}}_n^{\mathsf{\omega }}\ne {\sigma }_{\mathsf{\omega }}^2\mathbf{I}$ or ${\mathbf{Q}}_n^{\mathbf{v}}\ne {\sigma }_{\mathbf{v}}^2\mathbf{I}$). A typical low cost IMU has $R^{\mathsf{\omega }}\approx {10}^{-4}{\mathrm{rad}}^2/{\mathrm{s}}^2$ and $R^{\mathbf{v}}\approx {10}^{-4}{g}^2$. The values chosen for R represent an imprecise sensor (${10}^{-2}$), a normal sensor (${10}^{-4}$), and a precise sensor (${10}^{-6}$). The value of $W_0$ has 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 $W_0$.

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 maps $\mathit{q}$ and $-\mathit{q}$ with the same point of ${\mathbb{R}}^3$ and with the same $\mathbf{T}$-matrix; or of the image since it is all ${\mathbb{R}}^3$. In addition, the expressions of ${\phi }^{-1}$ and the $\mathbf{T}$-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 the $\mathbf{T}$-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 $S^3$, the unit sphere in ${\mathbb{R}}^4$, 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:

• 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.
• The MEKF is preferable to the MUKF due to its lower computational cost and its greater accuracy in orientation estimation.
• 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.