^{1}

^{*}

^{1}

^{2}

^{3}

^{1}

This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution license (http://creativecommons.org/licenses/by/3.0/).

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.

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 [

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

To this effect, various estimators have been proposed, using the quaternion attitude parametrization [

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 [

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.

Consider two orthogonal, right-handed coordinate frames: the body coordinate frame,
^{b}^{f}^{3×3} : ^{T}R

The cross product between two vectors, ^{3}, is represented by a matrix multiplication [^{×}]

The ^{n}^{+1} is denoted as
^{n}^{n+}^{1}: ^{T}x^{2}, by the map,
^{2} →

Hence, a unit quaternion, ^{3}, is defined as:
_{1} _{2} _{3}]^{T}^{3} and _{0} ∈ ℝ are known as the vector and scalar parts of the quaternion, respectively. ^{3} →

Note that ^{3}, ^{−1} := [_{0} –^{T}^{T}

Denoting by _{1} _{2} _{3}]^{T}^{b}^{f}^{b}

The attitude error is used to quantify the mismatch between two attitudes. If

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

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

The angular velocity _{1} _{2} _{3})^{T}_{G}_{v}^{3} are bounded noises and _{3} is a diagonal matrix of time constants.

Let us consider the representation of a vector, _{i}^{f}^{b}_{i}_{i}_{i}_{i}_{iq}_{iq}_{i}_{i}

From ^{4×4}. Note that

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 _{i}_{i}

In the case of perfect measurements, the measure Equation _{q}

On the other hand, if the initial value, _{0}), is known and the angular velocity measurements are perfect, _{G}_{0}) is, in general, unknown, furthermore, the angular velocities are polluted with bias and noise (see _{0}) 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 _{e}^{-1} ⊗ _{ps}_{ps}

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 _{G}_{ps}_{0} ∈ ℝ_{≥ 0}.

_{i} and b⃗_{j}

^{4m×4} _{i}_{i}_{1} _{2} … _{4m}) ∈ ℝ^{4m×4m}, _{1} _{2} _{3} _{4}) ∈ ℝ^{4×4} ^{4m×4}, _{k} along the diagonal and zeros everywhere else, where k

_{ps}_{2} = 1

The proposed attitude nonlinear observer that includes the bias and the error update is given by:
^{3×3} has been defined in _{1}, _{2} are positive constant parameters. _{G}_{e}_{e}_{ps}

Actually, the observer

Note that the attitude quaternion, _{ps}

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}

Δ^{T}, _{ps}

Combining _{1}_{e}

The system _{e}_{+}, _{e}_{−}}, where:

_{e}_{+} :=(1 0^{T}^{T}^{T}^{3} × ℝ^{3}

_{e}_{−} := (-1 0^{T}^{T}^{T}^{3} × ℝ^{3}

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

^{n} be given. The trivial solution x_{0}, _{0}) → 0 _{0}∈ _{0}∈ ℝ _{≥0}.

One considers that the state space of _{+}_{e}^{3}|_{e0}^{3} or _{e}^{3}|_{e0}^{3}, depending on which target equilibrium is chosen.

_{e0}_{e0}_{0})) ≥ 0 _{0} (_{–}). _{e}_{+} (_{e}_{−}) _{+}_{–}

^{3} × ℝ^{3}

The derivative of

Thus, _{e}→_{e0}_{+} converge asymptotically to _{e}_{+} := (1 0^{T}^{T}^{T}

_{e}_{+},_{e}_{−}}, _{e0}_{0}). _{e}_{+}, _{e0}_{0}) ≥ 0 _{e}_{−},

^{−1}^{T}^{T}

Since _{2} is a scalar, the dynamics error

Although the error system

Using

_{e}_{e}

^{−1}, is symmetric and a Hurwitz, one has for all

If the ^{−1}, one gets:

_{1},

_{q}

Analyzing for _{e0}_{e}

Since _{e0}

_{2} and
_{1}, are SPR and state strictly passive, respectively. Since the two systems are connected in feedback (see

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

This equation can be described as illustrated in _{1} and _{2}

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

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

_{2}, _{2}. _{2} (_{e}_{2}_{3}

^{−1}, is a Hurwitz, one obtains:
_{0})‖_{2},(_{0}) = ^{−}^{T}^{−1(}^{t}^{−}^{t}^{0)}‖_{0})‖_{2} is a
_{21} (‖_{e}_{2}) = _{2}‖_{e}_{2} and _{2}(‖_{v}_{2}) = ‖_{v}_{2} are
_{e}_{e}_{2}_{3}

_{1}, _{2}.

The key to showing the ISS of the system,
_{1}, 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.

Analyzing for _{e0}_{e}_{e0}_{q}_{1}, _{2}
_{∞} functions, such that:

Furthermore, _{3} ((1 – _{e0}^{2}) = _{1}_{2}((1 – _{e0}^{2} and _{3}((1−_{e0}^{2} = _{1}_{2})((1−_{e0}_{∞} functions. Then, the Lyapunov function, _{q}_{1}, is ISS.

Since the system,
_{1}, is ISS, the following inequality is accomplished:
_{1} is a
_{12}(‖_{2}) = _{2}‖_{2} and _{1}_{G}_{2}) = _{2}‖ _{G}_{2} are

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

_{2} _{2} < 1.

_{1}-
_{2}). From Propositions 4 and 5, it follows that:

The composition of the functions, _{12} and _{21}, is given by:

Since _{2}

Then, the system (
_{1}-
_{2}) is ISS, when the measurements disturbances, _{G}_{v}_{e}^{T}ṽ^{T}^{T}

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

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 [^{f}^{b}

The attitude and gyro bias observer is implemented at 55 Hz using a fourth-order Runge-Kutta method, with tuning parameters, _{1} = 3.2 and _{2} = 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 _{f}_{o}_{r}_{o}^{T}_{r}^{T}

For the first trial, the initial condition for the quaternion estimate is _{0}) = (0.85 0.06 0.11 – 0.49)^{T}_{0}) = (0.76 0 0 - 0.64)^{T}

It is worth mentioning that the algorithm itself uses only 81% of the 55 Hz cycle time. This is depicted in

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.

The authors wish to thank W. Fermín Guerrero-Sánchez, Facultad Ciencias Físico-Matemáticas (BUAP), for providing us with the Twin Rotor System and for many valuable discussions. German Ardul Muñoz-Hernández wishes to thank the Instituto Tecnologico de Puebla, who supported him with a sabbatical year that made the participation in this work possible.

The authors declare no conflict of interest.

_{a}A

_{x}= A

_{z}Ab

Block diagram of the nonlinear observer (⊗ quaternion multiplication, ⊗ matrix multiplication.)

Block diagram of the dynamics of attitude and bias error.

Block diagram of the dynamics of attitude and bias error with measurement disturbances.

Block diagram for the Attitude and Heading Reference System (AHRS) prototype.

The AHRS prototype.

Experimental setup.

Estimation of

Estimation of

Evolution of the rate gyro bias estimate.

Algorithm CPU usage.