1. Introduction
Attitude determination (or estimation) from vector observation pairs is a significant technology in aerospace engineering and related geodetic applications [
1,
2,
3]. For instance, inertial navigation, having an important role in modern military applications, has a high demand for attitude determination accuracy for the initial alignment [
4,
5,
6,
7]. A typical attitude-measuring system integrating a three-axis Accelerometer with a three-axis Magnetometer (AM) is extensively applied for real-time, continuous, stable and accurate attitude estimation for various navigation campaigns [
8,
9]. For most applications, AM sensors are very efficient for low-cost attitude determination [
10]. However, the magnetometer is easily disturbed by unknown and unexpected magnetic fields in electromagnetic signal-contaminated environments. On the other hand, for large-scale region applications, the reference magnetic vector is no longer a constant vector and needs to be corrected in a timely manner with other additional heading information. Otherwise, the overall system performance will be heavily degraded. Moreover, accelerometers inevitably suffer from biases, thus leading to inaccurate attitude estimation. Therefore, auxiliary sensors are necessary to overcome such a problem.
Alternatively, the Global Navigation Satellite System (GNSS) provides precise position and velocity information [
11]. It has been successfully used for land, marine and aircraft attitude determination applications [
12]. Traditional methods integrate GNSS with inertial sensors and simultaneously estimate the orientation with synchronized position, velocity and attitude loops [
13,
14,
15]. However, this leads to the risk of degrading the accuracy of the attitude solution associated with position and velocity estimation loops. Thus, Gebre-Egziabher and Elkaim [
16] proposed 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, a single GNSS antenna is preferred in many low-cost and low-power consumption platforms. It mainly uses the simultaneous velocity vector information generated by GNSS Doppler [
17,
18]. Indirectly, this produces important complementary orientation information, thus potentially benefiting the AM sensor system, especially for land vehicles. In addition, integrating high-rate sensors also contributes to seismogeodetic systems [
19,
20].
Efficient attitude estimation strategy is very crucial for a GNSS/AM integrated multi-sensor system. In essence, it can be mathematically converted into a multi-observation vector pairs matching problem. Representative methods are mainly about solutions to the famous Wahba’s problem [
21], which aims to obtain the optimal attitude determination results using weighted least-squares. Among these algorithms, Shuster’s QUaternionESTimator(QUEST [
22]), Markley’s Singular Value Decomposition (SVD [
23]) and Mortari’s algorithm (ESOQ [
24]) are the most frequently-used ones, which are mostly inspired by Davenport’s q-method [
25,
26]. Our newly-developed Fast Linear Attitude Estimator (FLAE [
27]) obtains the fastest Wahba’s solution, to our existing knowledge. Some other interesting approaches are proposed, as well, investigating the other aspects of this problem, e.g., Yang’s analytical method, the Riemannian-manifold algorithm and Forbes’ Linear Matrix Inequality (LMI) solution [
28,
29,
30].
There are still some weight-less algorithms for multi-sensor attitude determination. They are usually used in 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 [
31], it is able to obtain the attitude quaternion from arbitrary pairs of vectors. Via optimization approaches like the Gradient-Decent Algorithm (GDA [
32]) and the 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 Singular Value Decomposition (SVD) is employed to calculate the attitude matrix [
33]. Similarly, it is then introduced in computing both the attitude and translation vector in machine vision systems [
34].
For Wahba’s problem, it has been shown that most existing algorithms are based on Davenport’s q-method. For a long time, the attitude-solving process has been fixed in this framework, which aims to find the largest eigenvalue of the Davenport matrix . 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:
The main theory is based on our previous research contributions and is extended to linear arbitrary pairs vector measurements for GNSS/AM attitude application.
Three estimators, i.e., the Optimal Linear Estimator of Quaternion (OLEQ), Recursive OLEQ (ROLEQ) and Sub-optimal OLEQ (SOLEQ), are derived. We also proposed acceleration techniques to make the algorithms faster.
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 demonstrate the superiority of the proposed OLEQs.
This paper is structured as follows:
Section 2 briefly introduces the GNSS-AM sensor system along with its functional and stochastic models formulated by vector pair matching.
Section 3 contains the problem formulation and starts with the quaternion estimation from a single sensor observation. In
Section 4, the two-vector attitude determination theory along with the
n-vector theory are given accordingly.
Section 5 involves the numerical examples and the real field test where comparisons between the proposed SOLEQ and other representative methods are given. Finally,
Section 6 consists of concluding remarks and future work.
2. Fundamentals of the GNSS/AM System
For attitude determination, we require GNSS receivers and AM sensor 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:
where
denotes velocity vector; the subscript
G denotes the observation source ‘GNSS’; the symbols
r and
b represent the navigation frame (
r-frame, North-East-Down (NED)) and body frame (
b-frame, forward-right-down), respectively; the vector is transformed from the
r-frame to the
b-frame;
is the Gaussian white noise with variance of
. For the land-vehicle application, it should be pointed out that the velocities in the
r-frame and the
b-frame are respectively:
where
e denotes an Earth-Centred-Earth-Fixed (ECEF) coordinate frame (
e-frame), i.e., WGS-84;
denotes the transformation from the
e-frame to the
r-frame, which can be computed according to the GNSS position and Earth ellipsoid metrics in advance.
AM sensors consist of a three-axis MEMS accelerometer and three-axis magnetometer. The accelerometer gives the specific force measurement of a rigid body, and the magnetometer provides the users with the sensed local Earth geomagnetic field according to the IGRFmodel [
35].
where the subscripts
A and
M denote the accelerometer and magnetometer sources, respectively;
denotes the observed vector;
denotes the accelerometer bias;
, where
g is the gravity, is a function of the geo-location; the normalized Earth’s magnetic field reference vector
, where
is the local dip angle;
is the linear acceleration vector, which is usually treated as an external disturbance;
is the Gaussian white noise with the variance of
. 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
is estimated by using GNSS velocity information with differentiation between two adjacent epochs. Then, the vectors and matrices for the system model in the
b-frame and the
r-frame can be merged into the following multi-vector pair equations:
where:
Neglecting the cross-correlations between sensors, the stochastic model of the system is:
3. 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:
in which
denotes the optimal Direction Cosine Matrix (DCM);
and
are the
i-th pair of normalized vector observations from the body frame
b and the reference frame
r, respectively;
is the weight of the
i-th sensor output, which is given by the standard deviations of the input vectors from
to
:
provided that the variance information such as shown in Equation (
6) is predetermined [
22]. This ensures the biggest variance scalar with respect to the smallest weight. Wahba’s problem is solved via many representative methods [
36]. Many of these algorithms solve the problem via eigenvalue decompositions analytically or numerically [
28,
29]. When there is only one pair of vector observations, Wahba’s solutions fail to obtain the optimal quaternion since there will be ambiguous quaternions corresponding to the maximum eigenvalue of one [
37,
38]. In our previous contribution [
39], the continuous stable quaternion solution to an accelerometer-based attitude determination system is derived.
Quaternion from a Single Sensor Observation
Considering an attitude determination model from a pair of vector observations:
this section deals with the attitude determination from a single pair of sensor observations. Note that the DCM is decomposed with quaternions
[
39] via:
in which:
In this section, the theory is extended to an arbitrary sensor with a similar approach as in [
39]. Inserting Equation (
10) into Equation (
9) yields:
It has been proven in [
39] that:
where † stands for the Moore–Penrose pseudo-inverse. In fact, another property can be deduced from the following theorem:
Theorem 1. .
Proof. This can be proven via brute-force calculation:
☐
Lemma 1. Let , .
Proof. We have:
which gives Lemma 1. ☐
Hence, Equation (
12) can be transformed into:
Since
is in the form of a quaternion,
can be expanded by:
Furthermore, it can be obtained that:
Then, we have:
where
is given by:
Therefore, the attitude determination problem is shifted to:
Theorem 2. For any normalized vector observation pair , , where is the four-dimensional identity matrix.
Proof. The characteristic polynomial of
is given by:
Then, with the Cayley–Hamilton theorem, we substitute
for its eigenvalues and obtain:
and as
is real symmetric, it should be
, which completes the proof. ☐
According to our previous contribution [
39], Equation (
21) can be treated as an iterative dynamical system:
where
denotes the quaternion for the
n-th iteration. Furthermore, as has been proven, the discrete-time system can be converted to a corresponding continuous system. If
, the stable solution to the continuous-time system is calculated as:
where
denotes a randomly-chosen unit quaternion. This provides us with a new approach to obtaining the measurement quaternion from a single strapdown sensor.
4. Optimal Linear Estimator of the Quaternion
With Equation (
9), it is possible to list all the single rotation equations together as follows:
which can in turn be converted to:
This system is rewritten as:
Noticing that:
hence the pre-multiplication leads to:
It should be then noted that Equation (
28) is free of noises. In this case, the maximum eigenvalue of
in engineering practice is very close to the noise-free theoretical result of one. Corresponding to Equation (
24), based on Brouwer’s fixed-point theorem, it is possible to recursively obtain the normalized optimal quaternion by rotating a randomly given initial quaternion over and over again to infinity. This is something similar to the power method of the numerical eigenvalue factorization, but can be accelerated with geometric series-like iterations as follows:
where
denotes the rotation operator over the Hamilton space
. In fact, the above iterations can hardly be achieved when the maximum eigenvalue approaches one. The reason is that at this time, the power
approaches
, as well. A more robust way is shown to solve this problem. Considering both sides of Equation (
28), we find out that the right side is in fact the mixture of solutions to single vector observation pairs. As mentioned in Equation (
25), a stable and continuous solution to each single equation can be done by pre-multiplying
. Substituting
for
, i.e.,
the quaternion evolutes are from the mixture of the single optimal solution. Here, the rotation operator is revised as:
This equals the least-square of the set of pre-computed single rotated quaternions, 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:
where
is the vector part of the quaternion.
defines the following matrix:
4.1. Variant 1: Recursive-OLEQ
We have seen from the above formulations that for each epoch, the vector observations are batch processed thoroughly. When used in aerospace electronic systems, the measured vector observation pairs in neighbouring time epochs are usually continuous since they are always 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 fewer computations, and the accuracy is maintained. Furthermore, for highly-reliable attitude determination systems, high-precision gyroscopes are usually employed. This provides us with a second-stage acceleration scheme, inspired by the conventional recursive algorithms like filter QUEST, REQUEST, etc. [
40], by which we may first rotate the estimated quaternion in the last time epoch with the zero-order angular transition matrix by:
where:
in which
is the sampling time and
composed by the angular rate from the gyroscope, which is detailed in many classical literature works [
41]. After this, even a single rotation by
would then be very accurate. The one-step covariance matrix of the obtained quaternion is calculated by:
4.2. Variant 2: SOLEQ
4.2.1. Two-Vector Case
When there are two pairs of vector observations, regardless of the weights of the respective sensors, it is natural that the attitude quaternion may be computed via:
where
and
are for the first and second sensors. Equation (
25) is reasonable since we have:
However, for the two-vector case, one can write:
This reflects that the accurate quaternion can be recursively computed with:
which exits when the Euler distance
is less than one predetermined threshold
. Note that the above initialization procedure is equivalent to the following process:
where
is a randomly chosen quaternion;
j is chosen to make sure
. Let us define the following transformation operators:
Theorem 3. For the two-vector attitude determination case, the steady-state evolution in Equation (43) is not affected by the transformation operators’ order, such that: Proof. The integrated transformation can be computed by:
and accordingly
This proves that the mixed steady-state transformation can be achieved by independent transformations from or , which completes the proof. ☐
Following this theorem, the problem is to compute the power
. In fact,
is formed with
and
. Their respective eigenvalue decompositions can be given by:
where
and
are constituted by eigenvectors and eigenvalues respectively as
is real symmetric [
42]. Since
and
are in the same form, the eigenvalue matrices are equal to each other, i.e.,
Then,
can be rewritten as:
Combining Equations (
53) and (
54), it is obtained that:
Equation (
55) is simplified as:
Actually, it can be decomposed by:
An interesting fact is that the eigenvalue matrix
can be analytically calculated and is given by:
where
represents the diagonal matrix. This further yields
to be a matrix with the form of:
Then,
is computed by:
where
. Using this, we have:
can be decomposed with eigenvalue decomposition as well, such that:
which can be analytically given by:
with:
Letting:
is finally computed by:
The required computation of
is computed as follows:
where
stands for the element of
in the
x-th row and the
y-th column, the details of which are given by Equation (71):
in which the factor is computed by:
Related information can also be acquired from [
27]. It should be noted that:
Thus, the Gram–Schmidt orthogonalization is applied to
, enabling
[
42]. A typical commitment to achieve this is to implement the QR decomposition [
43], such that:
where
denotes an invertible upper triangular matrix. If
, the suboptimal quaternion is equal to the normalized first column of
.
4.2.2. n-Vector Case
Corresponding to the above notations and derivations, the n-vector case’s transformation operators are defined by:
Accordingly, the normalized first column of constitutes the attitude quaternion.
4.2.3. The Effect of Power Order
In this sub-section, we will show that the selection of
j is in fact not influential with respect to the final result at all. Letting:
we have:
Here, we also have:
since:
and:
Therefore, with increasing iteration numbers, the item multiplied by
gradually vanishes in the results. The limiting result of
turns out to be:
Additionally, the quaternion solution is not about which column of
, and the result is the normalization of the following vector:
4.3. Discussion of OLEQs
The three derived OLEQs can be used on different occasions. The OLEQ incorporates the weights so that the determination results are optimal in the sense of least-squares. With the aid of the gyroscope, the ROLEQ can achieve faster and smoother estimates. The meaning of the proposed SOLEQ is that it has a very simple linear expression that may generate short and concise analytic results for certain sensor combinations. Furthermore, when required in applications 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.