A Robust Nonlinear Observer for Real-Time Attitude Estimation Using Low-Cost MEMS Inertial Sensors

Abstract

This paper deals with the attitude estimation of a rigid body equipped with angular velocity sensors and reference vector sensors. A quaternion-based nonlinear observer is proposed in order to fuse all information sources and to obtain an accurate estimation of the attitude. It is shown that the observer error dynamics can be separated into two passive subsystems connected in “feedback”. Then, this property is used to show that the error dynamics is input-to-state stable when the measurement disturbance is seen as an input and the error as the state. These results allow one to affirm that the observer is “robustly stable”. The proposed observer is evaluated in real-time with the design and implementation of an Attitude and Heading Reference System (AHRS) based on low-cost MEMS (Micro-Electro-Mechanical Systems) Inertial Measure Unit (IMU) and magnetic sensors and a 16-bit microcontroller. The resulting estimates are compared with a high precision motion system to demonstrate its performance.

1. Introduction

1.1. Motivations and Background

The attitude control problem of a rigid body has attracted a strong interest during the past few decades, and it has been extensively studied. This interest comes from the fact that many aerospace systems, such as spacecrafts, satellites, tactical missiles and many others, enter within the framework of a rigid body with the requirement of precise attitude information [1]. In the last decade, the application of micro-electro-mechanical systems (MEMS) has gained a strong interest. Therefore, the attitude estimation problem has been tackled within new areas, such as terrestrial and aerial robotics [2,3], virtual reality [4], biomechanics [5] and wearable robots [6]. In these cases, the attitude information is obtained from inertial and magnetic sensors, namely, three rate gyros, three accelerometers and three magnetometers, orthogonally mounted, such that the sensor frame axes coincide with the principal axes of the rigid body. Since the attitude is not directly measured, it must be estimated via the measurements of the mentioned sensors. In general, these sensors can be classified into two main categories: Angular velocity sensors that measure the angular velocity of the body with respect to some inertial frame and reference vector sensors that give the coordinates of a fixed vector in the mobile frame.

Rate gyros are angular velocity sensors, and they provide continuous attitude information with good short-term stability when their measurements are integrated by means of the rigid body kinematic Equation. However, since rate gyro measurements are affected by drifts and noise, the attitude estimation based on these sensors diverges slowly from the real attitude. On the other hand, when reference vector sensors measurements, known as vector observations, are available, the attitude can be determinated by algebraic or optimization techniques [7]. Therefore, in order to decrease the effect of noise induced by the vector observations, as well as the attitude divergence provoked by the rate gyro drifts, it is important to fuse both information sources.

To this effect, various estimators have been proposed, using the quaternion attitude parametrization [8]. The Extended Kalman Filter (EKF) [9] and new alternatives to the standard EKF have been applied extensively (see [10] and the references therein). However, the divergence problem [11], the non-Gaussian noise induced [12] and the computational cost render difficult the embedded implementation of the Extended Kalman Filter (EKF). In recent years, significant research efforts have been addressed toward the synthesis of nonlinear attitude observers in order to tackle the previously mentioned issues. The first work on this topic was presented in [13]. Subsequently, in [14–16], quaternion-based nonlinear observers and gyro bias observers are proposed in the framework of satellite sensors calibration and marine vehicle navigation, respectively. Furthermore, the formulation of nonlinear attitude observers based on the the orthogonal group, SO(3), have been proposed in the few last years [17–20]. Recently, an excellent overview of rigid body attitude estimation and comparative study was given in [21,22]. Nevertheless, it is well known that a nonlinear observer is generally vulnerable to measurement disturbance, in the sense that a small arbitrary disturbance can lead to the divergence of the error state [23]. The above mentioned nonlinear attitude observers (with the exception of [20]) do not consider either the present noise in the gyro measurements nor a time-varying gyro bias term. Furthermore, the computational complexity in these approaches is a drawback, since the algorithms work with 3 × 3 matrices and they have to preserve orthogonality properties. The singular value decomposition (SVD) is a well-known approach and an extremely powerful and useful tool in linear control theory and signal processing. However, few works have exploited this approach in the framework of attitude estimation. In [24], the authors use the SVD method to estimate the attitude matrix minimizing Wahba's cost function. In the work reported in [25], the authors use the SVD approach for finding the relative orientation of a robotic manipulator. Nevertheless, the problem of attitude estimation from magnetic and inertial sensors using an SVD approach has not been addressed in the literature. Furthermore, unlike sensors and computer systems used in the marine and aerospace community, the signal output of the low-cost sensors is subject to high levels of noise and a time-varying bias term. These problems must be addressed, since, in a practical framework, the design of efficient and embedded attitude estimator using low-cost sensors is an important issue.

1.2. Contributions

In the present work, a quaternion-based attitude observer/estimator of a rigid body is presented. In the proposed approach, the attitude estimation problem is solved in two parts. Firstly, a quaternion attitude is estimated by means of vector observations. In this first step, the attitude estimation is performed using an SVD (singular value decomposition) approach. Then, the quaternion obtained in this step is considered an attitude measurement. Contrary to conventional techniques, the SVD maintains the quaternion's unit constraint naturally. Furthermore, the numerical robustness and numerical stability are guaranteed [26]. The second part of the proposed method consists of the design of a nonlinear observer in order to produce an estimate of the time-varying gyro bias and the attitude quaternion. This observer is driven by an attitude error obtained by means of the quaternion propagated by the observer, and this one obtained from the SVD technique. Asymptotic convergence of the estimation error is proven. Moreover, it is shown that the error dynamics can be decomposed in two passive subsystems connected in “feedback”. This result is exploited to prove that the observer is input-to-state stable (ISS) [27,28] when the rate gyro noise is seen as the input and the error as the state. In this sense, using the small gain theorem, one claims that the observer is “robustly stable”. To evaluate the proposed attitude observer behavior in real-time, a complete Attitude and Heading Reference System (AHRS) based on low-cost inertial and magnetic sensors and a 16-bit microcontroller is designed and implemented. A comparison with a high precision motion system is carried out, in order to demonstrate the observer performance.

The ISS paradigm in an attitude observer, the problem of estimating the attitude from vector observations using an SVD approach, as well as a real-time implementation have never been addressed in the literature. These facts show the originality of the present work.

The document is organized as follows. In Section 2, a mathematical background of the attitude parametrization and sensors modeling is given. The main problem is formulated in Section 3. The formulation of the nonlinear attitude observer and stability analysis is presented in Sections 4-6. The AHRS implementation and experimental results are given in Section 7. Finally, conclusions and further research are mentioned in Section 8.

2. Mathematical Background

2.1. Unit Quaternions and Attitude Kinematics

Consider two orthogonal, right-handed coordinate frames: the body coordinate frame, ${\mathbf{\text{E}}}^b=[{\overrightarrow{e}}_1^b,{\overrightarrow{e}}_2^b,{\overrightarrow{e}}_3^b]$, located at the center of mass of the rigid body, and the inertial coordinate frame, ${\mathbf{\text{E}}}^f=[{\overrightarrow{e}}_1^f,{\overrightarrow{e}}_2^f,{\overrightarrow{e}}_3^f]$, located at some point in the space. The rotation of the body frame, E^b, with respect to the fixed frame, E^f, is represented by the attitude matrix R ∈ SO(3) = { R ∈ ℝ^3×3 : R^TR = I, det R = 1}.

The cross product between two vectors, χ⃗, ξ⃗ ∈ ℝ³, is represented by a matrix multiplication [ξ⃗^×]χ = ξ⃗ × χ⃗, where:

$$[{\overrightarrow{\xi }}^{\times }]=\left(\begin{array}{ccc}0& -{\xi }_3& {\xi }_2\\ {\xi }_3& 0& -{\xi }_1\\ -{\xi }_2& {\xi }_1& 0\end{array}\right)$$

The n-dimensional unit sphere embedded in ℝ ⁿ⁺¹ is denoted as ⁿ = {x ∈ ℝⁿ⁺¹: x^Tx = 1}. Members of SO(3) are often parametrized in terms of a rotation, β ∈ ℝ, about a fixed axis, e⃗ ∈ ², by the map, : ℝ × ² → SO(3), defined as:

$$u(\beta ,\overrightarrow{e}):=I+sin(\beta )[{\overrightarrow{e}}^{\times }]+(1-cos(\beta )){[{\overrightarrow{e}}^{\times }]}^2$$

(1)

Hence, a unit quaternion, q ∈ ³, is defined as:

$$q:=\left(\begin{array}{c}cos\frac{\beta }{2}\\ \overrightarrow{e}sin\frac{\beta }{2}\end{array}\right)=\left(\begin{array}{c}{q}_0\\ \overrightarrow{q}\end{array}\right)\in {\mathbb{\text{S}}}^3$$

(2)

q⃗ = [q₁ q₂ q₃]^T ∈ ℝ³ and q₀ ∈ ℝ are known as the vector and scalar parts of the quaternion, respectively. q represents an element of SO(3) through the map, : ³ →SO(3), defined as:

$$\Re :=I+2q_0[{\overrightarrow{q}}^{\times }]+2{[{\overrightarrow{e}}^{\times }]}^2$$

(3)

Note that R = ( ) (q) = ( ) (—q) for each q ∈ ³, i.e., quaternions q and –q represent the same physical attitude. The inverse unit quaternion is given by q⁻¹ := [q₀ –q⃗^T]^T , and the quaternion product is defined by:

$$q_1\otimes q_2:=\left(\begin{array}{cc}{q}_{1_0}& -{\overrightarrow{q}}_1^T\\ {\overrightarrow{q}}_1& I_3{q}_{1_0}+\left[{\overrightarrow{q}}_1^{\times }\right]\end{array}\right)\left(\begin{array}{c}{q}_{2_0}\\ {\overrightarrow{q}}_2\end{array}\right)$$

(4)

Denoting by ω⃗ = [ω₁ ω₂ ω₃]^T the angular velocity vector of the the body coordinate frame, E^b, relative to the inertial coordinate frame, E^f, expressed in E^b, the kinematics equation is given by:

$$\left(\begin{array}{c}{\dot{q}}_o\\ \dot{\overrightarrow{q}}\end{array}\right)=\frac{1}{2}\left(\begin{array}{c}-{\overrightarrow{q}}^T\\ I_3{q}_0+\left[{\overrightarrow{q}}^{\times }\right]\end{array}\right)\overrightarrow{\omega }=\frac{1}{2}\Xi (q)\overrightarrow{\omega }$$

(5)

The attitude error is used to quantify the mismatch between two attitudes. If q defines the true attitude quaternion and q̂ is the estimated quaternion, then the error quaternion that represents the attitude error between the true orientation and the estimated one is given by [8]:

$$q_e:={\widehat{q}}^{-1}\oplus q={(q_{e_0}{\overrightarrow{q}}_e^T)}^T$$

(6)

In the case that the true quaternion and the estimated one coincide, the quaternion error becomes:

$$q_e={(\pm 10^T)}^T$$

(7)

2.2. Sensor Modeling

The sensors in the rigid body are classified into the two following categories.

2.2.1. Angular Velocity Sensors

The angular velocity ω⃗ = (ω₁ ω₂ ω₃)^T is measured by three rate gyros orthogonally mounted. The output signal of a rate gyro is influenced by noise and by a slowly varying function, v, called bias [29], that is:

$${\overrightarrow{\omega }}_G=\overrightarrow{\omega }+\overrightarrow{v}+{\overrightarrow{\eta }}_G$$

(8)

$$\dot{\overrightarrow{v}}=-T^{-1}\overrightarrow{v}+{\overrightarrow{\eta }}_v$$

(9)

where η⃗_G and η⃗_v ∈ ℝ³ are bounded noises and T = τI₃ is a diagonal matrix of time constants.

2.2.2. Reference Vector Sensors

Let us consider the representation of a vector, x⃗_i, with respect to E^f and E^b and denoted by r⃗_i and b⃗_i, respectively. The vectors, r⃗_i, are also called the “reference vectors” and, in general, are known quite accurately. The body vectors, b⃗_i, are known as “vector observations” and are obtained from on-board sensors (for instance, magnetometers, star trackers, accelerometers). Let b_iq and r_iq be the associate quaternion to the vectors, b⃗_i and r⃗_i:

$$b_{i_q}={(0{\overrightarrow{b}}_i^T)}^T{r}_{i_q}={(0{\overrightarrow{r}}_i^T)}^T$$

(10)

these quaternions are related by the quaternion rotation, q, such that:

$$b_{i_q}=q^{-1}\otimes r_{i_q}\otimes q$$

(11)

From Equation (4) and using quaternion algebra, the following measure model can be obtained [25]:

$$\left(\begin{array}{cc}0& -{({\overrightarrow{b}}_i-{\overrightarrow{r}}_i)}^T\\ {\overrightarrow{b}}_i-{\overrightarrow{r}}_i& -[{({\overrightarrow{b}}_i+{\overrightarrow{r}}_i)}^{\times }]\end{array}\right)q=Hq=0$$

(12)

with H ∈ ℝ^4×4. Note that Equation (12) is a well-structured linear system of equations.

3. Problem Statement

The objective is to design an attitude observer that estimates the rigid body attitude doing a trade-off between a good short-term precision given by rate gyro integration and a reliable long-term accuracy provided by vector observations.

If at least two vector observations are non-collinear, the quaternion can be estimated using these simultaneous vector measurements. For this purpose, the measurement Equation (12) is exploited. The observation matrix in a function of m pair vectors (b⃗_i, r⃗_i) with i ∈ {1, 2,…, m} is written as follows:

$$\overline{H}=\left(\begin{array}{c}{H}_1\\ ⋮\\ H_m\end{array}\right)=\left(\begin{array}{cc}0& -{({\overrightarrow{b}}_1-{\overrightarrow{r}}_1)}^T\\ ({\overrightarrow{b}}_1-{\overrightarrow{r}}_1)& -[{({\overrightarrow{b}}_1+{\overrightarrow{r}}_1)}^{\times }]\\ & ⋮\\ 0& -{({\overrightarrow{b}}_m-{\overrightarrow{r}}_m)}^T\\ {\overrightarrow{b}}_m-{\overrightarrow{r}}_m& -[{({\overrightarrow{b}}_m+{\overrightarrow{r}}_m)}^{\times }]\end{array}\right)\in {ℝ}^{4m\times 4}$$

(13)

In the case of perfect measurements, the measure Equation H̄_q = 0 is always satisfied. However, in practice, the measurements are polluted with perturbations originated from noise, vibrations, etc. Therefore, the problem consists of estimating the attitude quaternion from a perturbed observation matrix, H̄, that is:

$$\begin{array}{c}{q}_{ps}=arg\underset{q}{min}\left\{\frac{1}{2}{\Vert {\overline{H}}_q\Vert }_2^2\right\}\\ \text{subject to}{\Vert q_{ps}\Vert }_2=1\end{array}$$

(14)

On the other hand, if the initial value, q(t₀), is known and the angular velocity measurements are perfect, i.e., ω⃗ = ω⃗_G, it is possible to obtain q(t) by integrating the kinematics Equation (5). Nevertheless, q(t₀) is, in general, unknown, furthermore, the angular velocities are polluted with bias and noise (see Equation (8)), which results in slow divergence of attitude over time. Therefore, the observation idea can be used to on-line correct the integration, q̂(t), of some erroneous q̂ (t₀) and attitude errors caused by the bias, and the noise presents in the rate gyro measurements. This correction can be carried out according to the measurable error q_e = q̂^-1 ⊗ q_ps, where q_ps is considered an attitude measure obtained by the vector observations.

The observation problem 1can then be formulated as follows: Given the kinematic equation of the rigid body and the model of time-varying bias evolution, find an estimate q̂ (t) and v̂ (t) from the knowledge of ω_G(t) and q_ps(t) for all t ≥ t₀ ∈ ℝ_{≥ 0}.

4. Nonlinear Observer Formulation

4.1. Attitude Estimation from Vector Observations

Assumption 1. There are at least two linearly independent vectors, b⃗_i and b⃗_j, with i ≠ j, that is, rank(H̄) ≥ 3.

Lemma 1. Let H̄ ∈ ℝ^4m×4 be the observation matrix formed by the m pair of vectors (b⃗_i, r⃗_i) with i ∈ {1, 2, …,m}. Then, there is an orthonormal matrix U = (U₁ U₂ … U_4m) ∈ ℝ^4m×4m, an orthonormal matrix V = (V₁ V₂ V₃ V₄) ∈ ℝ^4×4 and an orthonormal matrix S ∈ ℝ^4m×4, with elements σ_k along the diagonal and zeros everywhere else, where k = 1 : 4, such that:

$$\overline{H}=US{V}^T$$

(15)

Then, the attitude quaternion is given by the last column of the matrix, V, that is:

$$q_{ps}=V_4$$

(16)

furthermore, the normality condition ‖q_ps‖₂ = 1 is satisfied.

Proof. The proof follows the standard least squares solution of homogeneous equations via the SDV approach [30].

4.2. Nonlinear Attitude Observer with Bias Estimation

The proposed attitude nonlinear observer that includes the bias and the error update is given by:

$${\mathcal{O}}_A:\{\begin{array}{l}\dot{\widehat{q}}=\frac{1}{2}\Xi (\widehat{q})({\overrightarrow{\omega }}_G-\widehat{\overrightarrow{v}}+K_1{\overrightarrow{q}}_e)\hfill \\ \dot{\widehat{\overrightarrow{v}}}=-T^{-1}\widehat{\overrightarrow{v}}-K_2{\overrightarrow{q}}_e\hfill \end{array}$$

(17)

where T ∈ ℝ^3×3 has been defined in Equation (9) and K₁, K₂ are positive constant parameters. q̂ is the prediction of the attitude at time t. It is obtained via the integration of the kinematics Equation (17), using the measured angular velocity, ω⃗_G, the bias estimate, ν̂ and q⃗_e, which is the vector part of the quaternion error, q_e, that measures the discrepancy between q̂ and the measured attitude, q_ps, obtained by means of Lemma 1, that is:

$$q_e={\widehat{q}}^{-1}\otimes q_{ps}={(q_{e_o}{\overrightarrow{q}}_e^T)}^T$$

(18)

Actually, the observer Equation (17) allows one to fuse the data that arise from the vector observations and the rate gyro measurements. The observer structure is shown in Figure 1.

Note that the attitude quaternion, q_ps, can be expressed by:

$$q_{ps}=q\otimes \mathrm{\Delta }q$$

(19)

where q denotes the true attitude and Δq represents a perturbation quaternion, which is caused by the sensor measurement noise encountered in practice.

For the first time, a Lyapunov stability analysis is carried out. As is always done, the error dynamic is obtained in a noise-free context; then, the following hypotheses are made:

●

η⃗_G = η⃗_v= 0. The measurement rate gyro noise is not included in the error dynamics.

●

Δq = (1 0 0 0)^T, i.e., q = q_ps. This is a good assumption for the case of static attitudes and for quasistatic movements [17].

Combining Equations (5)-(9) and (17), the error dynamics is expressed as:

$${\sum }_e:\{\begin{array}{l}{\dot{q}}_e=\frac{1}{2}\left(\begin{array}{cc}0& {\overrightarrow{\gamma }}^T\\ -\overrightarrow{\gamma }& [2{\overrightarrow{\omega }}^{\times }]+[{\overrightarrow{\gamma }}^{\times }]\end{array}\right)\left(\begin{array}{c}{q}_{e_0}\\ {\overrightarrow{q}}_e\end{array}\right)\hfill \\ \dot{\tilde{v}}=-T^{-1}\tilde{v}+K_2{\overrightarrow{q}}_e\hfill \end{array}$$

(20)

where γ⃗ = ṽ + K₁q⃗_e and $\tilde{v}=\vec{v}-\widehat{\vec{v}}$.

The system Equation (20) admits a set of equilibrium points := {p_e+, p_e−}, where:

●

p_e+ :=(1 0^T 0^T)^T ∈ ³ × ℝ³

●

p_e− := (-1 0^T 0^T)^T ∈ ³ × ℝ³

In both scenarios, one shows that the following stability property is ensured with respect to different sets.

Definition 1. (Asymptotic Stability in the Large) [31]. Let χ ⊂ ℝⁿ be given. The trivial solution x = 0 of ẋ = f(t, x) is called asymptotically stable in the large with respect to χ if it is stable in the sense of Lyapunov and every other solution x(t, t₀, x₀) → 0 as t → ∞ for any initial states, x₀∈ χ, and for any initial times, t₀∈ ℝ _≥0.

One considers that the state space of Equation (20) corresponds to either χ₊ = {q_e ∈ ³|q_e0 ∈ [0, 1]} × ℝ³ or χ_ = {q_e ∈ ³|q_e0 ∈ [-1, 0]} × ℝ³, depending on which target equilibrium is chosen.

Proposition 1. Assume that all trajectories with initial conditions, ( ${q_{e_o}(t_0){\overrightarrow{q}}_e^T(t_0){\tilde{v}}^T(t_0))}^T\in {\chi }_{+}$, satisfy sgn(q_e0(t)) = sgn(q_e0(t₀)) ≥ 0 for all t > t₀ (mutatis mutandis for χ_–). Then, the equilibrium point, p_e+ (respectively, the equilibrium point, p_e−) of the system Equation (20) is asymptotically stable in the large with respect to χ₊ (respectively, with respect to χ_–).

Proof. Consider the candidate Lyapunov function, V : ³ × ℝ³ → ℝ, which is proper and positive definite:

$$V=K_2({(1-q_{e_0})}^2+{\overrightarrow{q}}_e^T{\overrightarrow{q}}_e)+\frac{1}{2}{\tilde{v}}^T\tilde{v}=2K_2(1-q_{e_0})+\frac{1}{2}{\tilde{v}}^T\tilde{v}$$

(21)

The derivative of Equation (21), together with Equation (20), is given by:

$$\begin{array}{ll}\dot{V}\hfill & =-2K_2{\dot{q}}_{e_0}+{\tilde{v}}^T\dot{\tilde{v}}=-K_2{\overrightarrow{\gamma }}^T{\overrightarrow{q}}_e+{\tilde{v}}^T(-T^{-1}\tilde{v}+K_2{\overrightarrow{q}}_e)\hfill \\ \hfill & =-K_2({\tilde{v}}^T+K_1{\overrightarrow{q}}_e^T){\overrightarrow{q}}_e-{\tilde{v}}^T{T}^{-1}\tilde{v}+K_2{\tilde{v}}^T{\overrightarrow{q}}_e=-K_2{K}_1{\overrightarrow{q}}_e^T{\overrightarrow{q}}_e-{\tilde{v}}^T{T}^{-1}\tilde{v}\le 0\hfill \end{array}$$

(22)

Thus, q⃗_e→ 0 and ṽ → 0 and, due to the normality condition, q_e0 → 1. Hence, solutions of the system Equation (20) whose initial conditions are in χ₊ converge asymptotically to p_e+ := (1 0^T 0^T)^T . That concludes the proof.

Remark 1. In a practical context, one aims at improving the convergence speed of the observer. Since, physically, the set of equilibrium points, := {p_e+,p_e−}, corresponds to the same attitude error, it is necessary to establish the desired equilibrium point to be achieved, depending on the given initial condition. This improvement can be ensured by choosing the equilibrium point corresponding to the sign of q_e0(t₀). The point, p_e+, is chosen if q_e0(t₀) ≥ 0 and the point, p_e−, otherwise. Then, a slight modification on the observer Equation (17) is done, obtaining

$${\mathcal{O}}_{A_{dis}}:\{\begin{array}{l}\dot{\widehat{q}}=\frac{1}{2}\Xi (\widehat{q})({\overrightarrow{\omega }}_G-\widehat{\overrightarrow{v}}+\mathit{sign}(q_{e_o})K_1{\overrightarrow{q}}_e)\hfill \\ \dot{\widehat{\overrightarrow{v}}}=-T^{-1}\widehat{\overrightarrow{v}}-\mathit{sign}(q_{e_o})K_2{\overrightarrow{q}}_e\hfill \end{array}$$

(23)

This fact can be shown adapting the proof of Proposition 1 using the following Lyapunov function instead:

$$V=\{\begin{array}{c}{K}_2({(1-q_{e_0})}^2+{\overrightarrow{q}}_e^T{\overrightarrow{q}}_e)+\frac{1}{2}{\tilde{v}}^T\tilde{v},if{q}_{e_0}(t_0)\ge 0\\ K_2({(1+q_{e_0})}^2+{\overrightarrow{q}}_e^T{\overrightarrow{q}}_e)+\frac{1}{2}{\tilde{v}}^T\tilde{v},if{q}_{e_0}(t_0)<0\end{array}$$

(24)

5. Passivity Interpretation of the Attitude Nonlinear Observer

Lemma 2. (Kalman-Yakubovich-Popov (KYP) lemma) Let Z(s) = C(sI – A)⁻¹B be a p × p transfer function matrix, where A is Hurwitz, (A, B) is controllable and (A, C) is observable. Then, Z(s) is strictly positive real (SPR) if and only if there exist a positive definite symmetric matrix P = P^T > 0 and Q = Q^T > 0, such that:

$$A^{T}P+PA=-Q$$

(25)

$$B^{T}P=C$$

(26)

Since K₂ is a scalar, the dynamics error Equation (20) for the bias observer can be written as:

$$\dot{\tilde{v}}=-T^{-1}\tilde{v}+K_2{I}_3{\overrightarrow{q}}_e$$

(27)

Although the error system Equation (27) has no output, it is possible to define the following virtual output:

$${\overrightarrow{y}}_e=K_2{I}_3\tilde{v}$$

(28)

Using Equation (28), the error dynamics Equation (20) can be structured as the one shown in Figure 2.

Proposition 2. The transfer function matrix of the error system Equations (27) and (28) is SPR. That is, the mapping, q⃗_e → y⃗_e, satisfies the KYP lemma.

Proof. Since the matrix, –T⁻¹, is symmetric and a Hurwitz, one has for all Q symmetric positive definite matrices a P symmetric positive definite matrix, such that:

$${(-T^{-1})}^{T}P+P(-T^{-1})=-Q$$

(29)

If the Q matrix is specified as Q = 2T⁻¹, one gets:

$$P=I_3$$

(30)

then,

$$B^{T}P={(K_2{I}_3)}^T{I}_3=K_2{I}_3=C$$

(31)

which satisfies the equality Equation (26).

Proposition 3. The dynamics of the quaternion error, represented by the system, ₁, is state strictly passive.

Proof. Consider the candidate Lyapunov function, V_q, which is positive, definite and proper:

$$V_q=\{\begin{array}{l}{K}_2({(1-q_{e_0})}^2+{\overrightarrow{q}}_e^T{\overrightarrow{q}}_e)=2K_2(1-q_{e_0}),si{q}_{e_0}\ge 0\hfill \\ K_2({(1+q_{e_0})}^2+{\overrightarrow{q}}_e^T{\overrightarrow{q}}_e)=2K_2(1+q_{e_0}),si{q}_{e_0}<0\hfill \end{array}$$

(32)

Analyzing for q_e0 ≥ 0, the derivative of Equation (32) along the trajectories of q_e (Equation (20)), is given by:

$$\begin{array}{ll}{\dot{V}}_q\hfill & =-2K_2{\dot{q}}_{e_0}=-K_2{\overrightarrow{\gamma }}^T{\overrightarrow{q}}_e\hfill \\ \hfill & =-K_2({\tilde{v}}^T+K_1{\overrightarrow{q}}_e^T){\overrightarrow{q}}_e=-K_2{K}_1{\overrightarrow{q}}_e^T{\overrightarrow{q}}_e-K_2{\tilde{v}}^T{\overrightarrow{q}}_e\hfill \end{array}$$

(33)

or, similarly:

$$-K_2{\tilde{v}}^T{\overrightarrow{q}}_e={\dot{V}}_q+K_2{K}_1{\overrightarrow{q}}_e^T{\overrightarrow{q}}_e$$

(34)

Since q_e0 ∈ [0, 1], one obtains:

$${\overrightarrow{q}}_e^T{\overrightarrow{q}}_e=1-q_{e_0}^2\ge 1-q_{e_0}\ge {(1-q_{e_0})}^2$$

(35)

Then:

$$\underset{\text{Power Flow}}{\underbrace{K_2{\tilde{v}}^T{\overrightarrow{q}}_e}}\ge \underset{\text{Rate of change of energy stored}}{\underbrace{{\dot{V}}_q}}+\underset{\text{State dissipation rate}}{\underbrace{K_2{K}_1{(1-q_{e_0})}^2}}$$

(36)

Theorem 1. The attitude nonlinear observer Equation (17) is passive.

Proof. Propositions 2 and 3 establish that the systems, ₂ and ₁, are SPR and state strictly passive, respectively. Since the two systems are connected in feedback (see Figure 2), the attitude nonlinear observer Equation (17) is passive.

6. Input to State Stability (ISS) of the Attitude Nonlinear Observer

Note that the convergence and the passivity interpretation of the observer were accomplished within a context free from noise and perturbation. However, the rate gyro measurements are corrupted by measurements noise and bias components that are time-varying. Owing to the nonlinearity of the observer, this type of disturbance can cause instability or even the finite time escape of the estimate.

In this section, the robustness of the proposed observer is discussed, which follows from passivity propriety. It will be shown that the error dynamics is input-to-state stable when the disturbances are viewed as the input and the error quaternion as the state.

In the case that noise is included, the error dynamics Equation (20) becomes:

$${\sum }_{e_{\mathit{\text{noise}}}}:\{\begin{array}{l}{\dot{q}}_e=\frac{1}{2}\left(\begin{array}{cc}0& {\overrightarrow{\gamma }}^T+{\overrightarrow{\eta }}_G^T\\ -\overrightarrow{\gamma }+{\overrightarrow{\eta }}_G& [2{\overrightarrow{\omega }}^{\times }]+[{(\overrightarrow{\gamma }+{\overrightarrow{\eta }}_G)}^{\times }]\end{array}\right)\left(\begin{array}{c}{q}_{e_0}\\ {\overrightarrow{q}}_e\end{array}\right)\hfill \\ \dot{\tilde{v}}=-T^{-1}\tilde{v}+K_2{\tilde{q}}_e+{\overrightarrow{\eta }}_v\hfill \end{array}$$

(37)

This equation can be described as illustrated in Figure 3, where the inputs, d⃗₁ and d⃗₂, are such that:

$${\overrightarrow{d}}_1=\tilde{v}+{\overrightarrow{\eta }}_G,{\overrightarrow{d}}_2=K_2{\overrightarrow{q}}_e+{\overrightarrow{\eta }}_v$$

It is assumed that the noise components are bounded. That is:

$$\begin{array}{cc}\underset{s\in [t_0,t]}{sup}{\Vert {\overrightarrow{\eta }}_G(s)\Vert }_2<c_1& \underset{s\in [t_0,t]}{sup}{\Vert {\overrightarrow{\eta }}_v(s)\Vert }_2<c_2\end{array}$$

(38)

However, neither the bounds nor the noise distribution are necessarily known.

Proposition 4. The system, ₂, is input-to-state stable (ISS) with respect to the input, d⃗₂. Then, the output of ₂ (y⃗_e = K₂I₃ṽ) is always bounded.

Proof. Consider the second equation of the system Equation (37). Since the matrix, –T⁻¹, is a Hurwitz, one obtains:

$$\begin{array}{ll}{\Vert \tilde{v}(t)\Vert }_2\hfill & =e^{-T^{-1}}(t-t_0){\Vert \tilde{v}(t_0)\Vert }_2+{\int }_{t_0}^t{e}^{-T^{-1}(t-s))}{\overrightarrow{d}}_2(s)ds\hfill \\ \hfill & =e^{-T^{-1}(t-t_0)}{\Vert \tilde{v}(t_0)\Vert }_2+{\int }_{t_0}^t{e}^{-T^{-1}(t-s)}(K_2{\overrightarrow{q}}_e(s)+{\overrightarrow{\eta }}_v(s))ds\hfill \\ \hfill & \le e^{-T^{-1}(t-t_0)}{\Vert \tilde{v}(t_0)\Vert }_2+K_2\underset{s\in [t_0,t]}{sup}\{{\Vert {\overrightarrow{q}}_e(s)\Vert }_2\}+\underset{s\in [t_0,t]}{sup}\{{\Vert {\overrightarrow{\eta }}_v(s)\Vert }_2\}\hfill \end{array}$$

(39)

then:

$${\Vert \tilde{v}(t)\Vert }_2\le max\{{\zeta }_2({\Vert \tilde{v}(t_0)\Vert }_2,t-t_0),\underset{s\in [t_0,t]}{sup}{\varsigma }_{21}({\Vert {\overrightarrow{q}}_e(s)\Vert }_2),\underset{s\in [t_0,t]}{sup}{\varsigma }_2({\Vert {\overrightarrow{\eta }}_v(s)\Vert }_2)\}$$

(40)

where: ζ(‖ṽ(t₀)‖₂,(t − t₀) = e^{−T−1(t−t₀)}‖ṽ(t₀)‖₂ is a $ℒ$ function and ς₂₁ (‖q⃗_e(s)‖₂) = K₂‖q⃗_e(s)‖₂ and ς₂(‖η⃗_v(s)‖₂) = ‖η⃗_v(s)‖₂ are functions. Then, the bias estimation error, ṽ(t), is input-to-state stable, and the output y⃗_e(y⃗_e = K₂I₃ṽ) is always bounded.

Proposition 5. The system, ₁, is input-to-state stable with respect to d⃗₂.

The key to showing the ISS of the system, ₁, is the fact that a system is input-to-state stable if it admits an ISS-Lyapunov function. In this sense, a Lyapunov function will be proposed; then, it will be shown that this function is a ISS-Lyapunov function.

Proof. Consider the candidate Lyapunov function, V, which is proper and positive definite:

$$V_q=\{\begin{array}{c}{K}_2({(1-q_{e_0})}^2+{\overrightarrow{q}}_e^T{\overrightarrow{q}}_e)=2K_2(1-q_{e_0}),si{q}_{e_0}\ge 0\\ K_2({(1+q_{e_0})}^2+{\overrightarrow{q}}_e^T{\overrightarrow{q}}_e)=2K_2(1+q_{e_0}),si{q}_{e_0}<0\end{array}$$

(41)

Analyzing for q_e0 ≥ 0, the derivative of Equation (32) along the trajectories of q_e (Equation (37)) is given by:

$$\begin{array}{ll}{\dot{V}}_q\hfill & =-2K_2{\dot{q}}_{e_0}\hfill \\ \hfill & =-K_2({\overrightarrow{\gamma }}^T+{\overrightarrow{\eta }}_G^T){\overrightarrow{q}}_e\hfill \\ \hfill & =-K_2({\tilde{\nu }}^T+K_1{\overrightarrow{q}}_e^T+{\overrightarrow{\eta }}_G^T){\overrightarrow{q}}_e\hfill \\ \hfill & =-K_2{K}_1{\overrightarrow{q}}_e^T{\overrightarrow{q}}_e-K_2({\tilde{\nu }}^T+{\overrightarrow{\eta }}_G^T){\overrightarrow{q}}_e\hfill \\ \hfill & =-K_2{K}_1{\overrightarrow{q}}_e^T{\overrightarrow{q}}_e-K_2{\overrightarrow{d}}_1^T{\overrightarrow{q}}_e\hfill \end{array}$$

(42)

since q_e0 ∈ [0,1], one obtains:

$${\overrightarrow{q}}_e^T{\overrightarrow{q}}_e={\Vert {\overrightarrow{q}}_e\Vert }_2^2=1-q_{e_0}^2\ge 1-q_{e_0}\ge {\left(1-q_{e_0}\right)}^2$$

(43)

then:

$$\begin{array}{ll}{\dot{V}}_q\hfill & \le -K_2{K}_1{(1-q_{e_0})}^2-{\overrightarrow{d}}_1^T{\overrightarrow{q}}_e\hfill \\ \hfill & \le -K_2{K}_1{(1-q_{e_0})}^2+{\Vert {\overrightarrow{d}}_1\Vert }_2{\Vert {\overrightarrow{q}}_e\Vert }_2\hfill \\ \hfill & \le -K_2{K}_1{(1-q_{e_0})}^2+{\Vert {\overrightarrow{d}}_1\Vert }_2\hfill \\ \hfill & \le -{\alpha }_3({(1-q_{e_0})}^2)+{\alpha }_4({\Vert {\overrightarrow{d}}_1\Vert }_2)\hfill \end{array}$$

(44)

since V_q is proper ∃ α₁, α_{2 ∞} functions, such that:

$${\alpha }_1\left({\Vert q\left(t_0\right)\Vert }_2\right)\le V_q\left(q\left(t_0\right)\right)\le {\alpha }_2\left({\Vert q\left(t_0\right)\Vert }_2\right)$$

Furthermore, α₃ ((1 – q_e0)²) = K₁K₂((1 – q_e0)² and α₃((1−q_e0)² = K₁K₂)((1−q_e0)) are _∞ functions. Then, the Lyapunov function, V_q, is a ISS-Lyapunov function (see [27]), and this fact implies that the system, ₁, is ISS.

Since the system, ₁, is ISS, the following inequality is accomplished:

$$\begin{array}{ll}{\Vert {\overrightarrow{q}}_e\left(t\right)\Vert }_2\hfill & \le max\left\{{\zeta }_1({\Vert {\overrightarrow{q}}_e(t_0)\Vert }_2,t-t_0),\underset{s\in \left[t_0,t\right]}{sup}{\varsigma }_d({\Vert {\overrightarrow{d}}_1(s)\Vert }_2)\right\}\hfill \\ \hfill & \le max\left\{{\zeta }_1({\Vert {\overrightarrow{q}}_e(t_0)\Vert }_2,t-t_0),\underset{s\in \left[t_0,t\right]}{sup}{\varsigma }_{12}({\Vert \tilde{\nu }(s)\Vert }_2),\underset{s\in \left[t_0,t\right]}{sup}{\varsigma }_1({\Vert {\overrightarrow{\eta }}_G(s)\Vert }_2)\right\}\hfill \end{array}$$

(45)

where ζ₁ is a $ℒ$ function and ζ₁₂(‖ṽ‖₂) = K₂‖ṽ‖₂ and ζ₁(‖η⃗_G ‖₂) = K₂‖ η⃗_G ‖₂ are functions. That is, every trajectory of the attitude error dynamics asymptotically approaches a ball around the origin, whose radius is a function of supremum norm of the bias estimation error and of the measure noise of the rate gyro.

Using the interesting properties of ISS systems, the stability of the entire error dynamics is studied. Then, one has the following result.

Theorem 2. Let K₂ be a positive constant, such that K₂ < 1. Then, the attitude nonlinear observer is robust towards to measurement disturbance of the rate gyro.

Proof. The claim follows from the small gain theorem for the interconnection of nonlinear systems that are input-to-state stable [32]. The error dynamics of a nonlinear observer (Equation (37)) is composed of the feedback interconnection of the systems ₁- ₂). From Propositions 4 and 5, it follows that:

$${\Vert {\overrightarrow{q}}_e\left(t\right)\Vert }_2\le max\left\{{\zeta }_1\left({\Vert {\overrightarrow{q}}_e\left(t_0\right)\Vert }_2,t-t_0\right),\underset{s\in \left[t_0,t\right]}{sup}{\varsigma }_{12}\left({\Vert \tilde{\nu }\left(s\right)\Vert }_2\right),\underset{s\in \left[t_0,t\right]}{sup}{\varsigma }_1\left({\Vert {\overrightarrow{\eta }}_G\left(s\right)\Vert }_2\right)\right\}$$

(46)

$${\Vert \tilde{\nu }\left(t\right)\Vert }_2\le max\left\{{\zeta }_2\left({\Vert \tilde{\nu }\left(t_0\right)\Vert }_2,t-t_0\right),\underset{s\in \left[t_0,t\right]}{sup}{\varsigma }_{21}\left({\Vert {\overrightarrow{q}}_e\left(s\right)\Vert }_2\right),\underset{s\in \left[t_0,t\right]}{sup}{\varsigma }_2\left({\Vert {\overrightarrow{\eta }}_{\nu }\left(s\right)\Vert }_2\right)\right\}$$

(47)

The composition of the functions, ς₁₂ and ς₂₁, is given by:

$${\varsigma }_{12}\circ {\varsigma }_{21}({\Vert {\overrightarrow{q}}_e(s)\Vert }_2)=K_2(K_2{\Vert {\overrightarrow{q}}_e\Vert }_2)$$

(48)

Since K₂ < 1, it follows that:

$$K_2^2{\Vert {\overrightarrow{q}}_e\Vert }_2<{\Vert {\overrightarrow{q}}_e\Vert }_2\forall {\Vert {\overrightarrow{q}}_e\Vert }_2>0$$

(49)

Then, the system ( ₁- ₂) is ISS, when the measurements disturbances, η⃗_G and η⃗_v, are seen as the input and the error state, [q⃗_e^Tṽ^T]^T, as the state. This means that the attitude observer is robust with respect to measurement disturbance.

7. Experimental Results

The estimation methodology proposed in this work is implemented and evaluated in real time, in order to assess its effectiveness. For this purpose, an embedded system was designed and developed. Special attention was paid to the low power consumption requirements and weight, leading to the selection of the digital signal controller (DSC), dsPIC33FJ128MC802, which was used with a clock speed of 4 MHz. It contains extensive digital signal processor (DSP) functionality with a high performance, 16-bit microcontroller (MCU) architecture, but without a floating point unit. The sensor suite is based on a sensor board equipped with a tri-axis accelerometer (ADXL135), a dual axis gyro (LPR530AL), a single axis gyro (LY530ALH) and a tri-axis magnetometer (Micromag 3). All sensors outputs are analog, except for the Micromag 3, which is digital and uses the serial peripheral interface (SPI) bus system as the underlying physical communication layer. The aim was to test the DSC implementation with an update rate of the estimation of at least 55 Hz.

Furthermore, the system is equipped with a Bluetooth module (BlueSMiRF Silver), which provides wireless capabilities. The total system supply voltage is 3.3 V. The dimension and weight are 60 × 40 × 15 mm and 60 g, respectively. The system is depicted in Figures 4 and 5.

In order to experimentally evaluate the performance of the proposed AHRS, the experiments were carried out with a two degrees-of-freedom high accuracy system. Actually, this system is the Twin Rotor System [33] (see Figure 6). This system has two rotational joints, which allow for movement about two orthogonally axes with a resolution of 0.35° considered a true attitude for performance evaluation. In order to test the complete attitude estimation, two trials were carried out. In the first one, the axes, ${\overrightarrow{e}}_1^b$ and ${\overrightarrow{e}}_3^b$, of the AHRS coordinate frame (body coordinate frame ${\mathbf{\text{E}}}^b=[{\overrightarrow{e}}_1^b,{\overrightarrow{e}}_2^b,{\overrightarrow{e}}_3^b]$) coincide with the axes, ${\overrightarrow{e}}_1^t$ and ${\overrightarrow{e}}_3^t$, of the Twin Rotor System. Thus, θ (pitch) and Ψ (yaw) angles were evaluated. In order to evaluate the angle, Φ (roll), the AHRS was rotated π/2 about the axis, ${\overrightarrow{e}}_1^b$, such that ${\overrightarrow{e}}_1^b$ coincides with ${\overrightarrow{e}}_1^t$ and ${\overrightarrow{e}}_2^b$ with ${\overrightarrow{e}}_3^t$. Note that the attitude is estimated with respect to the inertial coordinate frame ${\mathbf{\text{E}}}^f=[{\overrightarrow{e}}_1^f,{\overrightarrow{e}}_2^f,{\overrightarrow{e}}_3^f]$. E^f is chosen to be the NED (north, east, down) coordinate frame. In this case, the reference vectors are the gravitational and magnetic vectors. The vector observations, i.e., the gravitational and magnetic vectors expressed in the body frame, E^b, are obtained from the mentioned tri-axis accelerometer and tri-axis magnetometer. The angular velocity is obtained from three rate gyros.

The attitude and gyro bias observer is implemented at 55 Hz using a fourth-order Runge-Kutta method, with tuning parameters, K₁ = 3.2 and K₂ = 0.9, such that the restriction of Theorem 2 is accomplished. No particular emphasis was given on the tuning process, since the resulting performance with these simple parameters is very acceptable. The attitude is maintained as a quaternion, which is normalized at each time step to compensate for numerical errors. After that, the quaternion is converted to Euler Angles, since they are more intuitive, and it allows for comparing with the angles obtained from the Twin Rotor System. For the entire experimentation time span, the vector v⃗_f = v⃗_o + v⃗_r was added to the vector of the rate gyro measurements, in order to test the ability to estimate and compensate for the rate gyro bias. In this case, one has v⃗_o = 0.1(1 – 1 1)^T and v⃗_r = 0.0001k(1 1 1)^T, where k represents the iteration index.

For the first trial, the initial condition for the quaternion estimate is q⃗(t₀) = (0.85 0.06 0.11 – 0.49)^T, i.e., (ϕ̂ = 0°, θ̂ = 15°, ψ̂ = −60°). For the second trial, the initial condition for the quaternion estimate is q̂(t₀) = (0.76 0 0 - 0.64)^T, i.e., (ϕ̂ = 0°, θ̂ = 0°, ψ̂ = −80°). In both cases, the rate gyro bias initial estimate was set to zero. The observer angle estimates are compared with the ones obtained from the encoders of the Twin Rotor System. In Figure 7, the evolution of θ (pitch) and Ψ (yaw) angles is depicted. Figure 8 shows the evolution of the second trial, i.e., Φ (roll) and Ψ (yaw) angles. Finally, the convergence of the rate gyros bias estimates is shown in Figure 9. These results indicate that the observer faces up to the slowly time-varying nature of the rate gyro biases.

It is worth mentioning that the algorithm itself uses only 81% of the 55 Hz cycle time. This is depicted in Figure 10. Finally, we invite the readers to watch the following video: https://www.dropbox.eom/s/r3pq57rwlf0pxuv/AttitudeObserver.mov.

8. Conclusions

This paper presented an original quaternion-based strategy for the attitude estimation of a rigid body equipped with angular velocity sensors and reference vector sensors. To accomplish the objective, a robust nonlinear observer for attitude estimation was developed. This observer ensures the asymptotic estimation convergence and local exponential estimation convergence. Furthermore, by means of an ISS analysis, it was shown that the observer is robust towards noise and the disturbance of the rate gyro sensors. Thus, an embedded AHRS was designed and developed. Experimental results were presented, where the AHRS outputs are compared with a two degrees-of-freedom high accuracy motion system. This illustrates the achievable performance of the proposed estimation schema.