A Global Interconnected Observer for Attitude and Gyro Bias Estimation with Vector Measurements

Abstract

This paper proposes a novel interconnected observer to get good estimates of attitude and gyro bias from high-noise vector measurements. The observer is derived based on the theory of nonlinear and linear cascade systems, and its error dynamics have the properties of global exponential stability and robustness to bounded noise. These properties ensure the convergence and boundedness of the attitude and gyro bias estimation errors. To obtain higher estimation accuracy, an approach to calculate time-varying gains for the proposed auxiliary observer is designed under the premise of considering noise terms in the rate gyro and vector sensors. The simulation results show that when the vector sensors’ outputs contain high-level noise, the proposed observer with time-varying gains yields better performance in both the transient and steady-state phases.

1. Introduction

Attitude estimation of a rigid body is an indispensable part of navigation. Questions of estimating attitude have been a field of concern for decades due to its numerous applications in various systems, such as unmanned underwater vehicles (UUVs) [1], unmanned aerial vehicles (UAVs) [2], and others [3]. A rigid body’s attitude can be resolved by integrating the angular velocity from a rate gyro output. However, even with high-precision gyros, the accumulated drift over time can affect the accuracy of the attitude estimation, not to mention the low-cost ones. A typical approach to estimate attitude is to utilize algebraic methods of vector measurements only by comparing vectors measured in either the body-fixed coordinate frame or the reference frame with vectors measured in the other. Triad and Quest in [4] used two or more nonparallel vector measurements to determine the attitude. Unfortunately, bias and noise can easily corrupt the vector measurements. Therefore, combining angular velocity sensors with vector sensors (e.g., accelerometers, magnetometers, star trackers, or sun sensors) has been developed for improving the estimation accuracy.

The current approaches for estimating attitude and gyro bias from vector sensors and rate gyros can be summarized into two classes, stochastic filtering algorithms (such as EKF, UKF, and their variants; see [5,6,7]) and nonlinear observers (e.g., [8,9]). Although EKFs and UKFs have been widely used, they cannot guarantee convergence in strongly nonlinear systems, and UKFs may increase the computational cost. In recent years, more efforts have been made in constructing nonlinear observers. In [10,11], nonlinear complementary filters designed on the special orthogonal group $SO(3)$ for low-cost measurement units became the foundation of other observers. On this basis, Grip et al. [12] developed semi-globally stable observers with gyro bias and time-varying reference vectors. After that, they came up with an alternative semi-global attitude observer in the unit quaternion space [13] by employing the theory of cascaded linear and nonlinear system observers, which appeared in [14,15]. For obtaining globally stable observers, some studies adopt the $SO(3)$ topological constraint lifting methods before estimating the attitude rotation matrix, such as Batista et al. [16,17], Grip et al. [9,18], Bryne et al. [19], and Fusini et al. [20]; in addition, the nonlinear observers in [18,19,20] are all based on extensions or various applications of the observer in [9]. Among these observers, except the observer in [16] attained global asymptotical stability; others achieved global exponential stability. Besides et al. [21,22] proposed two different “geometry-free” nonlinear observers and verified that the former has the property of global asymptotic stability, while the latter has global exponential stability. All the aforementioned nonlinear observers apply at least two vector measurements combined with gyro measurements. Unlike them, the methods described by Batista et al. [23,24] merely use a signal vector observation, coupled with a persistency-of-excitation (PE) condition, and also reach global exponential stability.

Most observers mentioned the preceding are designed and proved under the assumption of noise-free scenarios and do not allow vector measurement bias. Like [25,26,27], researchers took noise terms of the linear cascade system into account just when deriving and calculating the time-varying gains of the linear system observer, which means they do not consider the gyro and vector sensor noise. Therefore, a noisy attitude observer model was given in [28] while discussing and proofing the cascade attitude observer’s robustness to bounded noise and stochastic noise on all sensors. Additionally, [12] first offered a method of estimating body-fixed vector bias, using an exponential convergence algorithm, and then employing this method in cascade attitude and gyro bias observers to improve the estimation accuracy. Moreover, Martin et al. [29] presented a nonlinear “non-geometry” observer, regarded a bias of one vector observation and rate gyro. They verified that considering one vector measurement bias can also obtain global asymptotic stability and local exponential stability. The literature we listed has estimated the gyro bias on the premises that the rate gyro exists, and its accumulated drift is controllable. If those premises are not true, another trend requiring attention is angular velocity reconstruction without applying rate gyros to obtain reasonable gyro bias estimation. Recently, relatively simple angular velocity estimation observers have been proposed by Magnis and Petit [30,31]. These observers reconstruct the angular velocity directly from the vector measurements, even a single vector measurement, without any attitude information or gyro measurements, and quickly convergence. However, users need to assume that all the torques applied to the rigid body are known. To solve this problem, Magnis et al. [32] extended their work to estimate torques even with varying rotation rates and unknown direction. Notably, these angular velocity estimation observers can make a positive contribution to gyro bias estimation.

We consider the attitude and gyro bias estimation using a rate gyro and two or more vector sensors. Built on the theory of Grip et al. [15] for cascaded nonlinear and linear systems, the primary contribution of this paper is to construct an interconnected nonlinear observer to estimate attitude and gyro bias from the re-estimated vector measurements. The proposed observer includes two cascaded subsystems: a nonlinear attitude and gyro bias observer modified from the nonlinear observer in [9], and a linear vector measurement observer devised to filter the vector measurements, called an auxiliary observer. This paper analyzes the proposed observer’s stability and gives its noisy error dynamics to study its robustness. When proving its robustness to bounded noise, the input-to-state stability theorem is used, and all the sensors’ noise is regarded. Beyond that, the time-varying gains computed from the discrete Riccati equation, for the auxiliary observer, is designed to improve its accuracy and robustness. This model also thinks about noise on all sensors.

We begin with the required sensor models and assumptions in Section 2. We design the globally interconnected observer and analyze its stability in Section 3. Section 4 briefly proves the proposed observer’s robustness to bounded noise. Section 5 discusses the results of the numerical simulation with several cases in detail. Section 6 concludes this paper.

Notation and Preliminaries

The operator ${(\cdot )}^T$ denotes the transpose of a vector or matrix; $\Vert \cdot \Vert$ represents the vectors’ Euclidean norm and the matrices’ Frobenius norm; and $\mathrm{E}[\cdot ]$ forms the expectation of its inputs. The symbols $I_{n\times n}$ and $0_{m\times n}$ denote the $n\times n$ identity matrix and the $m\times n$ zero element matrix, respectively. For a vector ${ℝ}^3$, defined as $r={[\begin{array}{ccc}{r}_1& r_2& r_3\end{array}]}^T$, its skew-symmetric is denoted by $S\left(r\right)$, where

$$S\left(r\right)=\left[\begin{array}{ccc}0& -r_3& r_2\\ r_3& 0& -r_1\\ -r_2& r_1& 0\end{array}\right],$$

(1)

so that for any $a\in {ℝ}^3$, $S\left(r\right)a=r\times a$, where $\times$ means the vector cross product. Given a linear function, $\mathrm{vex}(\cdot )$ denotes the inverse operation of $S\left(r\right)$, such that $\mathrm{vex}\left(S\left(r\right)\right)=r$. For a square matrix $U$, ${\mathbb{P}}_a\left(\mathit{U}\right)=\left(\mathit{U}-{\mathit{U}}^{\mathit{T}}\right)/2$ denotes its skew-symmetric part, $\mathrm{tr}\left(U\right)$ means its trace, $\left|\mathrm{tr}\left(U\right)\right|\le \sqrt{3}\Vert U\Vert$; for a symmetric matrix $U$, $\mathrm{tr}\left(US\left(r\right)\right)=0$. Furthermore, the trace of a skew-symmetric matrix is zero, and there exists $\mathrm{tr}\left(S^T\left(r\right)S\left(a\right)\right)=2r^{T}a$. A block-diagonal matrix $U_3$ of two square matrices $U_1$ and $U_2$ is indicated with $U_3=\mathrm{blkdiag}\left(U_1,U_2\right)$. The saturation operation of a vector or matrix argument to the interval $[-L,L]$ is performed by ${\mathrm{sat}}_L(\cdot )$. $\mathrm{Proj}(\cdot )$ is a projection operator whose function here is to preserve its inputs in a predefined bound.

In general, we need two coordinate frames to describe the attitude: the body-fixed frame and the reference frame. The body-fixed frame, denoted by $\left\{B\right\}$, is on the rigid body; its origin is rigorously fastened to the body’s center of mass. The reference frame is ordinarily unmoving, called an inertial frame $\left\{I\right\}$. Here, the north–east–down (NED) navigation frame, denoted by $\left\{N\right\}$, is used as the inertial frame, which means that the rotation of the Earth would be ignored in low-precision applications. $R_B^N\in SO\left(3\right)$ represents a rotation matrix from $\left\{B\right\}$ to $\left\{N\right\}$, where $\mathrm{SO}\left(3\right)$ is defined by $\mathrm{SO}\left(3\right):=\left\{U\in {ℝ}^{3\times 3}|\det \left(U\right)=1,\hspace{0.17em}U{U}^T=U^{T}U=I_3\right\}$. For convenience, let $R\equiv R_B^N$. Furthermore, it is known that a vector norm remains unchanged after rotation, i.e., $\Vert Ra\Vert =\Vert a\Vert$. By using superscript indexes to represent a vector decomposed in different coordinate frames, we have $x^N$ as the component of $x$ in frame $\left\{N\right\}$, and $x^B$ is the same. According to the definition of $R$, the relationship between the above vector components can be written as $x^N=R{x}^B$.

2. Problem Statement

2.1. Sensor Model

Consider a rigid body consisting of a triaxial rate gyro and two or more additional vector sensors, such as accelerometers and magnetometers. The attitude kinematics about the rotation matrix satisfy

$$\dot{R}=RS\left(\omega \right),$$

(2)

where $\omega \in {ℝ}^3$ is the angular velocity of $\left\{B\right\}$ with reference to $\left\{N\right\}$, expressed in $\left\{B\right\}$, and [33] gave its normal model:

$$\omega ={\omega }_m-b_{\omega }-{\eta }_{\omega },$$

(3)

$${\dot{b}}_{\omega }=0,$$

(4)

where ${\omega }_m$ is the outputs of the gyro; $b_{\omega }\in {ℝ}^3$ is a constant gyro bias vector; and ${\eta }_{\omega }$ is the sensor noise.

As described, at least two other sensors provide nonparallel vector measurements, so let $M\ge 2$, and suppose that two sets of $M$ vector measurements $\left\{v_i\in {ℝ}^3,i=1,\dots ,M\right\}$ and $\left\{v_{mi}\in {ℝ}^3,i=1,\dots ,M\right\}$ in the body-fixed frame are available. The measurement model is

$$v_{mi}=v_i+{\eta }_i,$$

(5)

where $v_{mi}\in {ℝ}^3,i=1,\dots ,M$ denotes the corresponding sensor outputs; and ${\eta }_i\in {ℝ}^3,i=1,\dots ,M$ is the sensor noise on measurement $i$. The vector measurement $v_i$, in the body-fixed frame, of the known constant vectors $v_i^N$, when expressed in the reference frame (NED frame), conform to the following relationship:

$$v_i=R^T{v}_i^N.$$

(6)

In this paper, vector measurements stand for a set of vectors measured in the body-fixed frame from other vector sensors, except for rate gyros, and satisfy Equation (6).

Omit the noisy terms in Equations (3)–(5) and substitute Equations (3) and (6) into Equations (2) and (5), respectively. We can get the noisy-free nonlinear system model as follows:

$$\{\begin{array}{l}\dot{R}=RS\left({\omega }_m-b_{\omega }\right)\\ {\dot{b}}_{\omega }=0\\ v_i=R^T{v}_i^N,\hspace{0.17em}i=1,\dots ,M\end{array},$$

(7)

where ${\omega }_m=\omega +b_{\omega }$ in this noiseless system. The following observer design also utilizes this noiseless system.

2.2. Assumptions

Throughout this work, consider the following assumptions:

Assumption 1.

At least two vectors in the body-fixed frame are not parallel to each other, i.e., there exists a constant $\gamma >0$, for all $t\ge 0$, there are $i,j\in 1,\dots ,M$ and $i\ne j$, such that $\Vert v_i\times v_j\Vert \ge \gamma >0$.

Assumption 2.

The gyro output signal ${\omega }_m$ and its derivative ${\dot{\omega }}_m$ are bounded for all $t\ge 0$.

Assumption 3.

The gyro bias $b_{\omega }$ is constant; there exists a known constant $l_b>0$, such that $\Vert b_{\omega }\Vert \le l_b$.

The first two assumptions are standard assumptions in attitude estimation; see, e.g., [12,13,17,20]. Assumption 1 is necessary to guarantee uniform observability. Assumption 3 is about gyro bias; also, we assume that $v_i^N,\hspace{0.17em}i=1,\dots ,M$ and ${\dot{v}}_i^N,\hspace{0.17em}i=1,\dots ,M$ are all continuous at $t$ and uniformly bounded.

In this paper, we choose two non-collinear vector measurements denoted by $v_1$ and $v_2$ in the body-fixed frame, corresponding to $v_1^N$ and $v_2^N$ in the inertial frame, where $v_1^N$ and $v_2^N$ are also independent of each other; the above assumptions are equally applicable.

3. Nonlinear Interconnected Observer Design

As shown in Figure 1, the proposed nonlinear interconnected observer includes two subsystems. The first subsystem is an attitude observer, which estimates the rotation matrix and gyro bias established on comparing at least two nonparallel vector measurements in two corresponding coordinate frames. If the attitude observer directly applies the vector sensors’ outputs as its input, the injection term will contain the vector measurement noise. This injection noise can change the accuracy of the attitude observer. Therefore, to reduce the impact of the measurement noise, we construct the second subsystem, an auxiliary observer for estimating the vector measurements and feeding the estimated value back to the attitude observer. The interconnected system ${\sum }_1-{\sum }_2$ is based on the theory of Grip in [15].

3.1. Subsystem ${\sum }_1$: Attitude Observer

The general framework of ${\sum }_1$ introduced here for the rotation matrix $R$ and gyro bias $b_{\omega }$ is similar to that in Grip et al. [18], expressed as

$${\sum }_1:\{\begin{array}{l}\dot{\widehat{R}}=\widehat{R}S\left({\omega }_m-{\widehat{b}}_{\omega }\right)+\theta K_P\Gamma \\ {\dot{\widehat{b}}}_{\omega }=\mathrm{Proj}\left(\Vert {\widehat{b}}_{\omega }\Vert \le l_{\widehat{b}},-k_v\mathrm{vex}\left({\mathbb{P}}_{\mathrm{a}}\left({\widehat{R}}_s^T{K}_P\Gamma \right)\right)\right)\end{array},$$

(8)

where $\theta$, $K_P$, and $k_v$ are tunable parameters, tuned to obtain stability. In more detail, $\theta \ge 1$ is a scaling factor; $K_P$ is a symmetric positive–definite gain matrix; and $k_v>0$ is a scalar gain. Function $\mathrm{Proj}(\cdot )$ denotes a parameter projection operator to ensure the estimated gyro bias is bounded by $\Vert \widehat{b}\Vert \le l_{\widehat{b}}$, see [9] for detail; and $l_{\widehat{b}}$ is a constant, $l_{\widehat{b}}>l_b$. ${\widehat{R}}_s={\mathrm{sat}}_1\left(\widehat{R}\right)$. Referencing the TRIAD algorithm [4], the injection term $\Gamma$ is defined by

$$\Gamma \left(v_1,v_1^N,v_2,v_2^N,\widehat{R}\right):=A_N{A}_B^T-A_N{A}_N^T\widehat{R},$$

(9)

$$A_B:=\left[\begin{array}{ccc}{v}_1& v_2& v_1\times v_2\end{array}\right],$$

(10)

$$A_N:=\left[\begin{array}{ccc}{v}_1^N& v_2^N& v_1^N\times v_2^N\end{array}\right],$$

(11)

which is different from the design strategy of $J:=A_N{A}_B^T-\widehat{R}{A}_B{A}_B^T$ in [18]. Note that $A_B$ and $A_N$ are defined to fulfill Assumption 1. Besides, in the attitude observer, the rotation matrix $\widehat{R}$ does not strictly adhere to the topological structure of $\mathrm{SO}\left(3\right)$.

Property 1.

For all $t>0$, $A_B$ and $A_N$ are piecewise continuous in $t$ and uniformly bounded by$\Vert A_N\Vert =\Vert A_B\Vert \le L_A$. For all $t>0$, there is a constant ${\beta }_1>0$, such that $A_N{A}_N^{{\rm T}}\ge {\beta }_1{I}_{3\times 3}$.

Define the estimation errors of the rotation matrix and gyro bias as $\tilde{R}=R-\widehat{R}$, ${\tilde{b}}_{\omega }=b_{\omega }-{\widehat{b}}_{\omega }$. The error dynamics can be written as

$$\{\begin{array}{l}\dot{\tilde{R}}=RS\left({\omega }_m-b_{\omega }\right)-\widehat{R}S\left({\omega }_m-{\widehat{b}}_{\omega }\right)-\theta K_P\Gamma \\ {\dot{\tilde{b}}}_{\omega }=-\mathrm{Proj}\left({\widehat{b}}_{\omega },-k_v\mathrm{vex}\left({\mathbb{P}}_{\mathrm{a}}\left({\widehat{R}}_s^T{K}_P\Gamma \right)\right)\right)\end{array}.$$

(12)

Lemma 1.

For any choice of $K_P>0$ and $k_v>0$, there exists a constant ${\theta }^{*}\ge 1$, such that, for all $\theta \ge {\theta }^{*}$, the origin $\tilde{R}=R-\widehat{R}=0$ and ${\tilde{b}}_{\omega }=b_{\omega }-{\widehat{b}}_{\omega }=0$ of Equation (12) is global exponential stability (GES).

Proof. 

Having that $A_B=R^T{A}_N$, we can rewrite $\Gamma =A_N{A}_N^T\tilde{R}$, thus $\Vert \Gamma \Vert =\Vert A_N{A}_N^T\tilde{R}\Vert \le {\Vert A_N\Vert }^2\Vert \tilde{R}\Vert \le L_A^2\Vert \tilde{R}\Vert$. Hence, $\Vert \Gamma \Vert$ in the present observer also remains linear in $\Vert \tilde{R}\Vert$, so the remaining proof is analogous to [18]. □

To reduce the vector measurement noise injection, we utilized the estimated vector measurements instead of the output signals of the vector sensors. Therefore we replaced every occurrence of $v_1$ and $v_2$ in $A_B$ by the estimated states ${\widehat{v}}_1$ and ${\widehat{v}}_2$, using ${\widehat{A}}_B$ to replace $A_B$ in term $\Gamma$. Thus, $\widehat{\Gamma }$ and ${\widehat{A}}_B$ take the following forms:

$$\widehat{\Gamma }\left({\widehat{v}}_1,v_1^N,{\widehat{v}}_2,v_2^N,\widehat{R}\right):=A_N{\widehat{A}}_B^T-A_N{A}_N^T\widehat{R},$$

(13)

$${\widehat{A}}_B=\left[\begin{array}{ccc}{\widehat{v}}_1& {\widehat{v}}_2& {\widehat{v}}_1\times {\widehat{v}}_2\end{array}\right].$$

(14)

Consequently, with the feedback, the error dynamics of ${\sum }_1$ are

$${\sum }_1:\{\begin{array}{l}\dot{\widehat{R}}=\widehat{R}S\left({\omega }_m-{\widehat{b}}_{\omega }\right)+\theta K_P\widehat{\Gamma }\\ {\dot{\widehat{b}}}_{\omega }=\mathrm{Proj}\left(\Vert {\widehat{b}}_{\omega }\Vert \le l_{\widehat{b}},-k_v\mathrm{vex}\left({\mathbb{P}}_{\mathrm{a}}\left({\widehat{R}}_s^T{K}_P\widehat{\Gamma }\right)\right)\right)\end{array}.$$

(15)

Notice that, in Equation (9), $J$ has been changed by $\Gamma$ to facilitate subsequent derivation, because the second term of $\Gamma$ is to ensure that $\Vert \tilde{\Gamma }\Vert$ is a linearity to $\Vert {\tilde{A}}_B\Vert$, where $\tilde{\Gamma }:=\Gamma -\widehat{\Gamma }$ and ${\tilde{A}}_B:=A_B-{\widehat{A}}_B$.

3.2. Subsystem ${\sum }_2$: Auxiliary Observer

To weaken the injection of vector measurement noise in ${\sum }_1$, we determine that the vector measurements are not available as the input of ${\sum }_1$, as seen in Figure 2. Consequently, in this stage, we design an auxiliary observer whose role is to estimate the vector measurements for feeding the attitude observer. Since $v_1^N$ and $v_2^N$ are constants, from Equations (5) and (6), the derivative of the vector measurements is given by

$$\{\begin{array}{l}{\dot{v}}_1=v_1\times \omega \\ {\dot{v}}_2=v_2\times \omega \end{array}.$$

(16)

Combine the above equations with the measurement equation, Equation (6), and let $i=1,2$. Then, to estimate the states of the linear system in Equation (16), give the observer in a noiseless scenario:

$${\sum }_2:\{\begin{array}{l}{\dot{\widehat{v}}}_1={\widehat{v}}_1\times \left({\omega }_m-{\widehat{b}}_{\omega }\right)+K_1\left(v_1-{\widehat{v}}_1\right)\\ {\dot{\widehat{v}}}_2={\widehat{v}}_2\times \left({\omega }_m-{\widehat{b}}_{\omega }\right)+K_2\left(v_2-{\widehat{v}}_2\right)\end{array},$$

(17)

where the gain matrices $K_1$ and $K_2$ are positive. Notice that the auxiliary observer is closely related to that introduced by Martin et al. [22]. The main difference between them is that the auxiliary observer uses the estimated gyro bias as an input rather than an estimated state, and the error system of ${\sum }_2$ is linear. Compared with the extended linear cascade subsystem in [15], our framework can be viewed as a simplified version because it only contains the additional items.

What is noteworthy is that the structure of the auxiliary observer ${\sum }_2$ makes it possible to use time-varying reference vectors in the inertial frame, since the knowledge of the constant vectors $v_1^N$ and $v_2^N$ is not employed.

3.3. Stability Analysis

To analyze the stability, define the estimation errors of ${\sum }_2$ as ${\tilde{v}}_1=v_1-{\widehat{v}}_1$, ${\tilde{v}}_2=v_2-{\widehat{v}}_2$. The error dynamics of system ${\sum }_2$ are

$$\{\begin{array}{l}{\dot{\tilde{v}}}_1=-\left({\omega }_m-{\widehat{b}}_{\omega }\right)\times {\tilde{v}}_1-K_1{\tilde{v}}_1+{\tilde{d}}_1\\ {\dot{\tilde{v}}}_2=-\left({\omega }_m-{\widehat{b}}_{\omega }\right)\times {\tilde{v}}_2-K_2{\tilde{v}}_2+{\tilde{d}}_2\end{array},$$

(18)

where $\tilde{d}={[{\tilde{d}}_1,{\tilde{d}}_2]}^T={[-v_1\times {\tilde{b}}_{\omega },-v_2\times {\tilde{b}}_{\omega }]}^T$ is viewed as an input. Since $v_1$ and $v_2$ are uniformly bounded and $\Vert S\left(v_1\right)\Vert =\sqrt{2}\Vert v_1\Vert$, then $\Vert \tilde{d}\Vert \le \sqrt{2}{\left({\Vert v_1\Vert }^2{\Vert {\tilde{b}}_{\omega }\Vert }^2+{\Vert v_2\Vert }^2{\Vert {\tilde{b}}_{\omega }\Vert }^2\right)}^{1/2}\le L_1\Vert {\tilde{b}}_{\omega }\Vert$ for some $L_1>0$, which satisfies Assumption 3 in [15]. Let $\tilde{v}={[{\tilde{v}}_1,{\tilde{v}}_2]}^T$ and transform the error dynamics in Equation (18) into a linear time-varying (LTV) error system:

$$\dot{\tilde{v}}=\left(A-KC\right)\tilde{v}+B\tilde{d},$$

(19)

where $A=\left[\begin{array}{cc}-S\left({\omega }_m-{\widehat{b}}_{\omega }\right)& 0_{3\times 3}\\ 0_{3\times 3}& -S\left({\omega }_m-{\widehat{b}}_{\omega }\right)\end{array}\right]$, $B=\left[\begin{array}{cc}{I}_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& I_{3\times 3}\end{array}\right]$, $C=\left[\begin{array}{cc}{I}_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& I_{3\times 3}\end{array}\right]$, $K=\left[\begin{array}{cc}{K}_1& 0_{3\times 3}\\ 0_{3\times 3}& K_2\end{array}\right]$. To guarantee the stability of the observer error dynamics, $K$ must be selected to make sure that the matrix $A-KC$ is Hurwritz, as stated in Theorem 1 below.

According to the definition of $\widehat{\Gamma }$ in the present paper, the error dynamics of ${\sum }_1$ are

$$\{\begin{array}{l}\dot{\tilde{R}}=RS\left({\omega }_m-b_{\omega }\right)-\widehat{R}S\left({\omega }_m-{\widehat{b}}_{\omega }\right)-\theta K_P\widehat{\Gamma }\\ {\dot{\tilde{b}}}_{\omega }=-\mathrm{Proj}\left({\widehat{b}}_{\omega },-k_v\mathrm{vex}\left({\mathbb{P}}_{\mathrm{a}}\left({\widehat{R}}_s^T{K}_P\widehat{\Gamma }\right)\right)\right)\end{array}.$$

(20)

Theorem 1.

Let $H_K\left(s\right)={\left(Is-A+KC\right)}^{-1}B$, and let $\theta$ be chosen to warrant stability. There exists a $\kappa >0$, such that if $K$ is chosen to assure that $A-KC$ is Hurwritz and ${\Vert H_K\left(s\right)\Vert }_{\infty }<\kappa$, then the origin of Equations (19) and (20) are GES. Moreover, a $K$ that fulfills these conditions can be found all the time.

Proof. 

This proof acts similarly to [18]. First, $\left(A,C\right)$ and $\left(A,B\right)$ are easily verified to be observable and controllable, respectively. The pair $\left(A,B,C\right)$ is left-invertible. According to the properties of the skew-symmetric matrix, we can infer that $\left(A,B,C\right)$ is the minimum phase. As mentioned above, $K$ should be chosen to fulfill the conditions of Theorem 1, and the same as for proof of Lemma 1, $\Vert {\widehat{b}}_{\omega }\Vert$ must be in the definition domain of $\Vert {\widehat{b}}_{\omega }\Vert \le l_{\widehat{b}}$. Next, choosing the same Lyapunov function candidate $V$ as in [18], we get the equation of $\dot{V}$, which is

$$\begin{array}{ll}\dot{V}\le & -{\beta }_2\left({\Vert \tilde{R}\Vert }^2+{\Vert {\tilde{b}}_{\omega }\Vert }^2\right)+\mathrm{tr}\left({\tilde{R}}^T\theta K_P\tilde{\Gamma }\right)\\ & -\mu \mathrm{tr}\left(S\left(\mathrm{Proj}\left({\widehat{b}}_{\omega },\tau \left(\Gamma \right)\right)-\mathrm{Proj}\left({\widehat{b}}_{\omega },\tau \left(\widehat{\Gamma }\right)\right)\right)R^T\tilde{R}\right)\\ & -\mu \mathrm{tr}\left(S\left({\tilde{b}}_{\omega }\right)R^T\theta K_P\tilde{\Gamma }\right)\\ & +\frac{2\theta \mu }{k_v}{\tilde{b}}_{\omega }^T\left(\mathrm{Proj}\left({\widehat{b}}_{\omega },\tau \left(\Gamma \right)\right)-\mathrm{Proj}\left({\widehat{b}}_{\omega },\tau \left(\widehat{\Gamma }\right)\right)\right)\end{array}$$

(21)

where $\tau \left(\Gamma \right)=-k_v\mathrm{vex}\left({\mathbb{P}}_{\mathrm{a}}\left({\widehat{R}}_s^T{K}_P\Gamma \right)\right)$, and recall that $\tilde{\Gamma }=\Gamma -\widehat{\Gamma }$. Write $\tilde{\Gamma }=A_N{(A_B-{\widehat{A}}_B)}^T=A_N{\tilde{A}}_B{}^T$; it follows that $\Vert \tilde{\Gamma }\Vert \le \Vert A_N\Vert \Vert {\tilde{A}}_B\Vert$, where $\Vert A_N\Vert \le L_A$; see Property 1. Using the inequality ${\Vert v_1\times v_2\Vert }^2\le 2\sqrt{2}\left({\Vert v_1\Vert }^2+{\Vert v_2\Vert }^2\right)$ derived from the inequality in [34], we can bound $\Vert {\tilde{A}}_B\Vert$ in the following way: Let ${\tilde{A}}_B=A_B-{\widehat{A}}_B=[\begin{array}{ccc}{v}_1-{\widehat{v}}_1& v_2-{\widehat{v}}_2& v_1\times v_2-{\widehat{v}}_1\times {\widehat{v}}_2\end{array}]$, ${\tilde{v}}_1=v_1-{\widehat{v}}_1$, and ${\tilde{v}}_2=v_2-{\widehat{v}}_2$, then we have

$$\begin{array}{ll}\Vert S\left(v_1\right)v_2-S\left({\widehat{v}}_1\right){\widehat{v}}_2\Vert & =\Vert S\left(v_1\right)v_2-S\left(v_1-{\tilde{v}}_1\right)\left(v_2-{\tilde{v}}_2\right)\Vert \\ & =\Vert S\left(v_1\right){\tilde{v}}_2+S\left({\tilde{v}}_1\right)v_2-S\left({\tilde{v}}_1\right)\left({\tilde{v}}_2\right)\Vert \\ & \le \Vert S\left(v_1\right){\tilde{v}}_2\Vert +\Vert S\left({\tilde{v}}_1\right)v_2\Vert +\Vert S\left({\tilde{v}}_1\right)\left({\tilde{v}}_2\right)\Vert \\ & \le \sqrt{2}\left(\Vert v_1\Vert \Vert {\tilde{v}}_2\Vert +\Vert {\tilde{v}}_1\Vert \Vert v_2\Vert +2{\left({\Vert {\tilde{v}}_1\Vert }^2+{\Vert {\tilde{v}}_2\Vert }^2\right)}^{1/2}\right)\\ & \le \sqrt{2}\left(h_1\Vert {\tilde{v}}_2\Vert +h_2\Vert {\tilde{v}}_1\Vert +2{\left({\Vert {\tilde{v}}_1\Vert }^2+{\Vert {\tilde{v}}_2\Vert }^2\right)}^{1/2}\right)\\ & \le h_3{\left({\Vert {\tilde{v}}_1\Vert }^2+{\Vert {\tilde{v}}_2\Vert }^2\right)}^{1/2}\end{array},$$

(22)

for some constant $h_3>0$, where $h_1$ and $h_2$ are bound on $v_1$ and $v_2$, respectively. Then the norm of ${\tilde{A}}_B$ becomes

$$\begin{array}{ll}\Vert {\tilde{A}}_B\Vert & ={\left({\Vert {\tilde{v}}_1\Vert }^2+{\Vert {\tilde{v}}_2\Vert }^2+{\Vert S\left(v_1\right)v_2-S\left({\widehat{v}}_1\right){\widehat{v}}_2\Vert }^2\right)}^{1/2}\\ & \le {\left({\Vert {\tilde{v}}_1\Vert }^2+{\Vert {\tilde{v}}_2\Vert }^2+h_3^2\left({\Vert {\tilde{v}}_1\Vert }^2+{\Vert {\tilde{v}}_2\Vert }^2\right)\right)}^{1/2}\\ & \le {\gamma }_1{\left({\Vert {\tilde{v}}_1\Vert }^2+{\Vert {\tilde{v}}_2\Vert }^2\right)}^{1/2}\end{array}$$

(23)

for some ${\gamma }_1>0$. Given $\Vert \tilde{v}\Vert ={\left({\Vert {\tilde{v}}_1\Vert }^2+{\Vert {\tilde{v}}_2\Vert }^2\right)}^{1/2}$, it is easy to get that $\Vert \tilde{\Gamma }\Vert \le \Vert A_N\Vert \Vert {\tilde{A}}_B\Vert \le L_A{\gamma }_1\Vert \tilde{v}\Vert$. Hence, $\Vert \theta K_P\tilde{\Gamma }\Vert \le \theta L_A{\gamma }_1\Vert K_P\Vert \Vert \tilde{v}\Vert$. With the additional properties from Lemma 3 in [9], such that $\Vert \mathrm{Proj}\left({\widehat{b}}_{\omega },\tau \right)\Vert \le \tau$, $\Vert \mathrm{vex}\left({\mathbb{P}}_a\left(U\right)\right)\Vert \le \Vert U\Vert /\sqrt{2}$, there exists a ${\gamma }_2>0$, such that $\Vert \tau \left(\Gamma \right)\Vert -\Vert \tau \left(\widehat{\Gamma }\right)\Vert \le {\gamma }_2\Vert \tilde{v}\Vert$. Likewise, there is a constant ${\gamma }_3>0$, such that $\Vert \mathrm{Proj}\left(\widehat{b},\tau \left(\Gamma \right)\right)-\mathrm{Proj}\left(\widehat{b},\tau \left(\widehat{\Gamma }\right)\right)\Vert \le {\gamma }_3\Vert \tilde{v}\Vert$. Then $\dot{V}$ becomes

$$\begin{array}{ll}\dot{V}& \le -{\beta }_2\left({\Vert \tilde{R}\Vert }^2+{\Vert {\tilde{b}}_{\omega }\Vert }^2\right)+\sqrt{3}\theta \Vert K_P\Vert \left|{\gamma }_1\right|\Vert \tilde{R}\Vert \Vert \tilde{v}\Vert \\ & \hspace{0.17em}\hspace{0.17em}\hspace{0.17em}+\sqrt{6}\mu {\gamma }_3\Vert \tilde{R}\Vert \Vert \tilde{v}\Vert +\sqrt{6}\mu \theta \Vert K_P\Vert \left|{\gamma }_1\right|\Vert {\tilde{b}}_{\omega }\Vert \Vert \tilde{v}\Vert +\frac{2\theta \mu {\gamma }_3}{k_v}\Vert {\tilde{b}}_{\omega }\Vert \Vert \tilde{v}\Vert \\ & \le -{\beta }_2{\xi }^2+{\gamma }_4\xi \Vert \tilde{v}\Vert \end{array}$$

(24)

for some ${\gamma }_4>0$, ${\beta }_2>0$, where $\xi :={\left({\Vert \tilde{R}\Vert }^2+{\Vert {\tilde{b}}_{\omega }\Vert }^2\right)}^{1/2}$. □

Following the proof of Lemma 2 in [9], and using the same function $W={\tilde{v}}^{T}P\tilde{v}$ for some positive definite matrix $P$, one gets $\dot{W}\le -{\Vert \tilde{v}\Vert }^2+{\kappa }^2{\Vert \tilde{d}\Vert }^2$. From the above analysis we can obtain $\Vert \tilde{d}\Vert \le L_1\Vert {\tilde{b}}_{\omega }\Vert \le L_1\left(\Vert \tilde{R}\Vert +\Vert {\tilde{b}}_{\omega }\Vert \right)$. Thus, $\dot{W}\le -{\Vert \tilde{v}\Vert }^2+{\kappa }^2{\gamma }_5^2{\xi }^2$ for some ${\gamma }_5>0$. Considering the Lyapunov function $Y=W+gV$, we get $\dot{Y}<0$ for all adequately small $\kappa$. Therefore, this error system is GES.

3.4. Gain Selection

The task of this section is to choose the tunable gains. $K_P$, $k_v$ and $\theta$ of the attitude observer are chosen to conform to Lemma 1, and $K$ of the auxiliary observer should be selected to secure the total observer’s stability. In practice, one can first choose arbitrary $K_P$, $k_v$, and then tune other gains to achieve stability, of which $K_P$ and $K$ can be selected constants for improving the computational efficiency.

Additionally, according to the structure of Equation (18), we can find that it is straightforward to design time-varying gains for the auxiliary observer. One solution is to resolve the discrete time-varying Ricatti equation. This solution is similar to Kalman filter equations; the details are similar as in [25]. Furthermore, the utility of this method is that the noise and small interference items of the vector sensors can be under consideration. To make the derivations easier, we substitute $v_i,\hspace{0.17em}i=1,2$ with $v_i+n_i,\hspace{0.17em}i=1,2$ and replace ${\omega }_m$ with ${\omega }_m+{\eta }_{\omega }$ in Equation (17), like [28], to get the dynamics of the noisy auxiliary observer ${{\sum }^{\prime }}_2$

$${{\sum }^{\prime }}_2:\{\begin{array}{l}{\dot{\widehat{v}}}_1={\widehat{v}}_1\times \left({\omega }_m+{\eta }_{\omega }-{\widehat{b}}_{\omega }\right)+K_1\left(v_1+{\eta }_1-{\widehat{v}}_1\right)\\ {\dot{\widehat{v}}}_2={\widehat{v}}_2\times \left({\omega }_m+{\eta }_{\omega }-{\widehat{b}}_{\omega }\right)+K_2\left(v_2+{\eta }_2-{\widehat{v}}_2\right)\end{array}.$$

(25)

Then, from Equations (25) and (16), the estimation error dynamics of ${{\sum }^{\prime }}_2$ are

$$\{\begin{array}{l}{\dot{\tilde{v}}}_1={\dot{v}}_1-{\dot{\widehat{v}}}_1=-S\left({\omega }_m-{\widehat{b}}_{\omega }\right){\tilde{v}}_1-K_1{\tilde{v}}_1+{\tilde{d}}_1+S\left({\widehat{v}}_1\right){\eta }_{\omega }-K_1{\eta }_1\\ {\dot{\tilde{v}}}_2={\dot{v}}_2-{\dot{\widehat{v}}}_2=-S\left({\omega }_m-{\widehat{b}}_{\omega }\right){\tilde{v}}_2-K_2{\tilde{v}}_2+{\tilde{d}}_2+S\left({\widehat{v}}_2\right){\eta }_{\omega }-K_2{\eta }_2\end{array}.$$

(26)

Recall that $\tilde{d}={[{\tilde{d}}_1,{\tilde{d}}_2]}^T={[-v_1\times {\tilde{b}}_{\omega },-v_2\times {\tilde{b}}_{\omega }]}^T$, $\tilde{v}={[{\tilde{v}}_1,{\tilde{v}}_2]}^T$, given an input $u$, and $u=\tilde{d}$; thus, the LTV error system with noise terms is

$$\dot{\tilde{v}}=\left(A-KC\right)\tilde{v}+Bu+B{\eta }_u-K{\eta }_v,$$

(27)

where $K$ and the $\left(A,B,C\right)$ matrices are the same as before, ${\eta }_u$ is correlated with gyro noise, which gives ${\eta }_u={[S({\widehat{v}}_1){\eta }_{\omega },S({\widehat{v}}_2){\eta }_{\omega }]}^T$. ${\eta }_v$ is driven by vector measurement noise, having ${\eta }_v={[{\eta }_1,{\eta }_2]}^T$. As stated in Assumption 1, the two vector measurements are noncorrelated, thus defining the process noise covariance matrix and measurement noise covariance matrix as $Q=\mathrm{blkdiag}\left(Q_1,Q_2\right)$ and $R_v=\mathrm{blkdiag}\left(R_{v1},R_{v2}\right)$, respectively. Regarding $Q$, $Q_1=\mathrm{E}[S({\widehat{v}}_1){\eta }_{\omega }{\eta }_{\omega }^T{S}^T({\widehat{v}}_1)]$ and $Q_2=\mathrm{E}[S({\widehat{v}}_2){\eta }_{\omega }{\eta }_{\omega }^T{S}^T({\widehat{v}}_2)]$, which are obtained from the gyro measurement noise. Concerning $R$, the matrices $R_{v1}$ and $R_{v2}$ are from the vector measurement noise by $R_v{}_1=\mathrm{E}[{\eta }_1{\eta }_1^T]$ and $R_{v2}=\mathrm{E}[{\eta }_2{\eta }_2^T]$. For computing $K$, the following discrete Kalman filtering formulas can be used:

$$P_{k|k-1}=F_k{P}_{k-1|k-1}{F}_k^T+Q_k$$

(28)

$$K_k=P_{k|k-1}{C}_k^T{\left(C_k{P}_{k|k-1}{C}_k^T+R_{vk}\right)}^{-1}$$

(29)

$$P_{k|k}=\left(I-K_k{C}_k\right)P_{k|k-1}$$

(30)

where ${(\cdot )}_k$ means the discrete matrix at time $t_k$. Given the discretization of the state matrices, $F_k=e^{A\mathsf{\Delta }t}$ and $Q_k={\displaystyle {\int }_{t_k}^{t_{k+1}}{e}^{A\mathsf{\Delta }t}BQ{B}^T{e}^{A^T\mathsf{\Delta }t}\mathrm{d}s}$, in which $\mathsf{\Delta }t=t_{k+1}-t_k$ is the sampling interval. $P_{k|k-1}$ and $P_k$ denote the covariance of the estimated error $\tilde{v}$. In addition to producing time-varying gains, another advantage of this method is that it regards vector measurement noise at any time, and in some cases, smaller interference items can also be regarded as noise items.

4. Robustness to Noise

All analyses of the stability in Section 3.3 are performed under the assumption that there is a noise-free scenario. In fact, all sensors’ measurements cannot avoid noise interference. Our main work of this section is to analyze the observer’s robustness to the noise on sensor measurements. Toward this end, assume that all noise is bounded. We start with the noise of the rate gyro and vector sensors under consideration by substituting ${\omega }_m+{\eta }_{\omega }$ for ${\omega }_m$, $v_i+n_i,\hspace{0.17em}i=1,2$ for $v_i,\hspace{0.17em}i=1,2$. Hence, in the presence of bounded noise, the dynamics of the attitude observer Equation (8) are replaced with

$${{\sum }^{\prime }}_1:{\{\begin{array}{l}\dot{\widehat{R}}=\widehat{R}S\left({\omega }_m+{\eta }_{\omega }-{\widehat{b}}_{\omega }\right)+\theta K_P\overline{\Gamma }\\ {\dot{\widehat{b}}}_{\omega }=\mathrm{Proj}\left(\Vert {\widehat{b}}_{\omega }\Vert \le l_{\widehat{b}},-k_v\mathrm{vex}\left({\mathbb{P}}_{\mathrm{a}}\left({\widehat{R}}_s^T{K}_P\overline{\Gamma }\right)\right)\right)\end{array}}_{{}_{}},$$

(31)

where $\overline{\Gamma }$ can be written as

$$\overline{\Gamma }\left(v_1+{\eta }_1,v_1^N,v_2+{\eta }_2,v_2^N,\widehat{R}\right):=A_N{\overline{A}}_B^T-A_N{A}_N^T\widehat{R},$$

(32)

and ${\overline{A}}_B$ is denoted by

$${\overline{A}}_B=\left[\begin{array}{ccc}{v}_1+{\eta }_1& v_2+{\eta }_2& \left(v_1+{\eta }_1\right)\end{array}\times \left(v_2+{\eta }_2\right)\right].$$

(33)

Then the error dynamics are

$$\{\begin{array}{l}\dot{\tilde{R}}=RS\left({\omega }_m-b_{\omega }\right)-\widehat{R}S\left({\omega }_m+{\eta }_{\omega }-{\widehat{b}}_{\omega }\right)-\theta K_P\overline{\Gamma }\\ {\dot{\tilde{b}}}_{\omega }=-\mathrm{Proj}\left({\widehat{b}}_{\omega },-k_v\mathrm{vex}\left({\mathbb{P}}_{\mathrm{a}}\left({\widehat{R}}_s^T{K}_P\overline{\Gamma }\right)\right)\right)\end{array}.$$

(34)

Note that the noisy system dynamics and noisy error dynamics of the auxiliary observer are given by Equations (25) and (26).

Assumption 4.

For all $t\ge 0$, the sensor measurement noise ${\eta }_{\omega }$ and ${\eta }_i,\hspace{0.17em}i=1,2$ are all uniformly bounded.

Theorem 2.

Under the condition of Lemma 1, consider the attitude observer Equation (8), and let the input $u_1={[{\eta }_{\omega },{\eta }_v]}^T$ meet Assumption 4, then the attitude observer error dynamics Equation (34) are input-to-state stable.

Proof. 

See Appendix A. □

Theorem 3.

Under the condition of Theorem 1, consider the auxiliary observer Equation (17), and let the input $u_2={[{\tilde{b}}_{\omega },{\eta }_u,{\eta }_v]}^T$ meet Assumption 4, then the attitude observer error dynamics Equation (25) are input-to-state stable.

Proof. 

See Appendix B. □

Remark 1.

The premise of Theorem 3 is that the gyro bias estimation errors are bounded under Theorem 2.

5. Simulation Results and Discussion

In this section, via a simulated attitude estimation system, we evaluated the proposed nonlinear interconnected observer (named NLIO) by comparing it with the nonlinear observer (NLO) proposed by Grip et al. [18], the cascade observer (NLCO) discussed in [28], and the multiplicative extended Kalman filter (MEKF) [35]. With fixed gains and time-varying gains, we called the proposed observer NLIO-FG and NLIO-TV, respectively. Consider the attitude and heading reference system (AHRS) that consists of a triaxial rate gyro and two triaxial vector sensors: an accelerometer and a magnetometer. All sensors were low-cost and had a frequency of 100 Hz. The attitude trajectory in each simulation was driven by $\omega ={[0.1\sin \left(\left(\pi /12\right)t\right),-0.2\cos \left(\left(\pi /10\right)t\right),0.1\sin \left(\left(\pi /12\right)t\right)]}^T$, and lasted 500 s. Besides, the rate gyro bias was $b_{\omega }={[-0.017,-0.017,0.017]}^T\mathrm{rad}/\mathrm{s}$. Assume that the gravitational and magnetic fields are available, denoted as $g^N={[0,\hspace{0.17em}0,\hspace{0.17em}\mathrm{g}]}^T$ and $ma{g}^N={[0.3128,\hspace{0.17em}0,\hspace{0.17em}0.4282]}^T\mathrm{G}$, where $\mathrm{g}=9.81\mathrm{m}/{\mathrm{s}}^2$. All sensor noise was assumed as Gaussian white noise, and the rate gyro noise followed ${\eta }_{\omega }\sim N(0,\hspace{0.17em}{({10}^{-3}\mathrm{rad}/\mathrm{s})}^2)$. Here, two simulations were conducted to illustrate the performance of our proposed observer. We carried out 100 independent runs with each case and calculated the mean-absolute error (MAE) and root-mean-squared error (RMSE) of the Euler angles to assess the estimation accuracy. Note that, in this paper, we assumed that the acceleration of the rigid body is quite small and negligible compared with the gravity vector.

5.1. Simulation A: Comparsion of NLIO and NLO

In this section, we aim to compare the performance of the NLO and the NLIO, because the attitude observer frameworks of the NLO and the NLIO have many similarities, and the NLIO can be seen as a development of the NLO. Four cases are given as follows, with different initial attitude values and vector sensor noise.

In all four cases, we established two sets of tuning gains for attitude observers in the NLO and NLIO. These were $K_P=15I_{3\times 3}$(for NLO, NLIO-FG, and NLIO-TV) and $K_P=5I_{3\times 3}$(for NLO-a and NILO-TV-a). The scaling gain of the gyro bias observer was fixed to $k_v=0.2I_{3\times 3}$; the scaling parameter $\theta$ was fixed to $\theta =1$. Meanwhile, the initial covariance matrix of the NLIO-TV was $P_{0|0}=\mathrm{blkdiag}\left({10}^{-5}{I}_{3\times 3},\hspace{0.17em}5\times {10}^{-7}{I}_{3\times 3}\right)$.

Case 1.

The initial attitude was chosen randomly from a uniform distribution $[-\pi ,\hspace{0.17em}\pi ]$, and the initial gyro bias was ${[0,\hspace{0.17em}0,\hspace{0.17em}0]}^T\mathrm{rad}/\mathrm{s}$. The noise of the accelerometer and magnetometer was set to ${\eta }_1\sim N(0,\hspace{0.17em}{(5\times {10}^{-3}\mathrm{g})}^2)$ and ${\eta }_2\sim N(0,\hspace{0.17em}{(8\times {10}^{-3}\mathrm{G})}^2)$, respectively. In this simulation, there were random initial attitude values for all observers. The cascaded auxiliary observer gains of the NLIO-FG were chosen as $K_1=5.6I_{3\times 3}$, $K_2=3.3I_{3\times 3}$.

From Figure 3, we can find that the auxiliary observers of the NLIOs can weaken the measurement noise and has good convergence, and the auxiliary observer with time-varying gains has better performance. Figure 4 and Figure 5 show the attitude and gyro bias estimation errors for each observer, respectively. These figures illustrate that the NLIO-TV slightly outperforms other observers by applying the time-varying gains to the auxiliary observer and using the estimated vector measurements as the attitude observer’s inputs. With the random initial attitude in $[-\pi ,\hspace{0.17em}\pi ]$, we can see all observers can converge fast in the initial seconds of the transient phase, especially the NLIOs.

Table 1. MAEs of attitude for all the nonlinear observers in Case 1.

	Transient (0–200 s)			Steady-State (300–500 s)
	Roll (deg)	Pitch (deg)	Yaw (deg)	Roll (deg)	Pitch (deg)	Yaw (deg)
NLO	0.6196	0.0985	0.3662	0.1149	0.0748	0.3495
NLO-a	1.0376	0.1233	0.2557	0.0920	0.0510	0.2015
NLIO-FG	0.6499	0.1200	0.2095	0.1661	0.0778	0.1689
NLIO-TV	0.6180	0.0875	0.2051	0.0930	0.0515	0.1661

and

Table 2. RMSEs of attitude for all the nonlinear observers in Case 1.

	Transient (0–200 s)			Steady-State (300–500 s)
	Roll (deg)	Pitch (deg)	Yaw (deg)	Roll (deg)	Pitch (deg)	Yaw (deg)
NLO	1.0823	1.0171	0.1104	0.0936	0.4379	0.0933
NLO-a	1.7734	1.4817	0.1332	0.0635	0.2524	0.0772
NLIO-FG	1.0923	0.9448	0.1786	0.0961	0.2110	0.1450
NLIO-TV	1.0825	0.9442	0.1077	0.0641	0.2077	0.0776

give the MAE values and RMSE values of the Euler angles, which represent the transient and steady-state performance of all observers. It is indicated that under the same attitude observer gains, the performance of the NLIO-FG is only better than the NLO in yaw. This means that a single auxiliary observer gain cannot meet the higher accuracy requirements of all the Euler angles.

Case 2.

The initial attitude and gyro bias were ${[0,\hspace{0.17em}0,\hspace{0.17em}0]}^T\mathrm{rad}$ and ${[0,\hspace{0.17em}0,\hspace{0.17em}0]}^T\mathrm{rad}/\mathrm{s}$; there was mixed-Gaussian noise of the accelerometer and magnetometer, which follow ${\eta }_1\sim 0.8N(0,\hspace{0.17em}{(5\times {10}^{-3}\mathrm{g})}^2)+0.2N(0,\hspace{0.17em}{(5\times {10}^{-2}\mathrm{g})}^2)$, ${\eta }_2\sim 0.8N(0,\hspace{0.17em}{(8\times {10}^{-3}\mathrm{G})}^2)+0.2N(0,\hspace{0.17em}{(8\times {10}^{-2}\mathrm{G})}^2)$.

In this case, we assumed the vector measurement noise follows a mixed Gaussian distribution. The proposed observer gains were $K_1=4.7I_{3\times 3}$ and $K_2=1.5I_{3\times 3}$. We can find in Figure 6 that the auxiliary observers of the NLIOs can minimize the measurement noise to a level similar to that made in Case 1. However, there is high-level noise on the vector sensors here compared to Case 1. Figure 7 and Figure 8 show that the NLIOs have better performance than the NLOs with identical $K_P$; obviously, with different tuning, the NLIO-TV is better than the NLO-a, which has an unaggressive $K_P$, performing significantly better than the NLO. The results of the NLIO-FG in pitch also show that a single auxiliary observer gain cannot satisfy all accuracy requirements. It can be seen from the steady-state MAEs and RMSEs in

Table 3. MAEs of attitude for all the observers in Case 2.

	Transient (0–200 s)			Steady-State (300–500 s)
	Roll (deg)	Pitch (deg)	Yaw (deg)	Roll (deg)	Pitch (deg)	Yaw (deg)
NLO	0.1972	0.1478	0.7514	0.1968	0.1476	0.7498
NLO-a	0.1331	0.0904	0.4293	0.1295	0.0886	0.4280
NLIO-FG	0.1902	0.1037	0.2757	0.1836	0.0991	0.2566
NLIO-TV	0.1400	0.0726	0.2341	0.1061	0.0649	0.2253

and

Table 4. RMSEs of attitude for all the observers in Case 2.

	Transient (0–200 s)			Steady-State (300–500 s)
	Roll (deg)	Pitch (deg)	Yaw (deg)	Roll (deg)	Pitch (deg)	Yaw (deg)
NLO	0.1853	0.9419	0.1576	0.1850	0.9400	0.1573
NLO-a	0.1133	0.5383	0.1069	0.1109	0.5364	0.1043
NLIO-FG	0.1289	0.3704	0.1585	0.1234	0.3213	0.1548
NLIO-TV	0.1079	0.3461	0.0990	0.0810	0.2820	0.0870

that, compared to Case 1, the observers’ performance in Case 2 is degraded because of the mixed high-level noise. Yet, overall, the performance of the NLIO-TV and NLIO-FG is better than that of the NLO, especially the NLIO-TV.

Case 3.

The initial values of the attitude and gyro bias were the same as in Case 2. The accelerometer noise and magnetometer noise were five times as the above cases, such that ${\eta }_1\sim N(0,\hspace{0.17em}{(5\times 5\times {10}^{-3}\mathrm{g})}^2)$, ${\eta }_2\sim N(0,\hspace{0.17em}{(5\times 8\times {10}^{-3}\mathrm{G})}^2)$ in $110\mathrm{s}\le t\le 190\mathrm{s}$. About this case, the proposed observer tuning gains were set to $K_1=5.5I_{3\times 3}$, $K_2=1.8I_{3\times 3}$. As seen in Figure 9, Figure 10 and Figure 11, between 110 s and 190 s, all observers are sensitive to the added noise, and the auxiliary observers of the NLIOs still have a strong attenuating effect on the added noise, especially that of the NLIO-TV. In general, the auxiliary observer with time-varying gains works better on reducing the impact of vector sensor noise in all cases. Moreover, the NLIO-TV still performs significantly better than the others.

Case 4.

In the above cases, we conclude that it is necessary to choose time-varying gains for the auxiliary observer. In order to compare the performance of the NLO and the NLIO-TV, we made the following comparison and still ran 100 times the same conditions of Case 2. The results in Figure 12 and Figure 13 show that the NLIOs have significant advantages in attitude estimation and gyro bias estimation than the NLOs. Furthermore, the MAEs in

Table 5. MAEs of attitude for NLO and NLIO-TV with different parameters.

	Transient (0–200 s)			Steady-State (300–500 s)
	Roll (deg)	Pitch (deg)	Yaw (deg)	Roll (deg)	Pitch (deg)	Yaw (deg)
NLO	0.1969	0.1480	0.7508	0.1968	0.1476	0.7523
NLO-a	0.1330	0.0904	0.4296	0.1296	0.0884	0.4302
NLIO-TV	0.1304	0.0870	0.2901	0.1252	0.0842	0.2818
NLIO-TV-a	0.1237	0.0761	0.2641	0.1112	0.0710	0.2537

prove this. Meanwhile, we can obtain that the NLIO has a lower sensitivity to $K_P$ than the NLO.

5.2. Simulation B: Comparison of NLCO, MEKF, and NLIO-TV

In this section, we addressed a simulation to compare the performance of the NLIO-TV against the NLCO and the MEKF. The initial attitude was generated randomly from a uniform distribution $[-\pi ,\hspace{0.17em}\pi ]$, and the initial gyro bias was ${[0,\hspace{0.17em}0,\hspace{0.17em}0]}^T\mathrm{rad}/\mathrm{s}$. The noise of the accelerometer and magnetometer was set to ${\eta }_1\sim 0.8N(0,\hspace{0.17em}{(5\times {10}^{-3}\mathrm{g})}^2)+0.2N(0,\hspace{0.17em}{(5\times {10}^{-2}\mathrm{g})}^2)$, ${\eta }_2\sim 0.8N(0,\hspace{0.17em}{(8\times {10}^{-3}\mathrm{G})}^2)+0.2N(0,\hspace{0.17em}{(8\times {10}^{-2}\mathrm{G})}^2)$. In this case, the initial attitude values were randomly selected in a domain, and the vector measurement noise was mixed Gaussian noise. We chose the gyro bias parameters and the vector measurement parameters of the NLCO as $b_1=1.5$, $b_1=0.3$, and $m_1=m_2=0.02$. Fix the tuning gains for the attitude observers in the NLIO-TV as $K_P=1.5I_{3\times 3}$, $k_v=0.2I_{3\times 3}$. The initial covariance matrix of the NLIO-TV and the MEKF were $P_{NLIO\hspace{0.17em}0|0}=\mathrm{blkdiag}\left({10}^{-5}{I}_{3\times 3},\hspace{0.17em}5\times {10}^{-7}{I}_{3\times 3}\right)$ and $P_{MEKF\hspace{0.17em}0|0}=\mathrm{blkdiag}\left({10}^{-4}{I}_{3\times 3},\hspace{0.17em}1\times {10}^{-7}{I}_{3\times 3}\right)$, respectively.

The results in Figure 14 and Figure 15 show that the NLIO-TV has notable advantages in attitude estimation and gyro bias estimation than the NLCO and MEKF in the transient phase. With large initial angle errors, the NLIO-TV converge more quickly than the NLCO and MEKF, which is because the MEKF only works well at small initial angle errors. In the steady-state phase, the MEKF performs better than the NLCO and NLIO-TV. Although MEKF performs well at the steady-state phase, it is a locally stable observer and requires strict initial conditions. Furthermore, the MAEs and RSMEs in

Table 6. MAEs of attitude for all observers.

	Transient (0–200 s)			Steady-State (300–500 s)
	Roll (deg)	Pitch (deg)	Yaw (deg)	Roll (deg)	Pitch (deg)	Yaw (deg)
MEKF	3.8376	2.9606	4.7822	0.0689	0.0681	0.0950
NLCO	4.6182	1.9867	6.9415	0.1667	0.0878	0.1477
NLIO-TV	2.1399	0.3371	0.5107	0.0964	0.0533	0.1777

and

Table 7. RMSEs of attitude for all observers.

	Transient (0–200 s)			Steady-State (300–500 s)
	Roll (deg)	Pitch (deg)	Yaw (deg)	Roll (deg)	Pitch (deg)	Yaw (deg)
MEKF	6.5455	10.785	1.3573	0.0689	0.1178	0.0556
NLCO	7.0986	16.998	1.2130	0.1667	0.1798	0.1423
NLIO-TV	3.6250	3.3297	0.3448	0.0964	0.2216	0.0813

also record the excellent performance of NLIO-TV in the transient phase. Overall, the NLIO-TV is superior to the NLCO.

5.3. Discussion

Notice that the convergence speed of the attitude and gyro bias estimation errors can be controlled by tuning the attitude observer’s tunable parameters. As discussed in Section 3.4, cascaded observer tuning usually selects the proper values of $K_P$ and $k_v$ first, and then adjusts the linear system gains to get stability. Previous results show that the accuracy of the NLIO will not be greatly affected by $K_P$, which indicates that the auxiliary observer reduces part of the vector sensor noise before estimating the attitude and gyro bias. However, the selection of $K_P$ can influence the initial convergence rate of the NLIO.

Although the NLIO increases the computational burden, it can obtain more accurate estimates because of more adaptable and insensitive characteristics to the given $K_P$ and $k_v$. Furthermore, for the NLIO-FG, it is not easy to select a fixed gain for each channel, and constant adjustment is required to achieve excellent attitude estimation.

Therefore, the best choice is the NLIO-TV, which adds the gyro and vector measurement noise terms to the observer error model. By scaling $Q$ and $R_v$, uncertain noise and small interference items can also be considered. Besides, when the vector measurement noise is small, the NLO can be used to save calculation time.

When vector measurements have high noise levels, the NLIO-TV has apparent advantages because of the following reasons:

• The auxiliary observer is designed by avoiding injecting more measurement noise, which is reflected in the estimated vector measurement used in the first term of the dynamic equation.
• The auxiliary observer weakens the measurement noise when estimating the vector measurements. Utilizing the estimated vector measurement for estimating the attitude and gyro bias can effectively improve accuracy and robustness.
• Noise terms are taken into account in its filtering part. The previous derivation shows that the NLIO-FG also has the first two advantages.

Lastly, according to Section 4, it is worth noting that as long as the noise terms or small interference terms are bounded, the estimation errors of the attitude, gyro bias, and vector measurement are bounded.

6. Conclusions

We have introduced an interconnected observer with global exponential stability for attitude and gyro bias estimation by designing its structure, analyzing its stability and robustness to noise, and evaluating its performance through simulation. To obtain better accuracy and robustness, we have further proposed a method to compute the time-varying gains of the auxiliary observer by adding noise terms to the error dynamics model. The simulation results showed that our approach with time-varying gains is conducive to the rapid convergence of attitude and gyro bias estimation and suppression of vector measurement noise compared with other nonlinear observers. Future work will concentrate on utilizing time-varying reference vectors in the proposed observer and extending the proposed observer in the GNSS/INS system, which can be simulated and validated using NaveGo [36].