# Optimal, Recursive and Suboptimal Linear Solutions to Attitude Determination from Vector Observations

Zebo Zhou, Jin Wu, Jinling Wang, Hassen Fourati

<figure><img src="image_1.png" /></figure>

To cite this version:

Zebo Zhou, Jin Wu, Jinling Wang, Hassen Fourati. Optimal, Recursive and Suboptimal Linear Solutions to Attitude Determination from Vector Observations. 2017. hal-01525603v4

HAL Id: hal-01525603

https://inria.hal.science/hal-01525603v4

Preprint submitted on 11 Dec 2017

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

<figure><img src="image_2.png" /></figure>

Distributed under a Creative Commons Attribution 4.0 International License
Article

# Optimal, Recursive and Sub-optimal Linear Solutions to Attitude Determination from Vector Observations for GNSS/Accelerometer/Magnetometer Orientation Measurement

Zebo Zhou<sup>1,†</sup> ◆, Jin Wu<sup>1,2,†</sup>, Jinling Wang<sup>3</sup>, Hassen Fourati<sup>4</sup>

Academic Editor: name

Version December 11, 2017 submitted to Remote Sens.

**Abstract:** Integration of accelerometer and magnetometer (AM) provides continuous, stable and accurate attitude information for land-vehicle navigation campaigns. However, magnetic disturbance strongly degrades the overall system performance. As an important complementary, global navigation satellite system (GNSS) indirectly produces the orientation information thus can potentially benefit the AM system. Such GNSS/AM system for attitude estimation is mathematically converted to multi-observation vector pairs matching problem in this paper. The optimal, sub-optimal attitude determination and its time-varying recursive variants are all comprehensively investigated and discussed. The developed methods are named as the Optimal Linear Estimator of Quaternion (OLEQ), Suboptimal-OLEQ (SOLEQ) and Recursive-OLEQ (ROLEQ) for different application scenarios. The theory is established based on our previous contributions and the multi-vector matrix multiplications are decomposed with the eigenvalue factorization. Some analytical results are proved and given which provides the audience with a brand new viewpoint of the attitude determination and its evolution inside. With the derivations of two-vector case, the *n*-vector cased is then naturally formed. Simulations are carried out showing the advantages of accuracy, robustness and time consumption of the proposed OLEQs, compared with representative methods. The algorithms are then implemented using C++ programming language on the designed hardware with GNSS module, 3-axis accelerometer, 3-axis magnetometer, giving the effectiveness validation of them in real-world applications.

**Keywords:** Attitude Determination, GNSS Receiver, Wahba's Problem, Vector Observations, Autonomous Navigation

## 0. Introduction

Attitude determination (or estimation) from vector observation pairs is a significant technology in aerospace engineering and related geodetic applications [1–3]. For instance, the inertial navigation, as

<footnote n="1">¹ School of Aeronautics and Astronautics, University of Electronic Science and Technology of China, Chengdu, China; klinsmann.zhou@gmail.com</footnote>

<footnote n="2">² School of Automation, University of Electronic Science and Technology of China, Chengdu, China; jin_wu_uestc@hotmail.com</footnote>

<footnote n="3">³ University of New South Wales, New South Wales, Australia; Jinling.Wang@unsw.edu.au</footnote>

<footnote n="4">⁴ University Grenoble Alpes, CNRS, GIPSA-Lab, Grenoble 38400, France; hassen.fourati@gipsa-lab.grenoble-inp.fr</footnote>

<footnote>* Correspondence: jin_wu_uestc@hotmail.com; klinsmann.zhou@gmail.com, Tel.: +86-18615745916</footnote>

<footnote>† Zebo Zhou and Jin Wu contributed equally on the theory and experiment of this paper.</footnote>

<footnote>‡ The MATLAB source codes of the proposed algorithms are uploaded as open-source files on https://github.com/zarathustr/OLEQs.</footnote>
an important role in modern military applications, has a high demand on attitude determination accuracy for initial alignment [4–7]. A typical attitude measuring system integrating 3-axis accelerometer with 3-axis magnetometer (AM) is extensively applied for real-time, continuous, stable and accurate attitude estimation for various navigation campaigns [8,9]. For most of applications, AM sensors are very efficient for low-cost attitude determination. However, the magnetometer is easily to be tolerated by unknown and unexpected magnetic fields disturbances from electromagnetic signals contaminated environments. On the other hand, for the large-scale region application, the reference magnetic vector is no longer a constant vector and needs to be timely corrected by other additional heading information. Otherwise, the overall system performance will be heavily degraded. Moreover, the accelerometers inevitably suffer from their biases thus leading to inaccurate attitude estimation. Therefore, auxiliary sensors are necessary to overcome such problem.

Alternatively, the global navigation satellite system (GNSS) provides precise position and velocity information. It has been successfully used for land, marine and aircraft attitude determination applications [10]. Traditional methods integrate GNSS with inertial sensors and simultaneously estimate the orientation with a synchronized position, velocity and attitude loops [11–13]. However, this leads to a risk to contaminate the attitude solution associated with position and velocity estimation loops. Thus Gebre-Egziabher and Elkaim [14] proposes an independent attitude estimation loop by means of vector matching. Compared with GNSS antenna arrays which compute highly precise solutions of baselines by using carrier-phase measurement, single GNSS antenna is more preferred in many low-cost and low-power consumption platforms. It mainly uses the simultaneous velocity vector information generated by GNSS Doppler [15,16]. Indirectly, this produces an important complementary orientation information thus potentially benefits the AM sensors system, especially for land vehicles. In addition, integrating high-rate sensors also contributes to seismogeodetic systems [17,18].

Efficient attitude estimation strategy is very crucial for GNSS/AM integrated multi-sensor system. In essence, it can be mathematically converted to multi-observation vector pairs matching problem. Representative methods are mainly about the solutions to the famous Wahba's problem [19], which aims to obtain the optimal attitude determination results using weighted least squares. Among these algorithms, the Shuster's QUaternion ESTimator (QUEST, [20]), Markley's Singular Value Decomposition (SVD, [21]) and Mortari's (ESOQ, [22]) are the most frequently used ones, which are mostly inspired by Davenport's q-method [23,24]. Our newly developed Fast Linear Attitude Estimator (FLAE, [25]) obtains a fastest Wahba's solution to our existing knowledge. Some other interesting approaches are proposed as well, investigating the other internals of this problem e.g. Yang's analytical method, Riemannian-manifold algorithm and Forbes' Linear-Matrix-Inequality (LMI) solution. [26–28].

There are still some weight-less algorithms for multi-sensor attitude determination. They are usually used on applications like vision attitude determination where the a priori information of weights can hardly be accurately determined. For example, using the nonlinear special orthogonal group on SO3 [29], it is able to obtain the attitude quaternion from arbitrary pairs of vectors. Via optimization approaches like gradient-decent algorithm (GDA, [30]), Gauss-Newton algorithm (GNA, [9]), we may also compute stable solutions. Apart from these, a famous analytical method was proposed by Arun et al. where the Singular Value Decomposition (SVD) is employed to calculate the attitude matrix [31]. Invoking similar commitment, it is then introduced in computing both the attitude and translation vector in machine vision systems [32].

For Wahba's problem, it has been shown that most existing algorithms are based on the Davenport's q-method. For a long period, the attitude solving process is fixed on this framework which aims to find the largest eigenvalue of the Davenport matrix K. Is it possible to seek another quite different attitude determination approach? The answer is positive and in this paper, three novel quaternion attitude determination algorithms from pairs of vector measurements are proposed in the sense of optimal, time-recursive and sub-optimal formulations. The main contributions are listed below:
1. The main theory is based on our previous research contributions and is extended to arbitrary pairs vector measurements linearly for GNSS/AM attitude application.

2. Three estimators i.e. the Optimal Linear Estimator of Quaternion (OLEQ), Recursive OLEQ (ROLEQ) and Sub-optimal OLEQ (SOLEQ) are derived. We also proposed accelerating techniques to make the algorithms faster.

3. Simulations and real experiments are carried out, which verify the accuracy, flexibility, robustness and time consumption of various algorithms for GNSS/AM attitude determination. Detailed comparisons with representative methods are shown to draw the superiority of the proposed OLEQs.

This paper is structured as follows: Section I briefs the GNSS-AM sensor system along with its functional and stochastic models formulated in the way of vector pair matching. Section II contains the problem formulation and starts with the quaternion estimation from a single sensor observation. Section III The two-vector attitude determination theory along with the n-vector one are given accordingly. Section IV involves the numerical examples and real field test where comparisons between the proposed SOLEQ and other representative methods are given. Finally, Section V consists of concluding remarks and future work.

## 1. Fundamentals of GNSS/AM system

For attitude determination, we require GNSS receivers and AM sensors arrays for low-cost and accurate solutions. First, considering the motion behaviours of land vehicles, a vector pair for GNSS velocity can be established as

$$ \mathbf{v}_G^b = \mathbf{C}_r^b \mathbf{v}_G^r + \varepsilon_G^b \quad (1) $$

where v denotes velocity vector; the subscript G denotes the observation source 'GNSS'; the symbol r and b represent the navigation frame (r-frame, North-East-Down, NED) and body frame (b-frame, Forward-Right-Down) respectively; transforms the vector from r-frame to b-frame; $\varepsilon_G^b$ is the Gaussian white noise with variance of $\mathbf{R}_G$. For land-vehicle application, it should be pointed out that the velocity in r-frame and b-frame are respectively

$$ \mathbf{v}^r = \mathbf{C}_e^r \mathbf{v}^e, \mathbf{v}_G^e = \begin{bmatrix} \sqrt{(v_x^e)^2 + (v_y^e)^2 + (v_z^e)^2} \\ 0 \\ 0 \end{bmatrix} \quad (2) $$

where e denotes an Earth-Centred-Earth-Fixed (ECEF) coordinate frame (e-frame), i.e. WGS-84; denotes the transformation (from e-frame to n-frame) and can be computed according to the GNSS position and Earth ellipsoid metrics in advance.

AM sensors consist of 3-axis MEMS accelerometer and 3-axis magnetometer. The accelerometer gives the specific force measurement of a rigid body and magnetometer provides the users with sensed local Earth geomagnetic field according to IGRF model.

$$ \begin{aligned} \mathbf{z}_A &= \mathbf{C}_r^b (\mu^r - \mathbf{G}^r) + \mathbf{b}_A + \varepsilon_A, & \varepsilon_A \sim (\mathbf{0}, \mathbf{R}_A) \\ \mathbf{z}_M &= \mathbf{C}_r^b \mathbf{M}^r + \varepsilon_M, & \varepsilon_M \sim (\mathbf{0}, \mathbf{R}_M) \end{aligned} \quad (3) $$

where the subscripts A and M denote the accelerometer and magnetometer sources respectively; z denotes observed vector; b denotes the accelerometer bias; $\mathbf{G}^r = [0, 0, -g]^T$ where g the gravity, is a function of geo-location; The normalized Earth's magnetic field reference vector $\mathbf{M}^r = [\cos\alpha, 0, -\sin\alpha]^T$ where $\alpha$ is the local dip angle; $\mu$ is the linear acceleration vector which is usually treated as an external disturbance; $\varepsilon$ is the Gaussian white noise with the variance of $\mathbf{R}$. For simplicity, two points need to be clarified that: (1) the bias term has been obtained and compensated for the accelerometer readings. (2) The linear acceleration $\mu$ is estimated by using GNSS velocity information with differentiation between
two adjacent epochs. Then the the vectors and matrices for system model in b-frame and r-frame can be merged into following multi-vector pair equations:

$$ \mathbf{D}_i^b = \mathbf{C}_r^b \mathbf{D}_i^r + \varepsilon_{i}, i = 1,2,3 \qquad (4) $$

where

$$ \begin{pmatrix} \mathbf{D}_1^b \\ \mathbf{D}_2^b \\ \mathbf{D}_3^b \end{pmatrix} = \begin{pmatrix} \frac{\mathbf{v}_G^b}{\|\mathbf{v}_G^b\|} \\ \frac{\mathbf{z}_A}{\|\mathbf{z}_A\|} \\ \frac{\mathbf{z}_M}{\|\mathbf{z}_M\|} \end{pmatrix}, \begin{pmatrix} \mathbf{D}_1^r \\ \mathbf{D}_2^r \\ \mathbf{D}_3^r \end{pmatrix} = \begin{pmatrix} \frac{\mathbf{C}_e^r \mathbf{v}^e}{\|\mathbf{C}_e^r \mathbf{v}^e\|} \\ \frac{\mu^r - \mathbf{G}^r}{\|\mu^r - \mathbf{G}^r\|} \\ \frac{\mathbf{M}^r}{\|\mathbf{M}^r\|} \end{pmatrix}, \begin{pmatrix} \varepsilon_1 \\ \varepsilon_2 \\ \varepsilon_3 \end{pmatrix} = \begin{pmatrix} \varepsilon_G \\ \varepsilon_A \\ \varepsilon_M \end{pmatrix} \qquad (5) $$

Neglecting the cross-correlations between sensors, the stochastic model of system is

$$ \begin{pmatrix} \varepsilon_G \\ \varepsilon_A \\ \varepsilon_M \end{pmatrix} \sim (0, \mathbf{R}), \mathbf{R} = \begin{pmatrix} \mathbf{R}_G & & \\ & \mathbf{R}_A & \\ & & \mathbf{R}_M \end{pmatrix} \qquad (6) $$

## 2. Problem Formulation

The conventional Wahba's problem aims to find the optimal attitude matrix from vector observation pairs in the sense of least squares, such that

$$ L(\mathbf{C}) = \sum_{i=1}^{n} a_i \| \mathbf{D}_i^b - \mathbf{C} \mathbf{D}_i^r \|^2, n = 2,3,\dots \qquad (7) $$

in which $\mathbf{C}$ denotes the optimal direction cosine matrix (DCM); $\mathbf{D}_i^b = (D_{x,i}^b, D_{y,i}^b, D_{z,i}^b)^\top$ and $\mathbf{D}_i^r = (D_{x,i}^r, D_{y,i}^r, D_{z,i}^r)^\top$ are the i-th pair of normalized vector observations from the body frame b and the reference frame r respectively; $a_i$ is the weight of the i-th sensor output, which is given by the standard deviations of the input vectors from $\sigma_1$ to $\sigma_n$:

$$ a_i = \frac{\sigma_{tot}^2}{\sigma_i^2}, \sigma_{tot}^2 = \frac{1}{\sum_{i=1}^{n} \frac{1}{\sigma_i^2}} \qquad (8) $$

provided that the variance information such as shown in (6) is predetermined. Wahba's problem is solved via many representative methods [33]. Many of these algorithms solve the problem via eigenvalue decompositions analytically or numerically [26,27]. When there are only one pair of vector observations, the Wahba's solutions fail to obtain the optimal quaternion since there will be ambiguous quaternions corresponding to the maximum eigenvalue 1 [34,35]. In our previous contribution [36], the continuous stable quaternion solution to an accelerometer-based attitude determination system is derived.

### 2.1. Quaternion from A Single Sensor Observation

Considering an attitude determination model from a pair of vector observations

$$ \mathbf{D}^b = \mathbf{C} \mathbf{D}^r \qquad (9) $$

, this section deals with the attitude determination from a single pair of sensor observations. Note that the DCM is decomposed with quaternions $\mathbf{q} = (q_0, q_1, q_2, q_3)^\top$ [36] via:

$$ \mathbf{C} = (\mathbf{P}_1 \mathbf{q}, \mathbf{P}_2 \mathbf{q}, \mathbf{P}_3 \mathbf{q}) \qquad (10) $$
in which

$$ \mathbf{P}_1 = \begin{pmatrix} q_0 & q_1 & -q_2 & -q_3 \\ -q_3 & q_2 & q_1 & -q_0 \\ q_2 & q_3 & q_0 & q_1 \end{pmatrix}, \mathbf{P}_2 = \begin{pmatrix} q_3 & q_2 & q_1 & q_0 \\ q_0 & -q_1 & q_2 & -q_3 \\ -q_1 & -q_0 & q_3 & q_2 \end{pmatrix}, \mathbf{P}_3 = \begin{pmatrix} -q_2 & q_3 & -q_0 & q_1 \\ q_1 & q_0 & q_3 & q_2 \\ q_0 & -q_1 & -q_2 & q_3 \end{pmatrix} \quad (11) $$

In this section, the theory is extended to arbitrary sensor with exactly the similar approach in [36]. Inserting (10) into (9) gives

$$ \mathbf{D}^b = (D_x^r \mathbf{P}_1 + D_y^r \mathbf{P}_2 + D_z^r \mathbf{P}_3)\mathbf{q} \quad (12) $$

It has been proved in [36] that

$$ \mathbf{P}_1^\top = \mathbf{P}_1^\dagger, \mathbf{P}_2^\top = \mathbf{P}_2^\dagger, \mathbf{P}_3^\top = \mathbf{P}_3^\dagger \quad (13) $$

100 where † stands for the Moore-Penrose pseudo-inverse. In fact, another property can be shown in the
101 following theorem

102 Theorem 1. $\mathbf{P}_1\mathbf{P}_2^\top + \mathbf{P}_2\mathbf{P}_1^\top = \mathbf{P}_1\mathbf{P}_3^\top + \mathbf{P}_3\mathbf{P}_1^\top = \mathbf{P}_2\mathbf{P}_3^\top + \mathbf{P}_3\mathbf{P}_2^\top = \mathbf{0}_{3 \times 3}$.

Proof. This can be proved via brute-force calculation:

$$ \mathbf{P}_1 \mathbf{P}_2^\top = \begin{pmatrix} 0 & -q_0^1 + q_1^2 + q_2^2 - q_3^2 & 2q_0 q_1 + 2q_2 q_3 \\ q_0^1 - q_1^2 - q_2^2 + q_3^2 & 0 & 2q_0 q_2 - 2q_1 q_3 \\ -2q_0 q_1 - 2q_2 q_3 & -2q_0 q_2 + 2q_1 q_3 & 0 \end{pmatrix} \quad (14a) $$

$$ \mathbf{P}_2 \mathbf{P}_1^\top = \begin{pmatrix} 0 & q_0^1 - q_1^2 - q_2^2 + q_3^2 & -2q_0 q_1 - 2q_2 q_3 \\ -q_0^1 + q_1^2 + q_2^2 - q_3^2 & 0 & -2q_0 q_2 + 2q_1 q_3 \\ 2q_0 q_1 + 2q_2 q_3 & 2q_0 q_2 - 2q_1 q_3 & 0 \end{pmatrix} \quad (14b) $$

$$ \mathbf{P}_1 \mathbf{P}_3^\top = \begin{pmatrix} 0 & 2q_0 q_1 - 2q_2 q_3 & q_0^1 - q_1^2 + q_2^2 - q_3^2 \\ -2q_0 q_1 + 2q_2 q_3 & 0 & -2q_1 q_2 - 2q_0 q_3 \\ -q_0^1 + q_1^2 - q_2^2 + q_3^2 & 2q_1 q_2 + 2q_0 q_3 & 0 \end{pmatrix} \quad (14b) $$

$$ \mathbf{P}_3 \mathbf{P}_1^\top = \begin{pmatrix} 0 & -2q_0 q_1 + 2q_2 q_3 & -q_0^1 + q_1^2 - q_2^2 + q_3^2 \\ 2q_0 q_1 - 2q_2 q_3 & 0 & 2q_1 q_2 + 2q_0 q_3 \\ q_0^1 - q_1^2 + q_2^2 - q_3^2 & -2q_1 q_2 - 2q_0 q_3 & 0 \end{pmatrix} \quad (14c) $$

$$ \mathbf{P}_2 \mathbf{P}_3^\top = \begin{pmatrix} 0 & 2q_0 q_2 + 2q_1 q_3 & -2q_1 q_2 + 2q_0 q_3 \\ -2q_0 q_2 - 2q_1 q_3 & 0 & q_0^1 + q_1^2 - q_2^2 - q_3^2 \\ 2q_1 q_2 - 2q_0 q_3 & -q_0^1 - q_1^2 + q_2^2 + q_3^2 & 0 \end{pmatrix} \quad (14c) $$

$$ \mathbf{P}_3 \mathbf{P}_2^\top = \begin{pmatrix} 0 & -2q_0 q_2 - 2q_1 q_3 & 2q_1 q_2 - 2q_0 q_3 \\ 2q_0 q_2 + 2q_1 q_3 & 0 & -q_0^1 - q_1^2 + q_2^2 + q_3^2 \\ -2q_1 q_2 + 2q_0 q_3 & q_0^1 + q_1^2 - q_2^2 - q_3^2 & 0 \end{pmatrix} \quad (14c) $$

104 Lemma 1. Let $K(\mathbf{q}) = (D_x^r \mathbf{P}_1 + D_y^r \mathbf{P}_2 + D_z^r \mathbf{P}_3)$, $K^\top(\mathbf{q}) = K^\dagger(\mathbf{q})$.

<figure><img src="image_3.png" /></figure>
Proof. We have

$$
\begin{align}
& \mathbf{K}(\mathbf{q})\mathbf{K}^{\top}(\mathbf{q}) \nonumber \\
&= (D_x^r \mathbf{P}_1 + D_y^r \mathbf{P}_2 + D_z^r \mathbf{P}_3)(D_x^r \mathbf{P}_1 + D_y^r \mathbf{P}_2 + D_z^r \mathbf{P}_3)^{\top} \nonumber \\
&= (D_x^r)^2 \mathbf{P}_1 \mathbf{P}_1^{\top} + (D_y^r)^2 \mathbf{P}_2 \mathbf{P}_2^{\top} + (D_z^r)^2 \mathbf{P}_3 \mathbf{P}_3^{\top} \nonumber \\
&\quad + D_x^r D_y^r (\mathbf{P}_1 \mathbf{P}_2^{\top} + \mathbf{P}_2 \mathbf{P}_1^{\top}) + D_x^r D_z^r (\mathbf{P}_1 \mathbf{P}_3^{\top} + \mathbf{P}_3 \mathbf{P}_1^{\top}) + D_y^r D_z^r (\mathbf{P}_2 \mathbf{P}_3^{\top} + \mathbf{P}_3 \mathbf{P}_2^{\top}) \tag{15} \\
&= \left[ (D_x^r)^2 + (D_y^r)^2 + (D_z^r)^2 \right] \mathbf{I}_{3 \times 3} \nonumber \\
&= \mathbf{I}_{3 \times 3} \nonumber
\end{align}
$$

105 which gives Lemma 1.

Hence (12) can be transformed into

$$
\mathbf{D}^b = \mathbf{K}(\mathbf{q})\mathbf{q} \Rightarrow \mathbf{K}^\dagger(\mathbf{q})\mathbf{D}^b = \mathbf{q} \Rightarrow \mathbf{K}^\top(\mathbf{q})\mathbf{D}^b = \mathbf{q} \quad (16)
$$

Since $\mathbf{K}(\mathbf{q})$ is in the form of quaternion, $\mathbf{K}^{\top}(\mathbf{q})\mathbf{D}^b$ can be expanded by

$$
\mathbf{K}^{\top}(\mathbf{q})\mathbf{D}^b = D_x^r \mathbf{P}_1^{\top}\mathbf{D}^b + D_y^r \mathbf{P}_2^{\top}\mathbf{D}^b + D_z^r \mathbf{P}_3^{\top}\mathbf{D}^b \quad (17)
$$

Also it can be obtained that

$$
\mathbf{P}_1^\top \mathbf{D}^b = \begin{pmatrix} D_x^b q_0 + D_z^b q_2 - D_y^b q_3 \\ D_x^b q_1 + D_y^b q_2 + D_z^b q_3 \\ D_z^b q_0 + D_y^b q_1 - D_x^b q_2 \\ -D_y^b q_0 + D_z^b q_1 - D_x^b q_3 \end{pmatrix} = \begin{pmatrix} D_x^b & 0 & D_z^b & -D_y^b \\ 0 & D_x^b & D_y^b & D_z^b \\ D_z^b & D_y^b & -D_x^b & 0 \\ -D_y^b & D_z^b & 0 & -D_x^b \end{pmatrix} \mathbf{q} = \mathbf{M}_1 \mathbf{q} \quad (18a)
$$

$$
\mathbf{P}_2^\top \mathbf{D}^b = \begin{pmatrix} D_y^b q_0 - D_z^b q_1 + D_x^b q_3 \\ -D_z^b q_0 - D_y^b q_1 + D_x^b q_2 \\ D_x^b q_1 + D_y^b q_2 + D_z^b q_3 \\ D_x^b q_0 + D_z^b q_2 - D_y^b q_3 \end{pmatrix} = \begin{pmatrix} D_y^b & -D_z^b & 0 & D_x^b \\ -D_z^b & -D_y^b & D_x^b & 0 \\ 0 & D_x^b & D_y^b & D_z^b \\ D_x^b & 0 & D_z^b & -D_y^b \end{pmatrix} \mathbf{q} = \mathbf{M}_2 \mathbf{q} \quad (18\text{b})
$$

$$
\mathbf{P}_3^\top \mathbf{D}^b = \begin{pmatrix} D_z^b q_0 + D_y^b q_1 - D_x^b q_2 \\ D_y^b q_0 - D_z^b q_1 + D_x^b q_3 \\ -D_x^b q_0 - D_z^b q_2 + D_y^b q_3 \\ D_x^b q_1 + D_y^b q_2 + D_z^b q_3 \end{pmatrix} = \begin{pmatrix} D_z^b & D_y^b & -D_x^b & 0 \\ D_y^b & -D_z^b & 0 & D_x^b \\ -D_x^b & 0 & -D_z^b & D_y^b \\ 0 & D_x^b & D_y^b & D_z^b \end{pmatrix} \mathbf{q} = \mathbf{M}_3 \mathbf{q} \quad (18c)
$$

Then we have

$$
\mathbf{K}^{\top}(\mathbf{q})\mathbf{D}^{b} = D_{x}^{r}\mathbf{M}_{1}\mathbf{q} + D_{y}^{r}\mathbf{M}_{2}\mathbf{q} + D_{z}^{r}\mathbf{M}_{3}\mathbf{q} = \mathbf{W}\mathbf{q} \quad (19)
$$

where $\mathbf{W}$ is given by

$$
\mathbf{W} = D_x^r \mathbf{M}_1 + D_y^r \mathbf{M}_2 + D_z^r \mathbf{M}_3 \quad (20)
$$

Therefore the attitude determination problem is shifted to

$$
\mathbf{W}\mathbf{q} = \mathbf{q} \tag{21}
$$

106 Theorem 2. For any normalized vector observation pair {$\mathbf{D}^r, \mathbf{D}^b$}, $\mathbf{W}^2 = \mathbf{I}$, where $\mathbf{I}$ is the four-order identity matrix.

107

Proof. The characteristic polynomial of $\mathbf{W}$ is given by

$$
p(\lambda) = (\lambda - 1)^2 (\lambda + 1)^2 \qquad (22)
$$
Then with Cayley-Hamilton Theorem, we substitute $\mathbf{W}$ for its eigenvalues and obtain

$$ (\mathbf{W}^2 - \mathbf{I})^2 = \mathbf{0} \qquad (23) $$

108 as $\mathbf{W}^2 - \mathbf{I}$ is real symmetric, it should be $\mathbf{0}$, which finishes the proof.

In [36], we showed that (21) can be seen as an iterative dynamical system

$$ \mathbf{q}(n) = \mathbf{W}\mathbf{q}(n-1) \qquad (24) $$

where $\mathbf{q}(n)$ denotes the quaternion for the $n$th iteration. Also, as has been proved, the discrete system can be converted to a corresponding continuous system. The stable solution to the continuous system is calculated as

$$ \mathbf{q} = \frac{\mathbf{W} + \mathbf{I}}{2} \mathbf{q}_{\text{rand}} \qquad (25) $$

109 if $\mathbf{W}^2 = \mathbf{I}$. Where $\mathbf{q}_{\text{rand}}$ denotes a randomly-chosen unit quaternion. This provides us with a new approach to obtaining the measurement quaternion from a single strapdown sensor.

111 **3. Optimal Linear Estimator of Quaternion**

With (9), it is able to list all the single rotation equations together as follows:

$$ \left\{ \begin{array}{l} \mathbf{D}_1^b = \mathbf{C}\mathbf{D}_1^r \\ \mathbf{D}_2^b = \mathbf{C}\mathbf{D}_2^r \\ \vdots \\ \mathbf{D}_n^b = \mathbf{C}\mathbf{D}_n^r \end{array} \right. \Rightarrow \left\{ \begin{array}{l} \sqrt{a_1}\mathbf{D}_1^b = \sqrt{a_1}\mathbf{C}\mathbf{D}_1^r \\ \sqrt{a_2}\mathbf{D}_2^b = \sqrt{a_2}\mathbf{C}\mathbf{D}_2^r \\ \vdots \\ \sqrt{a_n}\mathbf{D}_n^b = \sqrt{a_n}\mathbf{C}\mathbf{D}_n^r \end{array} \right. \qquad (26) $$

which can in turn be converted to

$$ \left\{ \begin{array}{l} \sqrt{a_1} \mathbf{q} = \sqrt{a_1} \mathbf{W}_1 \mathbf{q} \\ \sqrt{a_2} \mathbf{q} = \sqrt{a_2} \mathbf{W}_2 \mathbf{q} \\ \vdots \\ \sqrt{a_n} \mathbf{q} = \sqrt{a_n} \mathbf{W}_n \mathbf{q} \end{array} \right. \qquad (27) $$

This system is rewritten as

$$ \begin{pmatrix} \sqrt{a_1} \mathbf{I} \\ \sqrt{a_2} \mathbf{I} \\ \vdots \\ \sqrt{a_n} \mathbf{I} \end{pmatrix} \mathbf{q} = \begin{pmatrix} \sqrt{a_1} \mathbf{W}_1 \\ \sqrt{a_2} \mathbf{W}_2 \\ \vdots \\ \sqrt{a_n} \mathbf{W}_n \end{pmatrix} \mathbf{q} \qquad (28) $$

Notice that

$$ (\sqrt{a_1}\mathbf{I}, \sqrt{a_2}\mathbf{I}, \dots, \sqrt{a_n}\mathbf{I}) \begin{pmatrix} \sqrt{a_1}\mathbf{I} \\ \sqrt{a_2}\mathbf{I} \\ \vdots \\ \sqrt{a_n}\mathbf{I} \end{pmatrix} = \left(\sum_{i=1}^{n} a_i\right) \mathbf{I} = \mathbf{I} \qquad (29) $$

Hence the pre-multiplication leads to

$$ \mathbf{q} = \left( \sum_{i=1}^{n} a_i \mathbf{W}_i \right) \mathbf{q} \qquad (30) $$
It should be then noted that (28) is free of the existence of noises. In fact, the optimal attitude determination i.e. the Wahba's problem, has been proved as a total least-square problem in which both the reference and observation models are corrupted by the stochastic items. In this case, the maximum eigenvalue of $\sum_{i=1}^{n} a_i \mathbf{W}_i$ in engineering practice is very close to the noise-free theoretical result of 1. Corresponding with (24), based on the Brouwer's fixed-point theorem, it is able to iteratively obtain the normalized optimal quaternion by rotating a randomly given initial quaternion over and over again till infinity. This is something similar with the power method of the numerical eigenvalue factorization but can be accelerated with geometric-series-like iterations as follows

$$ 
\begin{aligned}
\mathcal{R} &= \left( \sum_{i=1}^{n} a_i \mathbf{W}_i \right) \\
\mathcal{R}^2 &= \mathcal{R} \cdot \mathcal{R} \\
\vdots \\
\mathcal{R}^{2j} &= \mathcal{R}^{2j-1} \cdot \mathcal{R}^{2j-1}
\end{aligned} 
\qquad (31) $$

where $\mathcal{R}$ denotes the rotation operator over the Hamilton space $\mathcal{H}$. In fact, the above iterations can hardly be achieved when the maximum eigenvalue approaches 1. The reason is that at this time the power $\mathcal{R}^2$ approaches $\mathbf{I}$ as well. A more robust way is shown to solve this problem. Considering the both sides of (28), we may find out that the right side is in fact the mixture of solutions to single vector observation pairs. As mentioned in (25), a stable, continuous solution to each single equation can be done by pre-multiplying $\frac{1}{2}(\mathbf{W}_i + \mathbf{I})$. Substituting $\frac{1}{2}(\mathbf{W}_i + \mathbf{I})$ for $\mathbf{W}_i$, i.e.

$$ \begin{pmatrix} \sqrt{a_1}\mathbf{I} \\ \sqrt{a_2}\mathbf{I} \\ \vdots \\ \sqrt{a_n}\mathbf{I} \end{pmatrix} \mathbf{q} = \begin{bmatrix} \frac{1}{2}\sqrt{a_1}(\mathbf{W}_1 + \mathbf{I}) \\ \frac{1}{2}\sqrt{a_2}(\mathbf{W}_2 + \mathbf{I}) \\ \vdots \\ \frac{1}{2}\sqrt{a_n}(\mathbf{W}_n + \mathbf{I}) \end{bmatrix} \mathbf{q} \qquad (32) $$

, the quaternion evolves from the mixture of single optimal solution. Here, the rotation operator is revised as

$$ \mathcal{R} = \sum_{i=1}^{n} \frac{1}{2} a_i (\mathbf{W}_i + \mathbf{I}) = \frac{1}{2} \left( \mathbf{I} + \sum_{i=1}^{n} a_i \mathbf{W}_i \right) \qquad (33) $$

This equals to the least-square of the set of pre-computed single rotated quaternion, which is definitely faster and more robust than rotation from a randomly given initial quaternion.

The covariance of the calculated optimal quaternion is equal to that of QUEST under the condition of least-square optimality:

$$ 
\begin{aligned}
\mathbf{\Sigma}_{\mathbf{Q},\text{OLEQ}} &= \frac{1}{4} \sigma_{tot}^2 \left( \mathbf{I} - \sum_{i=1}^{n} \mathbf{D}^b \{ \mathbf{D}^b \}^{\top} \right) \\
\mathbf{\Sigma}_{\mathbf{q},\text{OLEQ}} &= [\mathbf{q}_{opt}] \begin{pmatrix} 0 & \mathbf{0}_{1 \times 3} \\ \mathbf{0}_{3 \times 1} & \mathbf{\Sigma}_{\mathbf{Q},\text{OLEQ}} \end{pmatrix} [\mathbf{q}_{opt}]^{\top}
\end{aligned} 
\qquad (34) $$
where $Q = (q_1, q_2, q_3)^T$ is the vector part of the quaternion. $[q]$ defines the following matrix

$$[q] = \begin{pmatrix} q_3 & -q_2 & q_1 & q_0 \\ q_2 & q_3 & -q_0 & q_1 \\ -q_1 & q_0 & q_3 & q_2 \\ -q_0 & -q_1 & -q_2 & q_3 \end{pmatrix} \qquad (35)$$

### 112 3.1. Variant One: Recursive-OLEQ

We have seen from the above formulations that for each epoch, the vector observations are batchedly processed thoroughly. When used in aerospace electronic systems, the measured vector observation pairs in neighboring time epochs are usually continuous since they are always been smoothed by the sum filters and low-pass filters (LPF). Therefore, with this consideration, the attitude quaternion can be propagated from the last estimated one using the rotation operator described before. In this way the quaternions are recursively computed with much less computations and the accuracy is maintained. A more convenient clue is that for high reliable attitude determination systems, high-precision rate gyroscopes are employed usually. This provides us with a second-stage accelerating scheme, inspired by the conventional recursive algorithms like filter QUEST, REQUEST and etc. [37], that we may first rotate the estimated quaternion in last time epoch with zero-order angular transition matrix by

$$q_k = \Phi_{k,k-1} q_{k-1} \qquad (36)$$

where

$$\Phi_{k,k-1} = I + \frac{\Delta t}{2} [\Omega \times] \qquad (37)$$

in which $\Delta t$ is the sampling time and $[\Omega \times]$ composed by the angular rate from the gyroscope, which is detailed in many classical literatures. After this, even a single rotation by $\mathcal{R}$ would be very accurate then. The one-step covariance matrix of the obtained quaternion is calculated by

$$\Sigma_{q_k, \text{ROLEQ}} = \mathcal{R} \Phi_{k,k-1} \Sigma_{q_{k-1}, \text{ROLEQ}} \Phi_{k,k-1}^{\top} \mathcal{R}^{\top} \qquad (38)$$

### 113 3.2. Variant Two: SOLEQ

#### 114 3.2.1. Two-Vector Case

When there are two pairs of vector observations, regardless of the weights of respective sensors, it is natural that the attitude quaternion may be computed via

$$q = \frac{1}{4} (\mathbf{W}_1 + \mathbf{I}) (\mathbf{W}_2 + \mathbf{I}) q_{\text{rand}} \qquad (39)$$

where $\mathbf{W}_1$ and $\mathbf{W}_2$ are for the 1st and 2nd sensors. (25) is reasonable since we have

$$\left[ \frac{1}{2} (\mathbf{W} + \mathbf{I}) \right]^2 = \frac{1}{4} (\mathbf{W}^2 + 2\mathbf{W} + \mathbf{I}) = \frac{1}{4} (2\mathbf{W} + 2\mathbf{I}) = \frac{1}{2} (\mathbf{W} + \mathbf{I}) \qquad (40)$$

However, for the two-vector case, one can write

$$\left[ \frac{1}{4} (\mathbf{W}_1 + \mathbf{I}) (\mathbf{W}_2 + \mathbf{I}) \right]^2 = \left[ \frac{1}{4} (\mathbf{W}_1 \mathbf{W}_2 + \mathbf{W}_1 + \mathbf{W}_2 + \mathbf{I}) \right]^2 \neq \frac{1}{4} (\mathbf{W}_1 + \mathbf{I}) (\mathbf{W}_2 + \mathbf{I}) \qquad (41)$$
This reflects that the accurate quaternion may be recursively computed with

$$ \left\{ \begin{aligned} \mathbf{q}_k &= \frac{1}{4} (\mathbf{W}_1 + \mathbf{I}) (\mathbf{W}_2 + \mathbf{I}) \mathbf{q}_{k-1} \\ \mathbf{q}_k &= \frac{\mathbf{q}_k}{\|\mathbf{q}_k\|} \end{aligned} \right. \qquad (42) $$

which exits when the Euler distance $||\mathbf{q}_k - \mathbf{q}_{k-1}||$ is less than one predetermined threshold $\eta$. Note that the above initialization procedure is equivalent to the following process

$$ \left\{ \begin{aligned} \mathbf{q}^j &= \left[ \frac{1}{4} (\mathbf{W}_1 + \mathbf{I}) (\mathbf{W}_2 + \mathbf{I}) \right]^j \mathbf{q}_{rand} \\ \mathbf{q}^j &= \frac{\mathbf{q}^j}{\|\mathbf{q}^j\|} \end{aligned} \right. \qquad (43) $$

where $j$ is chosen to make sure $||\mathbf{q}^j - \mathbf{q}^{j-1}|| < \eta$. Let us define some notations i.e. transformation operators

$$ \mathcal{A} = \frac{1}{4} (\mathbf{W}_1 + \mathbf{I}) (\mathbf{W}_2 + \mathbf{I}) \qquad (44a) $$

$$ \mathcal{B} = \mathcal{A}^{\top} = \frac{1}{4} (\mathbf{W}_2 + \mathbf{I}) (\mathbf{W}_1 + \mathbf{I}) \qquad (44b) $$

**Theorem 3.** For the two-vector attitude determination case, the steady-state evolution in (43) is not affected by transformation operators' order, such that

$$ \left\{ \begin{aligned} \mathbf{q}^j &= \mathcal{A}^j \mathbf{q}_{rand} \\ \mathbf{q}^j &= \frac{\mathbf{q}^j}{\|\mathbf{q}^j\|} \Leftrightarrow \begin{cases} \mathbf{q}^j = \mathcal{B}^j \mathbf{q}_{rand} \\ \mathbf{q}^j = \frac{\mathbf{q}^j}{\|\mathbf{q}^j\|}, j \to +\infty \end{cases} \end{aligned} \right. \qquad (45) $$

*Proof.* The integrated transformation can be computed by

$$ \begin{aligned} \mathcal{A}\mathcal{B} &= \frac{1}{16} (\mathbf{W}_1 + \mathbf{I}) (\mathbf{W}_2 + \mathbf{I})^2 (\mathbf{W}_1 + \mathbf{I}) = \frac{1}{8} (\mathbf{W}_1 + \mathbf{I}) (\mathbf{W}_2 + \mathbf{I}) (\mathbf{W}_1 + \mathbf{I}) \\ &= \frac{1}{2} \mathcal{A} (\mathbf{W}_1 + \mathbf{I}) = \frac{1}{2} (\mathbf{W}_1 + \mathbf{I}) \mathcal{B} \end{aligned} \qquad (46) $$

and accordingly

$$ \mathcal{B}\mathcal{A} = \frac{1}{2}\mathcal{B}(\mathbf{W}_2 + \mathbf{I}) = \frac{1}{2}(\mathbf{W}_2 + \mathbf{I})\mathcal{A} \qquad (47) $$

These generate

$$ \mathcal{A}\mathcal{B}\mathcal{A} = \frac{1}{2}(\mathbf{W}_1 + \mathbf{I})\mathcal{B}\mathcal{A} = \frac{1}{4}(\mathbf{W}_1 + \mathbf{I})(\mathbf{W}_2 + \mathbf{I})\mathcal{A} = \mathcal{A}^2 \qquad (48) $$

and

$$ \mathcal{A}\mathcal{B}\mathcal{A}\mathcal{B} = \mathcal{A}^2\mathcal{B} = \mathcal{A}(\mathcal{A}\mathcal{B}) = \frac{1}{2}\mathcal{A}^2(\mathbf{W}_1 + \mathbf{I}) \qquad (49) $$

Finally, we have

$$ (\mathcal{A}\mathcal{B})^j = \frac{1}{2}\mathcal{A}^j(\mathbf{W}_1 + \mathbf{I}) = \frac{1}{2}(\mathbf{W}_1 + \mathbf{I})\mathcal{B}^j \qquad (50) $$

<footnote n="115">115 This proves that the mixed steady-state transformation $(\mathcal{A}\mathcal{B})^j$ can be achieved by independent transformations from $\mathcal{A}$ or $\mathcal{B}$, which finishes the proof. ■</footnote>
Following this theorem, the confronted problem is to compute the power $A^j$. In fact, $A$ is formed by $\frac{1}{2}(W_1 + I)$ and $\frac{1}{2}(W_2 + I)$. Their respective eigenvalue decomposition can be given by

$$ \left\{ \begin{array}{l} \frac{1}{2}(W_1 + I) = V_1 S_1 V_1^{-1} = V_1 S_1 V_1^{\top} \\ \frac{1}{2}(W_2 + I) = V_2 S_2 V_2^{-1} = V_2 S_2 V_2^{\top} \end{array} \right. \qquad (51) $$

where $V$ and $D$ are constituted by eigenvectors and eigenvalues respectively as $(W_i + I)$ is real symmetric [38]. Since $W_1$ and $W_2$ are in the same form, the eigenvalue matrices are equal to each other, i.e.

$$ S_1 = S_2 = S \qquad (52) $$

Then $A$ can be rewritten as

$$ A = V_1 S V_1^\top V_2 S V_2^\top \qquad (53) $$

Identically, we have

$$ B = V_2 S V_2^\top V_1 S V_1^\top \qquad (54) $$

Combining (53) and (54), it is obtained that

$$ AB = V_1 S V_1^\top V_2 S V_2^\top V_2 S V_2^\top V_1 S V_1^\top \qquad (55) $$

Note that

$$ V_1 V_1^\top = V_1^\top V_1 = V_2 V_2^\top = V_2^\top V_2 = I \qquad (56) $$

(55) is simplified as

$$ AB = V_1 S V_1^\top V_2 S S V_2^\top V_1 S V_1^\top \qquad (57) $$

Here we define

$$ U = S V_1^\top V_2 S^2 V_2^\top V_1 S \qquad (58) $$

Actually it is decomposed by

$$ U = HH^\top \qquad (59) $$

$$ H = S V_1^\top V_2 S \qquad (59) $$

An interesting fact is that the eigenvalue matrix $S$ can be analytically calculated and is given by

$$ S = \mathrm{diag}(0, 0, 1, 1) \qquad (60) $$

where diag(·) represents the diagonal matrix. This further yields $H$ to be a matrix with the form of

$$ H = \begin{pmatrix} 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & h_1 & h_2 \\ 0 & 0 & h_3 & h_4 \end{pmatrix} \qquad (61) $$

Then $U$ is computed by

$$ U = HH^\top = \begin{pmatrix} 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & u_1 & u_2 \\ 0 & 0 & u_3 & u_4 \end{pmatrix} = \begin{pmatrix} 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & h_1^2 + h_2^2 & h_1 h_3 + h_2 h_4 \\ 0 & 0 & h_1 h_3 + h_2 h_4 & h_3^2 + h_4^2 \end{pmatrix} \qquad (62) $$
where $u_2 = u_3$. Using this, we have

$$ (AB)^j = \mathbf{V}_1 \mathbf{U} \mathbf{V}_1^\top \mathbf{V}_1 \mathbf{U} \mathbf{V}_1^\top \cdots \mathbf{V}_1 \mathbf{U} \mathbf{V}_1^\top = \mathbf{V}_1 \mathbf{U}^j \mathbf{V}_1^\top \quad (63) $$

U can be decomposed with eigenvalue decomposition as well, such that

$$ \mathbf{U} = \mathbf{V}_U \mathbf{S}_U \mathbf{V}_U^T \quad (64) $$

which can be analytically given by

$$ \mathbf{S}_U = \mathrm{diag} \left( 0, 0, -\frac{1}{2}\sqrt{\Delta} + \frac{u_1 + u_4}{2}, +\frac{1}{2}\sqrt{\Delta} + \frac{u_1 + u_4}{2} \right) \\ \mathbf{V}_U = \begin{pmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & \frac{-u_1 + u_4 + \sqrt{\Delta}}{2u_2} & \frac{u_1 - u_4 + \sqrt{\Delta}}{2u_2} \\ 0 & 0 & 1 & 1 \end{pmatrix} \quad (65) $$

with

$$ \Delta = u_1^2 - 2u_1u_4 + 4u_2^2 + u_4^2 \quad (66) $$

Letting

$$ \lambda_{U,1} = -\frac{1}{2}\sqrt{\Delta} + \frac{u_1 + u_4}{2} \quad (67) \\ \lambda_{U,2} = +\frac{1}{2}\sqrt{\Delta} + \frac{u_1 + u_4}{2} $$

, $\mathbf{U}^j$ is finally computed by

$$ \mathbf{U}^j = \mathbf{V}_U \mathbf{S}^j \mathbf{V}_U^T = \mathbf{V}_U \mathrm{diag}(0, 0, \lambda_{U,1}^j, \lambda_{U,2}^j) \mathbf{V}_U^T \quad (68) $$

Therefore

$$ (AB)^j = \mathbf{V}_1 \mathbf{U}^j \mathbf{V}_1^\top = \mathbf{V}_1 \mathbf{V}_U \mathrm{diag}(0, 0, \lambda_{U,1}^j, \lambda_{U,2}^j) \mathbf{V}_U^\top \mathbf{V}_1^\top \quad (69) $$

Required computation of $\mathbf{V}_i$ is given by

$$ \frac{1}{2}(\mathbf{W}_i + \mathbf{I}) = \tilde{\mathbf{V}}_i \mathbf{S} \tilde{\mathbf{V}}_i^{-1} \quad (70) $$

where $\tilde{\mathbf{V}}_i(x, y)$ stands for the element of $\tilde{\mathbf{V}}_i$ in x-th row and y-th column, whose details are given by (71)

$$ \begin{aligned} \tilde{\mathbf{V}}_i(1, 1) &= +\frac{1}{V_i}(D_{x,i}^b - D_{x,i}^r)(D_{z,i}^b - D_{z,i}^r) \\ \tilde{\mathbf{V}}_i(1, 2) &= -\frac{1}{V_i}(D_{x,i}^b - D_{x,i}^r)(D_{y,i}^b - D_{y,i}^r) \\ \tilde{\mathbf{V}}_i(1, 3) &= +\frac{1}{V_i}(D_{x,i}^b + D_{x,i}^r)(D_{z,i}^b + D_{z,i}^r) \\ \tilde{\mathbf{V}}_i(1, 4) &= -\frac{1}{V_i}(D_{x,i}^b + D_{x,i}^r)(D_{y,i}^b + D_{y,i}^r) \end{aligned} \quad (71a) $$
$$ 
\begin{aligned}
\tilde{\mathbf{V}}_i(2,1) &= +\frac{1}{V_i}(D_{x,i}^b - D_{x,i}^r)(D_{y,i}^b + D_{y,i}^r) \\
\tilde{\mathbf{V}}_i(2,2) &= +\frac{1}{V_i}(D_{x,i}^b - D_{x,i}^r)(D_{z,i}^b + D_{z,i}^r) \\
\tilde{\mathbf{V}}_i(2,3) &= +\frac{1}{V_i}(D_{x,i}^b + D_{x,i}^r)(D_{y,i}^b - D_{y,i}^r) \\
\tilde{\mathbf{V}}_i(2,4) &= +\frac{1}{V_i}(D_{x,i}^b + D_{x,i}^r)(D_{z,i}^b - D_{z,i}^r)
\end{aligned}
\qquad (71b) $$

$$ 
\begin{aligned}
\tilde{\mathbf{V}}_i(3,1) &= 1 \quad \tilde{\mathbf{V}}_i(3,2) = 0 \quad \tilde{\mathbf{V}}_i(3,3) = 1 \quad \tilde{\mathbf{V}}_i(3,4) = 0 \\
\tilde{\mathbf{V}}_i(4,1) &= 0 \quad \tilde{\mathbf{V}}_i(4,2) = 1 \quad \tilde{\mathbf{V}}_i(4,3) = 0 \quad \tilde{\mathbf{V}}_i(4,4) = 1
\end{aligned}
\qquad (71c) $$

in which the factor is computed by

$$ V_i = (D_{y,i}^b)^2 + (D_{z,i}^b)^2 - (D_{y,i}^r)^2 - (D_{z,i}^r)^2 \qquad (72) $$

Related information can also be acquired from [25]. It should be noted that

$$ \tilde{\mathbf{V}}_i \tilde{\mathbf{V}}_i^\top \neq \tilde{\mathbf{V}}_i^\top \tilde{\mathbf{V}}_i \neq \mathbf{I} \qquad (73) $$

Thus the Gram-Schmidt orthogonalization should be applied to $\tilde{\mathbf{V}}_i$, enabling $\mathbf{V}_i \mathbf{V}_i^\top = \mathbf{V}_i^\top \mathbf{V}_i = \mathbf{I}$ [38]. A typical commitment to achieve this is to compute the QR decomposition [39], such that

$$ \mathbf{V}_i \mathbf{R} = \tilde{\mathbf{V}}_i \qquad (74) $$

### 119 3.2.2. n-Vector Case

Corresponding to the above notations and derivations, the n-vector case's transformation operators are defined by

$$ 
\begin{aligned}
\mathcal{A} &= \mathbf{V}_1 \mathbf{S} \mathbf{V}_1^\top \cdots \mathbf{V}_i \mathbf{S} \mathbf{V}_i^\top \cdots \mathbf{V}_n \mathbf{S} \mathbf{V}_n^\top \\
\mathcal{B} &= \mathbf{V}_n \mathbf{S} \mathbf{V}_n^\top \cdots \mathbf{V}_i \mathbf{S} \mathbf{V}_i^\top \cdots \mathbf{V}_1 \mathbf{S} \mathbf{V}_1^\top
\end{aligned}
\qquad (75) $$

Defining

$$ \mathbf{H} = \mathbf{S} \mathbf{V}_1^\top \left( \prod_{i=2}^{n-1} \mathbf{V}_i \mathbf{S} \mathbf{V}_i^\top \right) \mathbf{V}_n \mathbf{S} = \frac{1}{2^{n-2}} \mathbf{S} \mathbf{V}_1^\top \left[ \prod_{i=2}^{n-1} (\mathbf{W}_i + \mathbf{I}) \right] \mathbf{V}_n \mathbf{S} \qquad (76) $$

, we have

$$ \mathbf{U} = \mathbf{S} \mathbf{V}_1^\top \cdots \mathbf{V}_i \mathbf{S} \mathbf{V}_i^\top \cdots \mathbf{V}_n \mathbf{S}^2 \mathbf{V}_n^\top \cdots \mathbf{V}_i \mathbf{S} \mathbf{V}_i^\top \cdots \mathbf{V}_1 \mathbf{S} = \mathbf{H} \mathbf{H}^\top \qquad (77) $$

Then

$$ \mathcal{AB} = \mathbf{V}_1 \mathbf{U} \mathbf{V}_1^\top \Rightarrow (\mathcal{AB})^j = \mathbf{V}_1 \mathbf{U}^j \mathbf{V}_1^\top = \mathbf{V}_1 \mathbf{V}_U \mathrm{diag}(0, 0, \lambda_{U,1}^j, \lambda_{U,2}^j) \mathbf{V}_U^\top \mathbf{V}_1^\top \qquad (78) $$

<footnote n="117">117 where $\mathbf{R}$ denotes an invertible upper triangular matrix. If $q_{rand} = (1, 0, 0, 0)$, the suboptimal quaternion is equal to the normalized first column of $(AB)^j$.</footnote>

<footnote n="120">120 Accordingly, the normalized first column of $(AB)^j$ constitutes the attitude quaternion.</footnote>

<footnote>121</footnote>
### 122 3.2.3. The Effect of Power Order

In this sub-section we show that the selection of $j$ is in fact not influential to the final result at all. Letting

$$ 
\begin{aligned}
g_1 &= \frac{-u_1 + u_4 + \sqrt{\Delta}}{2u_2} \\
g_2 &= \frac{u_1 - u_4 + \sqrt{\Delta}}{2u_2}
\end{aligned} 
\qquad (79) 
$$

we have

$$ 
\begin{aligned}
(AB)^j(x, y) &= \mathbf{V}_1 \mathbf{V}_U \mathrm{diag}(0, 0, \lambda_{\mathbf{U},1}^j, \lambda_{\mathbf{U},2}^j) \mathbf{V}_U^\top \mathbf{V}_1^\top \\
&= [V_1(y, 4) + V_1(y, 3)g_1] [V_1(x, 4) + V_1(x, 3)g_1] \lambda_{\mathbf{U},1}^j + [V_1(y, 4) + V_1(y, 3)g_2] [V_1(x, 4) + V_1(x, 3)g_2] \lambda_{\mathbf{U},2}^j
\end{aligned} 
\qquad (80) 
$$

Here we also have

$$ \lambda_{\mathbf{U},2} > \lambda_{\mathbf{U},1} > 0 \qquad (81) $$

since

$$ u_1 + u_4 = h_1^2 + h_2^2 + h_3^2 + h_4^2 > 0 \qquad (82) $$

and

$$ 
\begin{aligned}
\Delta - (u_1 + u_4)^2 &= -4u_1u_4 + 4u_2^2 \\
&= 4 \left[ (h_1h_3 + h_2h_4)^2 - (h_1^2 + h_2^2)(h_3^2 + h_4^2) \right] \\
&= 4 \left[ h_1^2 h_3^2 + h_2^2 h_4^2 + 2h_1h_2h_3h_4 - h_1^2 h_3^2 - h_2^2 h_3^2 - h_1^2 h_4^2 - h_2^2 h_4^2 \right] \\
&= -4(h_2h_3 - h_1h_4)^2 < 0
\end{aligned} 
\qquad (83) 
$$

Therefore with increasing iteration numbers, the item multiplied by $\lambda_{\mathbf{U},1}^j$ gradually vanishes in the results. The limiting result of $AB^j$ turns out to be

$$ \lim_{j \to +\infty} (AB)^j(x, y) = [V_1(y, 4) + V_1(y, 3)g_2] [V_1(x, 4) + V_1(x, 3)g_2] \lambda_{\mathbf{U},2}^j \qquad (84) $$

And the quaternion solution is none about which column of $AB^j$, and the result is the normalization of the following vector

$$ \hat{\mathbf{q}} = \begin{bmatrix} V_1(1, 4) + V_1(1, 3)g_2 \\ V_1(2, 4) + V_1(2, 3)g_2 \\ V_1(3, 4) + V_1(3, 3)g_2 \\ V_1(4, 4) + V_1(4, 3)g_2 \end{bmatrix} \qquad (85) $$

### 123 3.3. Discussion of OLEQs

The three derived OLEQs can be used in different occasions. The OLEQ incorporates the weights so that the determination results are optimal in the sense of lease square. When there is aid of gyroscope, the ROLEQ can achieve faster and more smooth estimates. The meaning of the proposed SOLEQ is that it owns very simple linear expression that may generate short and tidy analytic results for certain sensor combinations. Also, when required in application where the weights can hardly be accurately determined e.g. vision attitude determination, the SOLEQ could be an alternative choice, as well. The attitude determination results of the three OLEQs are evaluated in the following experimental section.
## 4. Simulations and Experiments

### 4.1. Simulation: Common Cases

In this sub-section, the sensor observations are simulated with random reference vectors and true DCM in

$$ \mathbf{D}^b = \mathbf{C}_{\text{true}}\mathbf{D}^r + \varepsilon \qquad (86) $$

where $\varepsilon$ is the noise item which is supposed to be independent and subject to Gaussian distribution. The reference vectors and the standard deviations of noise items are selected according to the classical test samples by Markley (see Table 1), where the reference DCM $\mathbf{C}_{\text{true}}$ is

$$ \mathbf{C}_{\text{true}} = \begin{pmatrix} 0.352 & 0.864 & 0.360 \\ -0.864 & 0.152 & 0.480 \\ 0.360 & -0.480 & 0.800 \end{pmatrix} \qquad (87) $$

<table><caption>Table 1. Test Cases</caption><thead><tr><th>Case</th><th>Reference Vectors</th><th>Noise Standard Deviations</th></tr></thead><tbody><tr><td>1</td><td>D<sub>1</sub><sup>r</sup> = [1, 0, 0]<sup>⊤</sup>, D<sub>2</sub><sup>r</sup> = [0, 1, 0]<sup>⊤</sup>, D<sub>3</sub><sup>r</sup> = [0, 0, 1]<sup>⊤</sup></td><td>σ<sub>1</sub> = 10<sup>-6</sup>, σ<sub>2</sub> = 10<sup>-6</sup>, σ<sub>3</sub> = 10<sup>-6</sup></td></tr><tr><td>2</td><td>D<sub>1</sub><sup>r</sup> = [1, 0, 0]<sup>⊤</sup>, D<sub>2</sub><sup>r</sup> = [0, 1, 0]<sup>⊤</sup></td><td>σ<sub>1</sub> = 10<sup>-6</sup>, σ<sub>2</sub> = 10<sup>-6</sup></td></tr><tr><td>3</td><td>D<sub>1</sub><sup>r</sup> = [1, 0, 0]<sup>⊤</sup>, D<sub>2</sub><sup>r</sup> = [0, 1, 0]<sup>⊤</sup>, D<sub>3</sub><sup>r</sup> = [0, 0, 1]<sup>⊤</sup></td><td>σ<sub>1</sub> = 0.01, σ<sub>2</sub> = 0.01, σ<sub>3</sub> = 0.01</td></tr><tr><td>4</td><td>D<sub>1</sub><sup>r</sup> = [1, 0, 0]<sup>⊤</sup>, D<sub>2</sub><sup>r</sup> = [0, 1, 0]<sup>⊤</sup></td><td>σ<sub>1</sub> = 0.01, σ<sub>2</sub> = 0.01</td></tr><tr><td>5</td><td>D<sub>1</sub><sup>r</sup> = [0.6, 0.8, 0]<sup>⊤</sup>, D<sub>2</sub><sup>r</sup> = [0.8, -0.6, 0]<sup>⊤</sup></td><td>σ<sub>1</sub> = 10<sup>-6</sup>, σ<sub>2</sub> = 0.01</td></tr><tr><td>6</td><td>D<sub>1</sub><sup>r</sup> = [1, 0, 0]<sup>⊤</sup>, D<sub>2</sub><sup>r</sup> = [1, 0.01, 0]<sup>⊤</sup>, D<sub>3</sub><sup>r</sup> = [1, 0, 0.01]<sup>⊤</sup></td><td>σ<sub>1</sub> = 10<sup>-6</sup>, σ<sub>2</sub> = 10<sup>-6</sup>, σ<sub>3</sub> = 10<sup>-6</sup></td></tr><tr><td>7</td><td>D<sub>1</sub><sup>r</sup> = [1, 0, 0]<sup>⊤</sup>, D<sub>2</sub><sup>r</sup> = [1, 0.01, 0]<sup>⊤</sup></td><td>σ<sub>1</sub> = 10<sup>-6</sup>, σ<sub>2</sub> = 10<sup>-6</sup></td></tr><tr><td>8</td><td>D<sub>1</sub><sup>r</sup> = [1, 0, 0]<sup>⊤</sup>, D<sub>2</sub><sup>r</sup> = [1, 0.01, 0]<sup>⊤</sup>, D<sub>3</sub><sup>r</sup> = [1, 0, 0.01]<sup>⊤</sup></td><td>σ<sub>1</sub> = 0.01, σ<sub>2</sub> = 0.01, σ<sub>3</sub> = 0.01</td></tr><tr><td>9</td><td>D<sub>1</sub><sup>r</sup> = [1, 0, 0]<sup>⊤</sup>, D<sub>2</sub><sup>r</sup> = [1, 0.01, 0]<sup>⊤</sup></td><td>σ<sub>1</sub> = 0.01, σ<sub>2</sub> = 0.01</td></tr><tr><td>10</td><td>D<sub>1</sub><sup>r</sup> = [1, 0, 0]<sup>⊤</sup>, D<sub>2</sub><sup>r</sup> = [0.96, 0.28, 0]<sup>⊤</sup>, D<sub>3</sub><sup>r</sup> = [0.96, 0, 0.28]<sup>⊤</sup></td><td>σ<sub>1</sub> = 10<sup>-6</sup>, σ<sub>2</sub> = 0.01, σ<sub>3</sub> = 0.01</td></tr><tr><td>11</td><td>D<sub>1</sub><sup>r</sup> = [1, 0, 0]<sup>⊤</sup>, D<sub>2</sub><sup>r</sup> = [0.96, 0.28, 0]<sup>⊤</sup></td><td>σ<sub>1</sub> = 10<sup>-6</sup>, σ<sub>2</sub> = 0.01</td></tr><tr><td>12</td><td>D<sub>1</sub><sup>r</sup> = [1, 0, 0]<sup>⊤</sup>, D<sub>2</sub><sup>r</sup> = [0.96, 0.28, 0]<sup>⊤</sup></td><td>σ<sub>1</sub> = 0.01, σ<sub>2</sub> = 10<sup>-6</sup></td></tr></tbody></table>

Using the simulated samples, the mean attitude root mean-squared errors (RMSEs) in Euler angles are evaluated with our proposed algorithms OLEQ, SOLEQ and representative algorithms including QUEST and FLAE, which are shown in Table 2, 3, 4. Table 5 contains the computed average Wahba's loss function values by different cases and algorithms. These algorithms are executed on the MATLAB r2016 software on a PC for 10000 times with each data sample.

<table><caption>Table 2. Roll RMSE (deg)</caption><thead><tr><th>Case</th><th>OLEQ</th><th>SOLEQ</th><th>QUEST</th><th>FLAE</th></tr></thead><tbody><tr><td>1</td><td>4.3516 × 10<sup>-05</sup></td><td>6.1268 × 10<sup>-05</sup></td><td>4.3516 × 10<sup>-05</sup></td><td>4.3516 × 10<sup>-05</sup></td></tr><tr><td>2</td><td>5.9303 × 10<sup>-05</sup></td><td>6.0734 × 10<sup>-05</sup></td><td>5.9303 × 10<sup>-05</sup></td><td>5.9303 × 10<sup>-05</sup></td></tr><tr><td>3</td><td>4.3482 × 10<sup>-01</sup></td><td>6.0730 × 10<sup>-01</sup></td><td>4.3482 × 10<sup>-01</sup></td><td>4.3482 × 10<sup>-01</sup></td></tr><tr><td>4</td><td>6.0292 × 10<sup>-01</sup></td><td>6.1798 × 10<sup>-01</sup></td><td>6.0292 × 10<sup>-01</sup></td><td>6.0292 × 10<sup>-01</sup></td></tr><tr><td>5</td><td>4.3313 × 10<sup>-01</sup></td><td>4.3313 × 10<sup>-01</sup></td><td>4.9065 × 10<sup>-01</sup></td><td>2.4281 × 10<sup>+01</sup></td></tr><tr><td>6</td><td>4.9590 × 10<sup>-03</sup></td><td>3.0793 × 10<sup>-01</sup></td><td>4.9590 × 10<sup>-03</sup></td><td>4.9590 × 10<sup>-03</sup></td></tr><tr><td>7</td><td>8.1132 × 10<sup>-03</sup></td><td>1.3400 × 10<sup>+00</sup></td><td>8.1132 × 10<sup>-03</sup></td><td>8.1132 × 10<sup>-03</sup></td></tr><tr><td>8</td><td>5.9553 × 10<sup>+01</sup></td><td>6.2840 × 10<sup>+01</sup></td><td>5.9553 × 10<sup>+01</sup></td><td>5.9553 × 10<sup>+01</sup></td></tr><tr><td>9</td><td>7.6662 × 10<sup>+01</sup></td><td>7.6696 × 10<sup>+01</sup></td><td>7.6662 × 10<sup>+01</sup></td><td>7.6662 × 10<sup>+01</sup></td></tr><tr><td>10</td><td>1.4313 × 10<sup>+00</sup></td><td>1.7781 × 10<sup>+00</sup></td><td>4.7663 × 10<sup>+01</sup></td><td>1.5265 × 10<sup>+00</sup></td></tr><tr><td>11</td><td>2.0254 × 10<sup>+00</sup></td><td>2.0254 × 10<sup>+00</sup></td><td>4.6392 × 10<sup>+01</sup></td><td>2.8441 × 10<sup>+00</sup></td></tr><tr><td>12</td><td>2.0818 × 10<sup>+00</sup></td><td>2.0888 × 10<sup>+00</sup></td><td>3.7218 × 10<sup>+01</sup></td><td>1.7415 × 10<sup>+01</sup></td></tr></tbody></table>
<table><caption>**Table 3.** Pitch RMSE (deg)</caption><thead><tr><th>Case</th><th>OLEQ</th><th>SOLEQ</th><th>QUEST</th><th>FLAE</th></tr></thead><tbody><tr><td>1</td><td>4.0108 × 10<sup>-05</sup></td><td>5.7335 × 10<sup>-05</sup></td><td>4.0108 × 10<sup>-05</sup></td><td>4.0108 × 10<sup>-05</sup></td></tr><tr><td>2</td><td>5.2860 × 10<sup>-05</sup></td><td>5.6736 × 10<sup>-05</sup></td><td>5.2860 × 10<sup>-05</sup></td><td>5.2860 × 10<sup>-05</sup></td></tr><tr><td>3</td><td>4.0104 × 10<sup>-01</sup></td><td>5.6744 × 10<sup>-01</sup></td><td>4.0104 × 10<sup>-01</sup></td><td>4.0104 × 10<sup>-01</sup></td></tr><tr><td>4</td><td>5.3887 × 10<sup>-01</sup></td><td>5.7656 × 10<sup>-01</sup></td><td>5.3887 × 10<sup>-01</sup></td><td>5.3887 × 10<sup>-01</sup></td></tr><tr><td>5</td><td>3.9149 × 10<sup>-01</sup></td><td>3.9149 × 10<sup>-01</sup></td><td>4.4335 × 10<sup>-01</sup></td><td>1.2561 × 10<sup>+01</sup></td></tr><tr><td>6</td><td>4.0121 × 10<sup>-05</sup></td><td>5.7809 × 10<sup>-05</sup></td><td>4.0121 × 10<sup>-05</sup></td><td>4.0121 × 10<sup>-05</sup></td></tr><tr><td>7</td><td>5.3398 × 10<sup>-05</sup></td><td>5.7657 × 10<sup>-05</sup></td><td>5.3398 × 10<sup>-05</sup></td><td>5.3398 × 10<sup>-05</sup></td></tr><tr><td>8</td><td>3.6755 × 10<sup>-01</sup></td><td>5.7326 × 10<sup>-01</sup></td><td>3.6755 × 10<sup>-01</sup></td><td>3.6755 × 10<sup>-01</sup></td></tr><tr><td>9</td><td>4.5938 × 10<sup>-01</sup></td><td>5.7880 × 10<sup>-01</sup></td><td>4.5938 × 10<sup>-01</sup></td><td>4.5938 × 10<sup>-01</sup></td></tr><tr><td>10</td><td>5.7186 × 10<sup>-05</sup></td><td>5.7186 × 10<sup>-05</sup></td><td>5.7184 × 10<sup>-05</sup></td><td>5.7186 × 10<sup>-05</sup></td></tr><tr><td>11</td><td>5.7845 × 10<sup>-05</sup></td><td>5.7845 × 10<sup>-05</sup></td><td>5.7846 × 10<sup>-05</sup></td><td>5.7844 × 10<sup>-05</sup></td></tr><tr><td>12</td><td>4.9161 × 10<sup>-01</sup></td><td>5.7554 × 10<sup>-01</sup></td><td>7.9376 × 10<sup>+00</sup></td><td>3.9359 × 10<sup>+00</sup></td></tr></tbody></table>

<table><caption>**Table 4.** Yaw RMSE (deg)</caption><thead><tr><th>Case</th><th>OLEQ</th><th>SOLEQ</th><th>QUEST</th><th>FLAE</th></tr></thead><tbody><tr><td>1</td><td>4.3587 × 10<sup>-05</sup></td><td>6.1084 × 10<sup>-05</sup></td><td>4.3587 × 10<sup>-05</sup></td><td>4.3587 × 10<sup>-05</sup></td></tr><tr><td>2</td><td>4.8694 × 10<sup>-05</sup></td><td>6.1015 × 10<sup>-05</sup></td><td>4.8694 × 10<sup>-05</sup></td><td>4.8694 × 10<sup>-05</sup></td></tr><tr><td>3</td><td>4.4127 × 10<sup>-01</sup></td><td>6.0868 × 10<sup>-01</sup></td><td>4.4127 × 10<sup>-01</sup></td><td>4.4127 × 10<sup>-01</sup></td></tr><tr><td>4</td><td>4.8593 × 10<sup>-01</sup></td><td>6.1289 × 10<sup>-01</sup></td><td>4.8593 × 10<sup>-01</sup></td><td>4.8593 × 10<sup>-01</sup></td></tr><tr><td>5</td><td>2.5186 × 10<sup>-01</sup></td><td>2.5186 × 10<sup>-01</sup></td><td>2.8536 × 10<sup>-01</sup></td><td>1.7459 × 10<sup>+01</sup></td></tr><tr><td>6</td><td>3.6421 × 10<sup>-05</sup></td><td>6.1651 × 10<sup>-05</sup></td><td>3.6421 × 10<sup>-05</sup></td><td>3.6421 × 10<sup>-05</sup></td></tr><tr><td>7</td><td>4.8748 × 10<sup>-05</sup></td><td>6.0826 × 10<sup>-05</sup></td><td>4.8748 × 10<sup>-05</sup></td><td>4.8748 × 10<sup>-05</sup></td></tr><tr><td>8</td><td>3.9812 × 10<sup>-01</sup></td><td>6.1557 × 10<sup>-01</sup></td><td>3.9812 × 10<sup>-01</sup></td><td>3.9812 × 10<sup>-01</sup></td></tr><tr><td>9</td><td>4.9366 × 10<sup>-01</sup></td><td>6.1163 × 10<sup>-01</sup></td><td>4.9366 × 10<sup>-01</sup></td><td>4.9366 × 10<sup>-01</sup></td></tr><tr><td>10</td><td>6.1834 × 10<sup>-05</sup></td><td>6.1834 × 10<sup>-05</sup></td><td>6.1836 × 10<sup>-05</sup></td><td>6.1844 × 10<sup>-05</sup></td></tr><tr><td>11</td><td>6.2069 × 10<sup>-05</sup></td><td>6.2069 × 10<sup>-05</sup></td><td>6.2069 × 10<sup>-05</sup></td><td>6.2077 × 10<sup>-05</sup></td></tr><tr><td>12</td><td>3.1726 × 10<sup>-01</sup></td><td>6.1275 × 10<sup>-01</sup></td><td>5.1968 × 10<sup>+00</sup></td><td>2.8814 × 10<sup>+00</sup></td></tr></tbody></table>

<table><caption>**Table 5.** Loss Function Values</caption><thead><tr><th>Case</th><th>OLEQ</th><th>SOLEQ</th><th>QUEST</th><th>FLAE</th></tr></thead><tbody><tr><td>1</td><td>5.0651 × 10<sup>-13</sup></td><td>1.0130 × 10<sup>-12</sup></td><td>5.0651 × 10<sup>-13</sup></td><td>5.0651 × 10<sup>-13</sup></td></tr><tr><td>2</td><td>2.4901 × 10<sup>-13</sup></td><td>4.9802 × 10<sup>-13</sup></td><td>2.4901 × 10<sup>-13</sup></td><td>2.4901 × 10<sup>-13</sup></td></tr><tr><td>3</td><td>4.9338 × 10<sup>-05</sup></td><td>9.8666 × 10<sup>-05</sup></td><td>4.9338 × 10<sup>-05</sup></td><td>4.9338 × 10<sup>-05</sup></td></tr><tr><td>4</td><td>2.5369 × 10<sup>-05</sup></td><td>5.0736 × 10<sup>-05</sup></td><td>2.5369 × 10<sup>-05</sup></td><td>2.5369 × 10<sup>-05</sup></td></tr><tr><td>5</td><td>5.0582 × 10<sup>-13</sup></td><td>5.0582 × 10<sup>-13</sup></td><td>6.5095 × 10<sup>-13</sup></td><td>8.5878 × 10<sup>-10</sup></td></tr><tr><td>6</td><td>5.0422 × 10<sup>-13</sup></td><td>9.4333 × 10<sup>-10</sup></td><td>5.0422 × 10<sup>-13</sup></td><td>5.0422 × 10<sup>-13</sup></td></tr><tr><td>7</td><td>2.4728 × 10<sup>-13</sup></td><td>8.8239 × 10<sup>-09</sup></td><td>2.4728 × 10<sup>-13</sup></td><td>2.4728 × 10<sup>-13</sup></td></tr><tr><td>8</td><td>4.8216 × 10<sup>-05</sup></td><td>1.1593 × 10<sup>-04</sup></td><td>4.8216 × 10<sup>-05</sup></td><td>4.8216 × 10<sup>-05</sup></td></tr><tr><td>9</td><td>2.5327 × 10<sup>-05</sup></td><td>5.0651 × 10<sup>-05</sup></td><td>2.5327 × 10<sup>-05</sup></td><td>2.5327 × 10<sup>-05</sup></td></tr><tr><td>10</td><td>1.4827 × 10<sup>-12</sup></td><td>1.7575 × 10<sup>-12</sup></td><td>4.8431 × 10<sup>-10</sup></td><td>1.7195 × 10<sup>-12</sup></td></tr><tr><td>11</td><td>4.8573 × 10<sup>-13</sup></td><td>4.8573 × 10<sup>-13</sup></td><td>2.4106 × 10<sup>-10</sup></td><td>9.2333 × 10<sup>-13</sup></td></tr><tr><td>12</td><td>5.0105 × 10<sup>-13</sup></td><td>5.0105 × 10<sup>-05</sup></td><td>1.3143 × 10<sup>-10</sup></td><td>3.4336 × 10<sup>-11</sup></td></tr></tbody></table>

<footnote n="138">138 We first observe the attitude RMSEs. From the results of OLEQ, QUEST and FLAE, it is noticeable to determine that they have the similar attitude determination accuracy. Combining the same statistics in Table 5, the proposed OLEQ is well verified for its optimality. From the presented results, we see that SOLEQ has larger attitude errors and loss function values other optimal methods. The proposed SOLEQ is sub-optimal as it actually approximates the attitude estimator where the weights are ignored.
139
140
141
142</footnote>
Therefore, this simulation scenario has validated the correctness and optimality of the proposed OLEQ and SOLEQ.

## 4.2. Simulation: Extreme Cases

$$D_1^r = \begin{pmatrix} 1 \\ 0 \\ 0 \end{pmatrix}, D_2^r = \begin{pmatrix} -0.99712 \\ 0.07584 \\ 0 \end{pmatrix}, D_3^r = \begin{pmatrix} -0.99712 \\ -0.07584 \\ 0 \end{pmatrix} \qquad (88)$$

$$\sigma_1 = 1 \text{ arcsec}, \sigma_2 = \sigma_3 = 1 \text{ deg}$$

Conventional Wahba's solutions face dilemma when exposed to some critical pairs of vector observations. For instance, Markley and Mortari give an example where the sensors are configured by (88) [40]. In such scenario, the root of the characteristic polynomial of the Davenport matrix can not be easily obtained by Newton iterations. The internal reason is given by Cheng [41] showing that it is resulted in by numerical loss according to the specific CPU word storage length. A flexible transformation of the characteristic polynomial is proposed then to significantly boost the convergence. As such configurations indeed happen in engineering practice, there is necessity to evaluate the proposed schemes by comparisons with representative solvers. With similar simulation techniques aforementioned, the vectors are simulated with given reference vectors and standard deviations by rotation of $C_{true}$. Here the QUEST algorithm is revised to the Cheng's form. First, we mainly compare the two iterative methods QUEST and OLEQ because in our existing paper [25] the QUEST and FLAE have been proved to have very similar behaviour facing this extreme case. Here the iteration stops when the Euclidean norm of neighboring attitude quaternion difference is less than $1 \times 10^{-8}$. For QUEST, the maximum iteration number is set to 50. The obtained results are depicted in Fig. 1. We can see that the supervised QUEST can obtain accurate quaternion solutions within several iterations. Actually, before the Cheng's improvement, the QUEST may exceed the maximum iteration number from time to time. The proposed OLEQ, however, shows better performance dealing with this extreme case. Also, the final mean loss function values of the two algorithms are computed as $4.9890 \times 10^{-11}$ and $2.8391 \times 10^{-10}$, which reveals that the proposed OLEQ can not only obtain faster solutions, but leads to smaller loss function values, compared with supervised QUEST. As is known to everyone, QUEST is the most representative Wahba's solution using Davenport's q-method. Many other algorithms like ESOQ, FOAM actually have the same performance with QUEST. Therefore, in this way, the OLEQ is proved to be faster and more robust than the whole class of the algorithms based on Davenport's q-method. This also shows that the presented novel attitude evolution method shows brand new abilities.

<figure><img src="image_4.png" /><figcaption>Figure 1. Iteration numbers of QUEST and OLEQ in face of an extreme case.</figcaption></figure>
<figure><img src="image_5.png" /><figcaption>**Figure 2.** Designed hardware for algorithm implementation.</figcaption></figure>

## 4.3. Experiment: Accelerometer-Magnetometer Case

In this sub-section, we conduct an experiment where the accelerometer-magnetometer combination is adopted. Such sensor combination is extensively applied in nowadays low-cost attitude estimation schemes. The accelerometer is pre-calibrated using the 6-face bias cancelling while the magnetometer is calibrated using the method proposed by Y. Wu et al. [42].

The hardware is constituted by a battery, a high-end attitude and heading reference system (AHRS) with high precision internal accelerometer, magnetometer and gyroscope, a transmitter for remote data transmission and a micro controller for implementation of the algorithm using C++ language on the FreeRTOS. With the designed hardware platform shown in Fig. 2, we collect a data set with 10000 samplings.

The main purpose of this sub-section is to validate the performances of the proposed OLEQ, SOLEQ and ROLEQ since the AHRS has angular rate readouts. The compared results with the reference angles from representative methods are obtained (see Fig. 3, 4, 5). Note that here the weights between the accelerometer and magnetometer for Wahba's solution are chosen as 0.63 and 0.37 according to their respective noise characteristics. Yet, the local magnetometer's reference vector is calculated as $\mathbf{M}^r = (0.60311, 0, -0.79766)^\top$ in Wuxi, Jiangsu Province, China.

In Fig. 3, the reference angles, QUEST solutions and SOLEQ solutions have been presented. The results indicate that the proposed sub-optimal estimator can estimate the attitude angles with similar macroscopic accuracy. Detailed attitude errors are plotted in Fig. 4 and 5. We may notice that in general, these algorithms have the same errors with respect to reference. The second picture shows that the overall attitude accuracy of the ROLEQ is slightly smaller than the others. This is because it is first processed by the angular rate data, which can be equivalent to a smoothing procedure.
<figure><img src="image_6.png" /><figcaption>Figure 3. Experiment results using QUEST and SOLEQ.</figcaption></figure>

<figure><img src="image_7.png" /><figcaption>Figure 4. Experiment results of OLEQ, SOLEQ and representative algorithms using sampled data and different algorithms.</figcaption></figure>
<figure><img src="image_8.png" /><figcaption>**Figure 5.** Experiment results of ROLEQ and representative algorithms using sampled data and different algorithms.</figcaption></figure>

## 4.4. Experiment: GNSS Attitude Determination for Land Vehicles

The GNSS receiver is widely employed in the attitude determination tasks for land and unmanned aerial vehicles. In this experiment, we use a designed rover (see Fig. 6) to validate the feasibility of the proposed algorithm for GNSS attitude determination.

<figure><img src="image_9.png" /><figcaption>**Figure 6.** The designed multi-functional rover for validation of proposed algorithms.</figcaption></figure>
The rover is armed with the aforementioned navigation computer and employs an external ublox M8N GPS module with serial comm connection to the board at the sampling frequency of 5Hz. This rover is controlled by a handheld 2.4GHz transmitter and the onboard Pixhawk autopilot generates PID controlling commands to the servos and motors according to internal measurements. In this experiment, the rover is ran on a playground of UESTC and we pick up one period of data in which the GPS velocity is valid. In the data history, the magnetometer was distorted by outer unknown electromagnetic and ferromagnetic fields. Also, during the execution process, sensor raw measurements from gyroscope, accelerometer are also logged with the speed of 250Hz. The raw data is shown in Fig. 7.

<figure><img src="image_10.png" /><figcaption>**Figure 7.** Raw sensor measurements from the logging memory.</figcaption></figure>

According to the sensor noise characteristics, the weights of the accelerometer-magnetometer combination are given by 0.9 and 0.1 respectively. While for the accelerometer-magnetometer-GNSS one, the weights are chosen as 0.474, 0.05, 0.474 respectively. The reference vector of magnetometer is determined by the initial GPS position with the IGRF model. By making use of algorithms including QUEST, FLAE, OLEQ and SOLEQ, the computation results are summarized in Fig. 8 and 9.
<figure><img src="image_11.png" /><figcaption>**Figure 8.** Attitude determination from accelerometer, magnetometer and velocity output of GNSS receiver, by means of QUEST and FLAE.</figcaption></figure>

We especially add the GPS velocity norm under Euler angle results to illustrate the influence of the velocity scalar to the attitude determination results. In principle, when the vehicle is not moving with relative discriminative velocities, the GPS receiver can not give accurate speed estimates. Therefore, it is shown in the initial stage of the attitude determination results where GNSS takes part in that the determination accuracy of the yaw angle is seemingly very poor. As the velocity increases, the accuracy is improved very fast accordingly. The accelerometer-magnetometer combination is largely distorted by the magnetic disurbances. The integrated results of roll, pitch and more over, the yaw angles are influenced generating very obvious differences with reference angles. With the aid of GNSS velocity, the corresponding attitude determination accuracy is not damaged because the Wahba's solution balances the sensor inputs by the weights. It is observed that the OLEQ is validated to have almost the same accuracy for normal sensors in aforementioned section and in this section such behaviour holds as well. The SOLEQ, however, does not employs the weights and therefore produces relative bad estimates but for GNSS case, it is still better than accelerometer-magnetometer ones. The results provided us with the information of the validity of the proposed algorithms especially the OLEQ.
<figure><img src="image_12.png" /><figcaption>**Figure 9.** Attitude determination from accelerometer, magnetometer and velocity output of GNSS receiver, by means of OLEQ and SOLEQ.</figcaption></figure>

4.5. *Computation Time*

From another point of view, the time consumption of various algorithms should be investigated. The time consumption is calculated on the embedded platform by C++ programming language that ensures the fairness. A rough evaluation is done with two pairs of vector observations in few samples which shows direct time consumption results (see Fig. 10). As shown in the figure, for two pairs of vector observations, the three proposed algorithms’ computation times are between QUEST and FLAE. However, from the expressions of the algorithms presented before, the number of vector observations is influential to the final time consumption. Hence with the simulation samples, each algorithm is again tested for 20000 times with different vector observation numbers. The time consumption is averaged, which is plotted in Fig. 11. The results show that the algorithms are all linear owning the time complexities of O(n). QUEST, OLEQ and ROLEQ join at the vector observation number of 20. For common tasks, such number covers most sensor amounts. Although FLAE owns the least time consumption, it can not overcome drawbacks of extreme cases so well as OLEQ. That is to say, the proposed algorithms can replace the original algorithms for faster and more robust attitude determination.
<figure><img src="image_13.png" /><figcaption>**Figure 10.** Time consumption of various algorithms.</figcaption></figure>

<figure><img src="image_14.png" /><figcaption>**Figure 11.** Time consumption of algorithms with respect to numbers of vector observations.</figcaption></figure>
## 5. Conclusion

This paper revisits the attitude determination from vector observation for a GNSS/AM case study. Novel linear algorithms are designed to obtain accurate attitude estimates in the sense of least-square. Handling in this manner, the computed quaternion is identical or suboptimal with respect to conventional Wahba's solutions including QUEST and FLAE. Numerical simulations exhibit that the proposed OLEQs own the similar accuracy with representative solvers. It is also evaluated that facing extreme cases, the OLEQs show much more robustness less computation iterations. The computation speeds of OLEQs are tested revealing that they belong to computationally efficient algorithms. Moreover, a real vehicular experiment of GNSS/AM system is designed and conducted showing the effectiveness of the proposed OLEQs in real-world embedded applications. The presented approach provides the audience with a brand new viewpoint of attitude evolution and hopefully would benefit related multi-sensor attitude determination applications.

**Acknowledgments:** This research is supported by National Natural Science Foundation of China under the grant number of 41604025. Dr. F. L. Markley provided constructive suggestions on the internal theory of this paper. Yunheng Inc., Chengdu gave us the instrumentation services during experimental validation. We genuinely thank them for their support.

**Conflicts of Interest:** The authors declare no conflict of interests regarding the publication of this paper.

1. Zhou, Z.; Li, Y.; Liu, J.; Li, G. Equality constrained robust measurement fusion for adaptive kalman-filter-based heterogeneous multi-sensor navigation. *IEEE Transactions on Aerospace and Electronic Systems* **2013**, *49*, 2146-2157.

2. Zhou, Z.; Li, Y.; Zhang, J.; Rizos, C. Integrated Navigation System for a Low-Cost Quadrotor Aerial Vehicle in the Presence of Rotor Influences. *Journal of Surveying Engineering* **2017**, *143*, 05016006.

3. Chang, G.; Xu, T.; Wang, Q. M-estimator for the 3D symmetric Helmert coordinate transformation. *Journal of Geodesy* **2017**.

4. Wu, Y.; Wang, J.; Hu, D. A New Technique for INS / GNSS Attitude and Parameter Estimation Using Online Optimization. *IEEE Transactions on Signal Processing* **2014**, *62*, 2642-2655.

5. Chang, L.; Qin, F.; Zha, F. Pseudo Open-Loop Unscented Quaternion Estimator for Attitude Estimation. *IEEE Sensors Journal* **2016**, *16*, 4460-4469.

6. Han, S.; Wang, J. Quantization and colored noises error modeling for inertial sensors for GPS/INS integration. *IEEE Sensors Journal* **2011**, *11*, 1493-1503.

7. Chang, L.; Li, J.; Chen, S. Initial Alignment by Attitude Estimation for Strapdown Inertial Navigation Systems. *IEEE Transactions on Instrumentation and Measurement* **2015**, *64*, 784-794.

8. Marins, J.; Yun, X.; Bachmann, E.; McGhee, R.; Zyda, M. An extended Kalman filter for quaternion-based orientation estimation using MARG sensors. *Proceedings 2001 IEEE/RSJ International Conference on Intelligent Robots and Systems. Expanding the Societal Role of Robotics in the Next Millennium (Cat. No.01CH37180)* **2001**, *4*.

9. Tian, Y.; Wei, H.; Tan, J. An adaptive-gain complementary filter for real-time human motion tracking with MARG sensors in free-living environments. *IEEE Transactions on Neural Systems and Rehabilitation Engineering* **2013**, *21*, 254-264.

10. Aleshechkin, A. Algorithm of GNSS-based attitude determination. *Gyroscopy and Navigation* **2011**, *2*, 269.

11. Grewal, M.; Andrews, A. Kalman filtering: theory and practice using MATLAB, 2001.

12. Niu, X.; Nassar, S.; EL-SHEIMY, N. An Accurate Land-Vehicle MEMS IMU/GPS Navigation System Using 3D Auxiliary Velocity Updates. *Navigation* **2007**, *54*, 177-188.

13. Niu, X.; Nasser, S.; Goodall, C.; El-Sheimy, N. A universal approach for processing any MEMS inertial sensor configuration for land-vehicle navigation. *The Journal of Navigation* **2007**, *60*, 233-245.

14. Gebre-Egziabher, D.; Elkaim, G.H. MAV attitude determination by vector matching. *IEEE Transactions on Aerospace and Electronic Systems* **2008**, *44*.

15. Zhang, J.; Zhang, K.; Grenfell, R.; Deakin, R. Short note: On the relativistic Doppler effect for precise velocity determination using GPS. *Journal of Geodesy* **2006**, *80*, 104-110.
290
291

16. Zhou, Z.; Li, B. Optimal Doppler-aided smoothing strategy for GNSS navigation. *GPS solutions* **2017**, *21*, 197–210.

292
293
294

17. Geng, J.; Bock, Y.; Melgar, D.; Crowell, B.W.; Haase, J.S. A new seismogeodetic approach applied to GPS and accelerometer observations of the 2012 Brawley seismic swarm: Implications for earthquake early warning. *Geochemistry, Geophysics, Geosystems* **2013**, *14*, 2124–2142.

295
296

18. Geng, J.; Melgar, D.; Bock, Y.; Pantoli, E.; Restrepo, J. Recovering coseismic point ground tilts from collocated high-rate GPS and accelerometers. *Geophysical Research Letters* **2013**, *40*, 5095–5100.

297
298
299

19. Wahba, G. A Least Squares Estimate of Satellite Attitude, 1965.

300
301
302
303
304
305

20. Shuster, M.D.; Oh, S.D. Three-axis attitude determination from vector observations. *Journal of Guidance, Control, and Dynamics* **1981**, *4*, 70–77.

306
307
308
309
310
311
312
313
314

21. Markley, F.L. Attitude Determination using Vector Observations and the Singular Value Decomposition, 1988.
22. Mortari, D. ESOQ: A Closed-Form Solution to the Wahba Problem. *The Journal of the Astronautical Sciences* **1997**, *45*, 195–204.

315
316

23. Davenport, P.B. A vector approach to the algebra of rotations with applications. Technical Report August, 1968.

317
318
319
320
321
322
323
324
325
326

24. Chang, G.; Xu, T.; Wang, Q. Error analysis of Davenport's q method. *Automatica* **2017**, *75*, 217–220.

327
328
329
330
331
332
333
334
335

25. Wu, J.; Zhou, Z.; Li, R.; Yang, L. Attitude Determination Using a Single Sensor Observation: Analytic Quaternion Solutions and Property Discussion. *IET Science, Measurement & Technology (Accepted) 2017*.

336
337
338
339
340
341

26. Yang, Y.; Zhou, Z. An analytic solution to Wahba's problem. *Aerospace Science and Technology* **2013**, *30*, 46–49, [1309.5679].

342
343
344
345
346
347
348
349
350
351

27. Yang, Y. Attitude determination using Newton's method on Riemannian manifold. *Proceedings of the Institution of Mechanical Engineers, Part G: Journal of Aerospace Engineering* **2015**, *0*, 1–6.

352
353
354
355
356
357
358
359
360
361

28. Forbes, J.R.; de Ruiter, A.H.J. Linear-Matrix-Inequality-Based Solution to Wahba's Problem. *Journal of Guidance, Control, and Dynamics* **2015**, *38*, 147–151.

362
363
364
365
366
367
368
369
370
371

29. Mahony, R.; Hamel, T.; Pflimlin, J.M. Nonlinear complementary filters on the special orthogonal group. *IEEE Transactions on Automatic Control* **2008**, *53*, 1203–1218.

372
373
374
375
376
377
378
379
380
381

30. Madgwick, S.O.H.; Harrison, A.J.L.; Vaidyanathan, R. Estimation of IMU and MARG orientation using a gradient descent algorithm. *IEEE International Conference on Rehabilitation Robotics* **2011**.

382
383
384
385
386
387
388
389
390
391

31. Arun, K.S.; Huang, T.S.; Blostein, S.D. Least-Squares Fitting of Two 3-D Point Sets. *IEEE Transactions on Pattern Analysis and Machine Intelligence* **1987**, *PAMI-9*, 698–700.

394
395
396
397
398
399
400
401
402
403

32. Ye, C.; Hong, S.; Tamjidi, A. 6-DOF Pose Estimation of a Robotic Navigation Aid by Tracking Visual and Geometric Features. *IEEE Transactions on Automation Science and Engineering* **2015**, *12*, 1169–1180.

404
405
406
407
408
409
410
411
412
413

33. Markley, F.L.; Mortari, D. Quaternion attitude estimation using vector observations. *Journal of the Astronautical Sciences* **2000**, *48*, 359–380.

414
415
416
417
418
419
420
421
422
423

34. Markley, F.L.; Mortari, D. How to estimate attitude from vector observations. *Advances in the Astronautical Sciences* **2000**, *103*, 1979–1996.

424
425
426
427
428
429
430
431

35. Markley, F.L. Humble problems. *Advances in the Astronautical Sciences* **2006**, *124 II*, 2205–2222.

435
436
437
438
439
440
441
442
443
444

36. Wu, J.; Zhou, Z.; Chen, J.; Fourati, H.; Li, R. Fast Complementary Filter for Attitude Estimation Using Low-Cost MARG Sensors. *IEEE Sensors Journal* **2016**, *16*, 6997–7007.

445
446
447
448
449
450
451
452
453
454

37. Shuster, M.D. Filter QUEST or REQUEST. *Journal of Guidance, Control, and Dynamics* **2009**, *32*, 643–645.

455
456
457
458
459
460
461
462
463
464

38. Horn, R.A.; Johnson, C.R. *Matrix analysis*; Cambridge university press, 2012.

466
467
468
469
470
471
472
473
474
475

39. Yang, Y. Spacecraft attitude determination and control: Quaternion based method. *Annual Reviews in Control* **2012**, *36*, 198–219.

476
477
478
479
480
481
482
483
484
485

40. Markley, F.L.; Mortari, D. Quaternion Attitude Estimation Using Vector Observations. *Journal of the Astronautical Sciences* **2000**, *48*, 359–380.

486
487
488
489
490
491
492
493
494
495

41. Cheng, Y.; Shuster, M.D. Improvement to the Implementation of the QUEST Algorithm. *Journal of Guidance, Control, and Dynamics* **2014**, *37*, 301–305.

500
501
502
503
504
505
506
507
508
509

42. Wu, Y.; Shi, W. On Calibration of Three-Axis Magnetometer. *IEEE Sensors Journal* **2015**, *15*, 6424–6431.

510
511
512
513
514
515
516
517
518
519