During the entire scheme investigation process in this paper, a set of road captive test data is used. As shown in Figure 1, the test runs were performed in a spiral structured parking building and the test scenario comprised the following procedures: (1) start (to induce linear acceleration), (2) spiral up (centrifugal acceleration), (3) forward (linear acceleration), (4) backward (linear acceleration), (5) spiral down (centrifugal acceleration), (6) forward (linear acceleration), and stop.

The commercial off-the-shelf IMU ADIS16405 used in the experiment is composed of three accelerometers, three gyros and three magnetometers with digital interface outputs. This inertial unit provides measurements of linear accelerations, angular rates, and the Earth’s magnetic fields for each body axis. Table 1 summarizes the basic specifications of each sensor. An AHRS prototype (Figure 2) was made using a DSP processor for the proposed estimator algorithm. The data acquisition was performed through a RS-232 communication serial port.

The measurements of accelerations (a_x, a_y, a_z), angular rates (p, q, r), and magnetic fields (m_x, m_y, m_z) during the road captive test are shown in Figure 3. The reference attitude and heading angles for this analysis obtained from the on-board vertical gyro are shown in Figure 4.

The roll, pitch and yaw rate (p, q, and r respectively) of the vehicle are measured using rate gyros with respect to its body axis system. The relationship between rate gyro output and time rate of the Euler angles can be described using the direction cosine matrices as follows: (1) [ ϕ ˙ θ ˙ ψ ˙ ]   =   [ 1 sin   ϕ   tan   θ cos   ϕ   tan   θ 0 cos   ϕ − sin   ϕ 0 sin   ϕ cos   θ cos   ϕ cos   θ ] [ p q r ]where ϕ is roll angle, θ is pitch angle, and ψ is yaw angle.

As a physical instrument, the rate gyro device also carries some errors such as axis misalignment, fixed bias, drift bias, fixed scale factor errors, asymmetric scale factor errors, and so on. The bias drift is one of the most serious errors deteriorating the accuracy of an AHRS. The bias drift, which shows normally nonlinear characteristics, causes the integration result to drift-off from the true attitude as a function of time and rapidly renders any calculations useless.

Euler angles obtained from the open-loop integration process of Equation (1) using first order Euler method of integration are illustrated in Figure 5. This result shows that without correction the bias errors creating wandering attitude angles and the gradual instability of the integration drifting.

Attitude angles ϕ and θ can also be obtained from a vehicle’s gravity vector. The following equation dictates a vehicle’s equation of motion in terms of specific forces (f_x, f_y and f_z), which are the accelerometers reading in the body axes: (2) [ f x f y f z ] = [ u ˙ v ˙ w ˙ ] + [ 0 w − v − w 0 u v − u 0 ] [ p q r ] + [ − r 2 − q 2 pq − r 2 pr + q ˙ pq + r ˙ − p 2 − r 2 rq − p ˙ pr − q ˙ rq + p ˙ − q 2 − p 2 ] [ l x l y l z ] + g [ sin   θ − cos   θ sin   ϕ − cos   θ cos   ϕ ]where, (u, v, w) and (l_x, l_y, l_z) are linear velocity components and accelerometer’s coordinates along each axis in the body frame with its origin at the center of gravity, respectively. When all of the parameters of Equation (2), (f_x, f_y, f_z, u̇, v̇, w̄, u, v, w, p, q, r), are measured in flight, near-true attitude angles can be determined. Normally, however, it is hard for a low-cost UAV to measure (v, w), and their time derivatives. Thus, with reduced sensor fit in the UAV, it is not easy to obtain true attitude measurements from accelerometers. Assuming that the UAV is driving steady-level states, linear acceleration terms integrate to zero over time and (p, q, r) can be neglected. With this simplification, Equation (2) may be simplified as follows: (3) ϕ =   atan   2 ( f y , f z )       ,                 θ = atan   2 ( − f x , f y 2 + f z 2 )

It should be noted that the attitude determination using Equation (3) is true only for specific dynamic conditions such as steady level flight. If an aircraft is circling for an extended period, the accelerometer will not only detect gravitational acceleration, but also centrifugal forces, resulting in an incorrect attitude determination. In addition, the change in transient forward acceleration is another possible error source. This means that the attitude calculation from accelerometers is not valid under all dynamic conditions.

The attitude angles obtained from Equation (3) are illustrated in Figure 6 and compared with the reference from the vertical gyros. These results represent that the attitude from accelerometers tends to approximately follow the vertical gyro output in a steady level state, but shows large errors with noises and loose reliability under transient and high dynamic conditions.

A standard three-axis magnetometer reads the magnetic field in an aircraft’s body axis system as (m_x, m_y and m_z). The magnetometer readings in relation to the Earth’s magnetic field vectors (m_N, m_E and m_D) arranged in North, East and Down are as follows: (4) [ m x m y m z ] = [ cos   θ cos   ψ cos   θ sin   ψ − sin   ψ sin   ϕ sin   θ cos   ψ − cos   ϕ sin   ψ sin   ϕ sin   θ sin   ψ + cos   ϕ cos   ψ sin   ϕ cos   θ cos   ϕ sin   θ cos   ψ + sin   ϕ sin   ψ cos   ϕ sin   θ sin   ψ − sin   ϕ cos   ψ cos   ϕ cos   θ ] [ m N m E m D ]

Equation (4) shows how ϕ, θ, and Ψ are related to a magnetometer’s reading and geomagnetic field vectors toward North, East and Down. The yaw angle ψ refers to true magnetic north can be produced after going through a certain calculation provided the results of attitude estimations, ϕ and θ.

Resolving the geomagnetic vector onto a local tangent plane where the x-axis towards the North Pole will put the geomagnetic vector toward East is equal to zero. Hence, Equation (4) can be rearranged as: (5) [ m N   cos   ψ m N   sin   ψ m D ] =   [ cos   θ sin   θ sin   ϕ sin   θ cos   ϕ 0 − cos   ϕ sin   ϕ − sin   θ cos   θ sin   ϕ cos   θ cos   ϕ ] [ m x m y m z ]hence, the division of first and second row gives: (6) ψ = − atan   2 ( Y h , X h )where, X_h = m_N cos ψ and Y_h = m_N sin ψ represent the earth’s horizontal magnetic field components. Preserving the sign of the numerator and denominator of Equation (6) and using the appropriate logical expressions will give heading angle information for all conditions of attitude and magnetometer readings from 0 to 2π.

The heading angles obtained from Equation (6) with the actual reading of magnetic vector (m_x, m_y and m_z) and the vertical gyro information of ϕ and θ are illustrated in Figure 7. These results represent that the heading angle information from magnetometer tends to follow the reference output approximately in all test scenarios. However, the phase loss in high dynamic situations and the noisy conditions are shown in Figure 7 of the expanded scale.

Figure 8 shows the conventional filtering estimator block diagram for the roll axis channel. A similar block diagram could also apply to the pitch and heading channel. The role of the estimator is to compare attitude (or heading) angles resulting from the integration of the gyros with the attitude angle products from the accelerometers (or with heading angle products from magnetometer). The error between ϕ_m and ϕ_a is fed-back through a proportional and integral controller with a pair of estimator gains K_p and K_i.

The mathematical relation of the estimated attitude (ϕ_m), the attitude products from the accelerometers (ϕ_a), and the attitude rate products from the gyros (ϕ̇_g) in the Laplace form (with Laplace operator s) are as follows: (7) ϕ m = 1 s ϕ ˙ g + K p s ( ϕ a − ϕ m ) + K i s 2 ( ϕ a − ϕ m )

The error equation of the estimated attitude yields: (8) δ ϕ m = s δ ϕ ˙ g + ( K p s + K i ) δ ϕ a s 2 + K p s + K i

Applying the final value theorem to Equation (8), we can see that the estimated attitude error converges to the error of the angle calculated from accelerometer: (9) lim t → ∞ δ ϕ m = δ ϕ a

The controller gains, K_p and K_i, are chosen by relating them to the cut-off frequency (ω) and damping ratio (ς) of the estimator as: (10) K i = ω 2     ,     K p = 2 ς ω

During the investigation, the damping ratio ς is fixed to a suitable value of 0.707 to provide a good transient response. Hence: (11) K p = 2 ω

Therefore, the system is only characterized by the cut-off frequency, and this should be chosen appropriately to optimize the process.

As noted in Section 2.4, the magnetometer shows phase loss under a high dynamic condition. Thus, the basic requirements for the filter are: (1) to exhibit constant amplification and small phase loss up to frequencies well above the cut-off frequency of the magnetometer and (2) to use magnetometer in the widest possible ranges of frequencies in order to keep the sensitivity to offset of the rate gyro at a minimum.

To decouple the effectiveness of the heading filter with attitude ones, the attitude information (ϕ and θ) that is needed to go through a certain calculation of Equation (5) is provided by the reference vertical gyro. After having identified the dynamics of all sensors, we tried several complementary filters and could select the best one. The estimator’s cut-off frequency had been set at 0.1 rad/s. The heading angles obtained from the filtering estimator is illustrated in Figure 9. These results represent that the heading estimates tend to follow the reference output satisfactorily in all conditions, showing the noises and phase losses of magnetometer are minimized.

Variations in the estimator’s cut-off frequency (natural frequency) had been tested at 1 rad/s and 0.05 rad/s. The estimation results are compared with the vertical gyro outputs in Figure 10. When the cut-off frequency is 1 rad/s (a relatively high frequency), the estimation angles converge well at the steady state, but not well during high dynamic motion. Though the cut-off frequency is set to a lower frequency, 0.05 rad/s, the estimation results are not improved on the whole.

As the cut-off frequency goes lower, the high frequency dynamic characteristic of the filter becomes better, but the low frequency one becomes worse. So, there should be an optimization problem of selecting a cut-off frequency value to improve the performance of the estimation filter over a wide range of dynamic conditions. However, it seems quite difficult to achieve acceptable performance under all dynamic conditions using this fixed gains linear filtering structure, especially with low-cost (high bias drift) MEMS sensors. To surmount these typical problems and drawbacks, it is necessary to expand the current attitude estimation algorithm so as to have an adaptive function under varying flight dynamics.

Figure 11 shows a block diagram for the gain-scheduled attitude filtering estimator augmented by switching logic for roll axis channel. The same block diagram could also apply to the pitch axis channel. The approach taken here is to exploit switching logic to generate proper parameters of the filtering estimator according to varying dynamic conditions. As seen earlier in Equations (10) and (11), the parameters are determined only by the cut-off frequency. Using simulation study results, the nominal cut-off frequencies are set at 0.05 rad/s and 0.01 rad/s for roll and pitch channel, respectively.

In the proposed scheme, the cut-off frequency is switched to the predetermined value according to the acceleration levels (non-acceleration, low-acceleration, and high-acceleration mode) determined by the scalar dynamic acceleration α(k), defined as: (12) α ( k ) = | f x ( k ) 2 + f y ( k ) 2 + f z ( k ) 2 − g |

The threshold values of the scalar dynamic acceleration α(k) to identify the acceleration level are extracted experimentally based on the characteristics of gyros and accelerometers and the design requirements of the applications. The switching logic for the filter gain has the following scenarios:

Non-acceleration mode: In this mode, α(k) < 0.015 g, the accelerometer measurement of the gravity has observability and yield good estimates of the attitude. To assign more weighting to accelerometers, the cut-off frequency is set at 0.1 rad/s for both roll and pitch axes.

The proposed acceleration based switching architecture has been tested in the same situations with conventional estimator in Section 2. The measurements of low-cost MEMS inertial and magnetic sensors output obtained during the experimental road captive test are used for the implementation of the estimation scheme. The time responses for roll, pitch, and heading estimates are plotted in Figure 12. The results obtained from the high-precision vertical gyro are also presented for comparison. The error between reference and filtering is provided in Figure 13.

It is obvious that these results are much better than the conventional filtering estimator results shown in Figure 9. In order to verify the effectiveness of the estimation, the levels of agreement between the ‘truth model’ and the scheme results are calculated. The calculated errors based upon a simple root mean square (RMS) method are 0.2214, 0.6720, 2.0788 deg for roll, pitch, and heading, respectively. These performances are very encouraging since the estimation process was performed with low-cost MEMS inertial and magnetic sensors with a simple gain-scheduled complementary filter. Figure 14 shows the gains switched online as a function of the scalar dynamic acceleration α(k).