Design of an Attitude and Heading Reference System Based on Distributed Filtering for Small UAV

Abstract

A development procedure for a low-cost attitude and heading reference system (AHRS) based on the distributed filter has been proposed. The AHRS consists of three single-axis accelerometers, three single-axis gyroscopes, and one 3-axis digital compass. The initial attitude estimation is readily accomplished by using the complementary filtering. The attitude estimation for UAV flying in the real time is realized by using the three low orders EKF. The validation results show that the estimated orientations of the developed AHRS are within the acceptable region, and AHRS can give a stabilized attitude and heading information for a long time.

1. Introduction

Recent advances in autonomous vehicle technologies have made unmanned aerial vehicles (UAV) to become an attractive solution for the modern military and civilian applications such as aerial surveying, pipeline and power line inspection, post-disaster assessment, remote sensing, and cruise missiles [1, 2]. A 2011 report by the Teal Group, who is specialized in aerospace market analysis, forecasts that UAV expenditures worldwide will double within the next ten years to reach $11.3 billion dollars annually in 2020 [3]. By 2018, analysts expect that over 15,000 UAVs could be operating within United States Airspace [4]. Commonly, it needs to know the UAV’s orientation, velocity, and position to be operated, whether manually or with computer assistance. When cost or weight is an issue, inertial navigation using very accurate inertial sensors has been excluded. Instead, low-cost systems using inertial sensors based on microelectromechanical systems (MEMS) have been widely used. To ensure safe and reliable flight of UAV, an accurate attitude and heading reference system (AHRS) is a key component and can give information about the UAV’s orientation in three-dimensional spaces. MEMS-based AHRS are of low cost, light weight, and consume little power. However, the advantages of inexpensive MEMS sensors are coupled with the drawback of having greater potential error in reported roll, pitch, and yaw due to increased sensor noise and drift. Most of the previous works at fusing are based on inertial sensors with GPS for attitude determination [5, 6]. However, GPS signals are susceptible to interference and deception. GPS is unreliable in hostile jammed environments or caused by shadowing effects since the possibility of the outage of GPS. More importantly, the transmitters cannot afford long-term, stable, and reliable positioning information because of the risk of destruction. Recently, many AHRS based on low-cost MEMS gyroscopes, accelerometers, and magnetometers have been developed [7–10]. A number of estimation filters have been proposed since 1970s. The basic idea is to blend several different measurements to obtain the best approximation of the signals. Although many filtering techniques have been proposed and studied to design observers for attitude determination [11–15], the Extended Kalman Filter (EKF) is still one of the most well-known and widely adopted filtering algorithms to design such estimators [16, 17]. The Kalman filter provides the best estimates based on the system dynamics and a priori knowledge of the noise characteristics of the signals. However, EKF divergence due to the linearization of the system and large-state initialization error is a frequent stumbling block to the implementation of the filter. Major difficulties when implementing a centralized Kalman filter (CKF) on a microcontroller arise from the complexity caused by the need for inverting certain matrices. This problem is exacerbated by the need to implement an EKF in case the system is nonlinear and with a large number of states. In contrast to the CKF, the distributed filter is simple, easy to implement, and it has been successfully used for decades on a low-performance micro-controller [18].

The attitude and heading estimation filter proposed in this work is based on the distributed filtering theory, initial attitude estimation is based on the complementary filtering theory, and the real-time attitude estimation will use the three low orders EKF for UAV flying.

The paper is organized as follows. The design of attitude estimation filter is presented in Section 2. Section 3 focuses on the implementation of the attitude estimation filter. Experimental results obtained during UAV flying are presented in Section 4 to illustrate the performance of the proposed AHRS. Concluding remarks and future work are pointed out in Section 5.

2. Problem Statement and System Design

The Euler angles describe the body-axis orientation of UAV body coordinate frame (𝑏 frame) in North, East, and Up navigation coordinates frame (𝑛 frame). Here, 𝜓 is the yaw angle, 𝜗 is the pitch angle, and 𝛾 is the roll angle, as illustrated in Figure 1. The initial attitude and heading of the UAV are needed to initialize the AHRS. When UAV is stationary, Initial attitude and heading of UAV are computed by accelerators and digital compass. The calculation formulas are described as follows: 𝛾=tan⁻¹𝑓^𝑏_𝑥−𝑓^𝑏_𝑧,𝜃=tan⁻¹𝑓^𝑏_𝑦𝑓^𝑏_𝑥²+𝑓^𝑏_𝑧²,𝜓_𝑚=tan⁻¹𝑚^𝑏_𝑥cos𝛾+𝑚^𝑏_𝑧sin𝛾𝑚^𝑏_𝑥sin𝛾sin𝜗+𝑚^𝑏_𝑦cos𝜗−𝑚^𝑏_𝑧𝜗,cos𝛾sin (1) where, 𝑓^𝑏_𝑥, 𝑓^𝑏_𝑦, and 𝑓^𝑏_𝑧 are the specific force of the accelerometers in body coordinate frame (𝑏 frame). 𝑚^𝑏_𝑥, 𝑚^𝑏_𝑦, and 𝑚^𝑏_𝑧 are the components of the magnetic field strength in body coordinate frame.

Figure 1 

𝑏 frame, 𝑛 frame, and Euler angles.

However, stationary UAV is difficult owing to wind and engine vibration in actual test, the initial attitude and heading are seriously influenced by high-frequency noise when the initial attitude and heading are computed by using (1). When the condition of UAV is stationary or low dynamic, the attitude and heading of UAV are accurate and have good low-pass property. However, the error of attitude and heading computed from the gyroscopes output is large since it is easily influenced by drift of the gyroscope. There is complementary property between the attitude derived from the gyroscope outputs and the attitude derived from the accelerometer outputs. The complementary filters theory provides an estimate of the true signal by employing two complementary high-pass and low-pass filters, and an unknown signal can be estimated using corrupted measurements from one or more sensors whose information naturally stands in distinct and complementary frequency bands [19–21]. The initial attitude estimation filter based on complementary filter is to pass the attitude derived from the gyroscope through a high-pass filter and the attitude derived from the accelerometer through a low-pass filter and then to fuse those signals to obtain the estimated attitude; thus, compensating for the drift on the gyroscope and for the slow dynamics of the accelerometer, the block is shown in Figure 2.

Figure 2 

Flow diagram of complementary filtering.

The real attitude and heading are processed on an embedded processor in actual flying test, major difficulties when implementing a centralized extended Kalman filter (CEKF) on a embedded processor arise from the complexity caused by the need of the Jacobian matrix computing and the system equation linearizing, and this problem is exacerbated by the need to implement an EKF with a large number of states. To solve this problem, the attitude and heading estimation filter based on the distributed filtering is designed and completed, three low-order parallel EFK are implemented on the ARM 9 embedded processor. The block diagram of the orientation estimating filter based on the distributed filtering is shown in Figure 3.

Figure 3 

Design of the orientation estimating filter based on the distributed filtering.

3. Attitude and Heading Estimation Filter

3.1. Initial Alignment Filter

Complementary filters have been widely used to combine two independent noisy measurements of the same signal, where each measurement is corrupted by different types of spectral noise [19]. The complementary filters have first-order filter and second-order filter; the second-order complementary filter consists of a second-order high-pass filter and a second-order low-pass filter. The second-order complementary filter of the attitude is described as follows [22]: 𝜔𝛾=²⁰2𝜍/𝜔₀𝐷𝐷²+2𝜍𝜔₀𝐷+𝜔²₀𝛾_𝑎+𝐷²𝐷²+2𝜍𝜔₀𝐷+𝜔²₀𝜔𝛾,𝜗=²⁰2𝜍/𝜔₀𝐷𝐷²+2𝜍𝜔₀𝐷+𝜔²₀𝜗_𝑎+𝐷²𝐷²+2𝜍𝜔₀𝐷+𝜔²₀𝜗, (2) where 𝐷 is the differential operator, 𝜔₀ is the natural frequency, and 𝜍 is the damping ratio.

With the estimated roll and pitch, the yaw can be derived from the measured strength of the magnetic field in body coordinate frame by the digital compass: 𝜓_𝑚=tan⁻¹𝑀^𝑛_𝑥𝑀^𝑛_𝑦=tan⁻¹𝑚^𝑏_𝑥cos𝛾+𝑚^𝑏_𝑧sin𝛾𝑚^𝑏_𝑥sin𝛾sin𝜗+𝑚^𝑏_𝑦cos𝜗−𝑚^𝑏_𝑧𝜗,cos𝛾sin (3) where 𝑚^𝑛_𝑥, 𝑚^𝑛_𝑦, and 𝑚^𝑛_𝑧 are the components of the magnetic field strength in navigation coordinate frame (𝑛 frame). The relationship between real heading 𝜓_𝑚 and 𝜓_𝑚 is described as follows [23]: 𝜓_𝑚=𝜓_𝑚,𝑀^𝑛_𝑦>0,𝜓_𝑚𝜓>0,_𝑚+2𝜋,𝑀^𝑛_𝑦>0,𝜓_𝑚𝜓<0,_𝑚+𝜋,𝑀^𝑛_𝑦<0. (4)

3.2. Attitude and Heading Estimation Filter

According to the initial attitude and heading, the initial strapdown attitude matrix is computed as follow [23]: 𝑇^𝑛_𝑏=𝑇₁₁𝑇₁₂𝑇₁₃𝑇₂₁𝑇₂₂𝑇₂₃𝑇₃₁𝑇₃₂𝑇₃₃=𝑞²₀+𝑞²₁−𝑞²₂−𝑞²₃2𝑞₁𝑞₂−𝑞₀𝑞₃2𝑞₁𝑞₃+𝑞₀𝑞₂2𝑞₁𝑞₂+𝑞₀𝑞₃𝑞²₀−𝑞²₁+𝑞²₂−𝑞²₃2𝑞₂𝑞₃−𝑞₀𝑞₁2𝑞₁𝑞₃−𝑞₀𝑞₂2𝑞₂𝑞₃+𝑞₀𝑞₁𝑞²₀−𝑞²₁−𝑞²₂+𝑞²₃. (5)

Attitude and heading estimation depend on the updated quaternion; the system state equation selected the system state 𝑋=𝑞₀𝑞₁𝑞₂𝑞₃^𝑇 is built as follows: ̇𝑞̇𝑋=⁰𝑞̇₁𝑞̇₂𝑞̇₃=120−𝜔^𝑏_𝑥−𝜔^𝑏_𝑦−𝜔^𝑏_𝑧𝜔^𝑏_𝑥0𝜔^𝑏_𝑧−𝜔^𝑏_𝑦𝜔^𝑏_𝑦−𝜔^𝑏_𝑧0𝜔^𝑏_𝑥𝜔^𝑏_𝑧𝜔^𝑏_𝑦−𝜔^𝑏_𝑥0𝑞₀𝑞₁𝑞₂𝑞₃=120−𝜔^𝑏_𝑥−𝜔^𝑏_𝑦−𝜔^𝑏_𝑧𝜔^𝑏_𝑥0𝜔^𝑏_𝑧−𝜔^𝑏_𝑦𝜔^𝑏_𝑦−𝜔^𝑏_𝑧0𝜔^𝑏_𝑥𝜔^𝑏_𝑧𝜔^𝑏_𝑦−𝜔^𝑏_𝑥0𝑞₀𝑞₁𝑞₂𝑞₃+12𝑞₁𝑞₂𝑞₃−𝑞₀𝑞₃−𝑞₂−𝑞₃−𝑞₀𝑞₁𝑞₂−𝑞₁−𝑞₁𝛿^𝑏_𝑥𝛿^𝑏_𝑦𝛿^𝑏_𝑧=𝐹(𝑡)𝑋(𝑡)+𝐺(𝑡)𝑊(𝑡), (6) where 𝜔^𝑏=𝜔^𝑏_𝑥𝜔^𝑏_𝑦𝜔^𝑏_𝑧^𝑇=𝜔^𝑏_𝑥−𝛿^𝑏_𝑥𝜔^𝑏_𝑦−𝛿^𝑏_𝑦𝜔^𝑏_𝑧−𝛿^𝑏_𝑧^𝑇, 𝜔^𝑏=𝜔^𝑏_𝑥𝜔^𝑏_𝑦𝜔^𝑏_𝑧^𝑇, and 𝛿^𝑏=𝛿^𝑏_𝑥𝛿^𝑏_𝑦𝛿^𝑏_𝑧^𝑇 is the ideal value, real output value, and drift of gyroscopes in body coordinate frame (𝑏 frame), respectively. 𝐹(𝑡) is the state transition matrix; 𝐺(𝑡) is the covariance matrix of noise.

According to (1) and (5), the measurement equation is built by the attitude derived from the gyroscope output and the attitude derived from the accelerometers output. The measurement equation is described as follows: 𝑍(𝑡)=𝑍₁(𝑡)𝑍₂(𝑡)𝑍₃(𝑡)==Δ𝜓𝜓Δ𝜗Δ𝛾^𝑚−𝜓_𝑞𝜗_𝑎−𝜗_𝑞𝛾_𝑎−𝛾_𝑞=𝐻𝑡,𝑞+𝑉(𝑡), (7) where 𝑍(𝑡)=𝛿𝜓𝛿𝜗𝛿𝜆^𝑇 is the error of the attitude and heading. 𝜓_𝑚 is the yaw derived from the digital compass. 𝜗_𝑎 and 𝛾_𝑎 is respectively the pitch, and roll derived from the accelerators; the computing equations are described in (1) 𝜓_𝑞, 𝜗_𝑞, and 𝛾_𝑞 is respectively the yaw, pitch, and roll derived from the quaternion, the computing equation is described as follows [23]: 𝜓_𝑞=tan⁻¹2𝑞₁𝑞₂−𝑞₀𝑞₃𝑞²₀−𝑞²₁+𝑞²₂−𝑞²₃,𝜗_𝑞=sin⁻¹2𝑞₂𝑞₃+2𝑞₀𝑞₁,𝛾_𝑞=tan⁻¹2𝑞₁𝑞₃−𝑞₀𝑞₂𝑞²₀−𝑞²₁−𝑞²₂+𝑞²₃. (8)

The measurement matrix (See Appendix) is obtained by linearizing (6) and (7): 𝐻=𝜕Δ𝜓𝜕𝑋𝜕Δ𝜗𝜕𝑋𝜕Δ𝛾𝜕𝑋^𝑇,𝜕Δ𝜓=𝜕𝑋𝜕Δ𝜓𝜕𝑞₀𝜕Δ𝜓𝜕𝑞₁𝜕Δ𝜓𝜕𝑞₂𝜕Δ𝜓𝜕𝑞₃,𝜕Δ𝜗=𝜕𝑋𝜕Δ𝜗𝜕𝑞₀𝜕Δ𝜗𝜕𝑞₁𝜕Δ𝜗𝜕𝑞₂𝜕Δ𝜗𝜕𝑞₃,𝜕Δ𝛾=𝜕𝑋𝜕Δ𝛾𝜕𝑞₀𝜕Δ𝛾𝜕𝑞₁𝜕Δ𝛾𝜕𝑞₂𝜕Δ𝛾𝜕𝑞₃. (9)

The quaternion is updated by (6) and (7), the principal value of pitch, roll and yaw are derived from the updated 𝑇^𝑛_𝑏 [23], for example, 𝜗_𝑝=sin⁻¹𝑇₃₂,𝛾_𝑝=tan⁻¹−𝑇₃₁𝑇₃₃,𝜓_𝑝=tan⁻¹−𝑇₁₂𝑇₂₂. (10)

The domain of yaw, pitch, and roll is 0^∘360^∘, −90^∘90^∘ and −180^∘180^∘, respectively. The domain and range of pitch are the same; the real value of pitch is the principal value, for example, 𝜗=𝜗_𝑝. The real value of yaw and roll is computed as follows, respectively [23], 𝛾𝛾=^𝑝,𝑇₃₃𝛾>0,_𝑝+𝜋,𝑇₃₃<0,𝛾_𝑝𝛾<0,_𝑝−𝜋,𝑇₃₃<0,𝛾_𝑝𝜓>0,𝜓=^𝑝,𝑇₂₂𝜓<0,_𝑝+2⋅𝜋,𝑇₂₂>0,𝜓_𝑝𝜓>0,_𝑝+𝜋,𝑇₂₂>0,𝜓_𝑝<0. (11)

4. Experimental Test and Results Analysis

Experiments are conducted using an autopilot self-developed from Digital Navigation Center (DNC), BUAA (see Figure 4). This system is composed of three single-axis accelerometers, three single-axis gyroscopes, one 3-axis digital compass, one barometer, one airspeed meter, and one Global Positioning System (GPS) receiver. In order to test the proposed attitude and heading calculation method based on distributed filtering, the dynamic test on the ground and flying test are implemented by using the autopilot from DNC. In the process of dynamic testing, the autopilot is put on the ground, the pitch and roll of UAV do not change, the autopilot is turned around the 𝑧 axis from the initial yaw to 360°, the yaw is turned to make a quarter turn, and the autopilot is stationary for a period of time. The outputs of the AHRS and the reference values for the dynamic test on the ground is represented in Figure 5. The error between of AHRS outputs and the reference values are represented in Figure 6 and Table 1.

Table 1 

The error between AHRS outputs and the reference values for the dynamic test on the ground.

Figure 4 

Self-developed autopilot from DNC.

(a)

(b)

(c)

Figure 5 

AHRS outputs and the reference values for the dynamic test on the ground, (a) pitch, (b) roll, and (c) yaw.

(a)

(b)

(c)

Figure 6 

The error between AHRS outputs and the reference values for the dynamic test on the ground, (a) pitch error, (b) roll error, and (c) yaw error.

Seen from Figures 5 and 6, and Table 1 the attitude calculated output from the AHRS system this paper proposed is stable and can track the reference value. The pitch, roll, and yaw have big fluctuation because when the yaw is turned manually, the MIMU cannot maintain horizontality, and the precision of pitch, roll, and yaw is 0.069°, 0.089°, and 0.15°, respectively.

In order to further test the AHRS system this article proposed, the flight test is carried out using a fixed-wing UAV. After the plane takes off through manual manipulation, the condition is changed to automated driving condition during aircruise phases and the acceleration, gyroscope, and magnetic compass data are stored in an SD card. The test verification is based on the actual data and the results are shown in Figure 7 and Table 2.

Table 2 

The attitude error between AHRS outputs and MIMU/GPS integrated system outputs for flight test.

(a)

(b)

(c)

Figure 7 

Results of AHRS for fixed-wing UAV flight test, (a) pitch, (b) roll, and (c) attitude error.

Seen from Figure 7 and Table 2, the attitude calculated output from the AHRS system this paper proposed is also stable under the dynamic condition through actual flight test. The attitude error between AHRS outputs and reference value is small and the precisions of pitch and roll are 3.73° and 4.43°, respectively. So, AHRS can give a stabilized attitude and heading information in long time.

5. Conclusions

In this paper, based on the distributed filter, the low-cost attitude and heading reference system has been developed. The dynamic test on the ground and actual flight test are implemented, and the test results show that the presented AHRS based on the distributed filter can give the stabilized attitude and heading information for a long time and can implement the automatic flight control of UAV.

Appendix

The attitude and heading based on quaternion were updated as follows:𝜓=tan⁻¹2𝑞₁𝑞₂−𝑞₀𝑞₃𝑞²₀−𝑞²₁+𝑞²₂−𝑞²₃,𝜗=sin⁻¹2𝑞₂𝑞₃+2𝑞₀𝑞₁,𝛾=tan⁻¹2𝑞₁𝑞₃−𝑞₀𝑞₂𝑞²₀−𝑞²₁−𝑞²₂+𝑞²₃,𝜕𝜓𝜕𝑞₀=−2𝑞₃𝑞²₀−𝑞²₁+𝑞²₂−𝑞²₃−4𝑞₀𝑞₁𝑞₂−𝑞₀𝑞₃4(𝑞₁𝑞₂−𝑞₀𝑞₃)²+(𝑞²₀−𝑞²₁+𝑞²₂−𝑞²₃)²,𝜕𝜓𝜕𝑞₁=2𝑞₂𝑞²₀−𝑞²₁+𝑞²₂−𝑞²₃+4𝑞₁𝑞₁𝑞₂−𝑞₀𝑞₃4(𝑞₁𝑞₂−𝑞₀𝑞₃)²+(𝑞²₀−𝑞²₁+𝑞²₂−𝑞²₃)²,𝜕𝜓𝜕𝑞₂=2𝑞₁𝑞²₀−𝑞²₁+𝑞²₂−𝑞²₃−4𝑞₂𝑞₁𝑞₂−𝑞₀𝑞₃4(𝑞₁𝑞₂−𝑞₀𝑞₃)²+(𝑞²₀−𝑞²₁+𝑞²₂−𝑞²₃)²,𝜕𝜓𝜕𝑞₃=−2𝑞₀𝑞²₀−𝑞²₁+𝑞²₂−𝑞²₃+4𝑞₃𝑞₁𝑞₂−𝑞₀𝑞₃4(𝑞₁𝑞₂−𝑞₀𝑞₃)²+(𝑞²₀−𝑞²₁+𝑞²₂−𝑞²₃)²,𝜕𝜗𝜕𝑞₀=2𝑞₁1−(2(𝑞₀𝑞₁+𝑞₂𝑞₃))²,𝜕𝜗𝜕𝑞₁=2𝑞₀1−(2(𝑞₀𝑞₁+𝑞₂𝑞₃))²,𝜕𝜗𝜕𝑞₂=2𝑞₃1−(2(𝑞₀𝑞₁+𝑞₂𝑞₃))²,𝜕𝜗𝜕𝑞₃=2𝑞₂1−(2(𝑞₀𝑞₁+𝑞₂𝑞₃))²,𝜕𝛾𝜕𝑞₀=−2𝑞₂𝑞²₀−𝑞²₁−𝑞²₂+𝑞²₃−4𝑞₀𝑞₁𝑞₃−𝑞₀𝑞₂4(𝑞₁𝑞₃−𝑞₀𝑞₂)²+(𝑞²₀−𝑞²₁−𝑞²₂+𝑞²₃)²,𝜕𝛾𝜕𝑞₁=2𝑞₃𝑞²₀−𝑞²₁−𝑞²₂+𝑞²₃+4𝑞₁𝑞₁𝑞₃−𝑞₀𝑞₂4(𝑞₁𝑞₃−𝑞₀𝑞₂)²+(𝑞²₀−𝑞²₁−𝑞²₂+𝑞²₃)²,𝜕𝛾𝜕𝑞₂=−2𝑞₀𝑞²₀−𝑞²₁−𝑞²₂+𝑞²₃+4𝑞₂𝑞₁𝑞₃−𝑞₀𝑞₂4(𝑞₁𝑞₃−𝑞₀𝑞₂)²+(𝑞²₀−𝑞²₁−𝑞²₂+𝑞²₃)²,𝜕𝛾𝜕𝑞₃=2𝑞₁𝑞²₀−𝑞²₁−𝑞²₂+𝑞²₃−4𝑞₃𝑞₁𝑞₃−𝑞₀𝑞₂4(𝑞₁𝑞₃−𝑞₀𝑞₂)²+(𝑞²₀−𝑞²₁−𝑞²₂+𝑞²₃)². (A.1)

Acknowledgments

This project is supported by the Key Program of the National Natural Science Foundation of China (Grant no. 61039003), the National Natural Science Foundation of China (Grant no. 41274038), the Aeronautical Science Foundation of China (Gratis no. 20100851018), and the Aerospace Innovation Foundation of China (CASC201102).