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 (b frame) in North, East, and Up navigation coordinates frame (n 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:
(1)γ=tan-1(fxb(-fzb)),θ=tan-1(fyb(fxb2+fzb2)),ψ^m=tan-1(mxbcosγ^+mzbsinγ^mxbsinγ^sinϑ^+mybcosϑ^-mzbcosγ^sinϑ^),
where, fxb, fyb, and fzb are the specific force of the accelerometers in body coordinate frame (b frame). mxb, myb, and mzb are the components of the magnetic field strength in body coordinate frame.
b frame, n 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.
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.
Design of the orientation estimating filter based on the distributed filtering.
3. Attitude and Heading Estimation Filter3.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)γ^=ω02(2ς/ω0)DD2+2ςω0D+ω02γa+D2D2+2ςω0D+ω02γ,ϑ^=ω02(2ς/ω0)DD2+2ςω0D+ω02ϑa+D2D2+2ςω0D+ω02ϑ,
where D is the differential operator, ω0 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:
(3)ψ^m=tan-1(MxnMyn)=tan-1(mxbcosγ^+mzbsinγ^mxbsinγ^sinϑ^+mybcosϑ^-mzbcosγ^sinϑ^),
where mxn, myn, and mzn are the components of the magnetic field strength in navigation coordinate frame (n frame). The relationship between real heading ψm and ψ^m is described as follows [23]:
(4)ψm={ψ^m,Myn>0,ψ^m>0,ψ^m+2π,Myn>0,ψ^m<0,ψ^m+π,Myn<0.
3.2. Attitude and Heading Estimation Filter
According to the initial attitude and heading, the initial strapdown attitude matrix is computed as follow [23]:
(5)Tbn=[T11T12T13T21T22T23T31T32T33]=[q02+q12-q22-q322(q1q2-q0q3)2(q1q3+q0q2)2(q1q2+q0q3)q02-q12+q22-q322(q2q3-q0q1)2(q1q3-q0q2)2(q2q3+q0q1)q02-q12-q22+q32].
Attitude and heading estimation depend on the updated quaternion; the system state equation selected the system state X=[q0q1q2q3]T is built as follows:
(6)X˙=[q˙0q˙1q˙2q˙3]=12[0-ω^xb-ω^yb-ω^zbω^xb0ω^zb-ω^ybω^yb-ω^zb0ω^xbω^zbω^yb-ω^xb0][q0q1q2q3]=12[0-ωxb-ωyb-ωzbωxb0ωzb-ωybωyb-ωzb0ωxbωzbωyb-ωxb0][q0q1q2q3]+12[q1q2q3-q0q3-q2-q3-q0q1q2-q1-q1][δxbδybδzb]=F(t)X(t)+G(t)W(t),
where ω^b=[ω^xbω^ybω^zb]T=[ωxb-δxbωyb-δybωzb-δzb]T, ωb=[ωxbωybωzb]T, and δb=[δxbδybδzb]T is the ideal value, real output value, and drift of gyroscopes in body coordinate frame (b frame), respectively. F(t) is the state transition matrix; G(t) 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)Z(t)=[Z1(t)Z2(t)Z3(t)]=[ΔψΔϑΔγ]=[ψm-ψqϑa-ϑqγa-γq]=H(t,q)+V(t),
where Z(t)=[δψδϑδλ]T is the error of the attitude and heading. ψm is the yaw derived from the digital compass. ϑa and γa is respectively the pitch, and roll derived from the accelerators; the computing equations are described in (1) ψq, ϑq, and γq is respectively the yaw, pitch, and roll derived from the quaternion, the computing equation is described as follows [23]:
(8)ψq=tan-1(2(q1q2-q0q3)q02-q12+q22-q32),ϑq=sin-1(2q2q3+2q0q1),γq=tan-1(2(q1q3-q0q2)q02-q12-q22+q32).
The measurement matrix (See Appendix) is obtained by linearizing (6) and (7):
(9)H=[∂Δψ∂X∂Δϑ∂X∂Δγ∂X]T,∂Δψ∂X=[∂Δψ∂q0∂Δψ∂q1∂Δψ∂q2∂Δψ∂q3],∂Δϑ∂X=[∂Δϑ∂q0∂Δϑ∂q1∂Δϑ∂q2∂Δϑ∂q3],∂Δγ∂X=[∂Δγ∂q0∂Δγ∂q1∂Δγ∂q2∂Δγ∂q3].
The quaternion is updated by (6) and (7), the principal value of pitch, roll and yaw are derived from the updated Tbn [23], for example,
(10)ϑp=sin-1(T32),γp=tan-1(-T31T33),ψp=tan-1(-T12T22).
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, ϑ=ϑp. The real value of yaw and roll is computed as follows, respectively [23],
(11)γ={γp,T33>0,γp+π,T33<0,γp<0,γp-π,T33<0,γp>0,ψ={ψp,T22<0,ψp+2·π,T22>0,ψp>0,ψp+π,T22>0,ψp<0.
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 z 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.
The error between AHRS outputs and the reference values for the dynamic test on the ground.
Pitch angle error (°)
Roll angle error (°)
Yaw angle error (°)
Mean
Variance
Mean
Variance
Mean
Variance
Dynamic test
-0.017
0.069
-0.056
0.089
-0.013
0.150
Self-developed autopilot from DNC.
AHRS outputs and the reference values for the dynamic test on the ground, (a) pitch, (b) roll, and (c) yaw.
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.
The attitude error between AHRS outputs and MIMU/GPS integrated system outputs for flight test.
Pitch angle error (°)
Roll angle error (°)
Yaw angle error (°)
Mean
Variance
Mean
Variance
Mean
Variance
Flighting test
1.84
3.73
-1.12
4.43
—
—
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:(A.1)ψ=tan-1(2(q1q2-q0q3)q02-q12+q22-q32),ϑ=sin-1(2q2q3+2q0q1),γ=tan-1(2(q1q3-q0q2)q02-q12-q22+q32),∂ψ∂q0=-2q3(q02-q12+q22-q32)-4q0(q1q2-q0q3)4(q1q2-q0q3)2+(q02-q12+q22-q32)2,∂ψ∂q1=2q2(q02-q12+q22-q32)+4q1(q1q2-q0q3)4(q1q2-q0q3)2+(q02-q12+q22-q32)2,∂ψ∂q2=2q1(q02-q12+q22-q32)-4q2(q1q2-q0q3)4(q1q2-q0q3)2+(q02-q12+q22-q32)2,∂ψ∂q3=-2q0(q02-q12+q22-q32)+4q3(q1q2-q0q3)4(q1q2-q0q3)2+(q02-q12+q22-q32)2,∂ϑ∂q0=2q11-(2(q0q1+q2q3))2,∂ϑ∂q1=2q01-(2(q0q1+q2q3))2,∂ϑ∂q2=2q31-(2(q0q1+q2q3))2,∂ϑ∂q3=2q21-(2(q0q1+q2q3))2,∂γ∂q0=-2q2(q02-q12-q22+q32)-4q0(q1q3-q0q2)4(q1q3-q0q2)2+(q02-q12-q22+q32)2,∂γ∂q1=2q3(q02-q12-q22+q32)+4q1(q1q3-q0q2)4(q1q3-q0q2)2+(q02-q12-q22+q32)2,∂γ∂q2=-2q0(q02-q12-q22+q32)+4q2(q1q3-q0q2)4(q1q3-q0q2)2+(q02-q12-q22+q32)2,∂γ∂q3=2q1(q02-q12-q22+q32)-4q3(q1q3-q0q2)4(q1q3-q0q2)2+(q02-q12-q22+q32)2.
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).
CoxT. H.NagyC. J.SkoogM. A.SomersI. A.Civil UAV capabil it y assessmentWachterZ.A cost effective motion platform for performance testing of MEMS-based attitude and heading reference systemsZalogaS. J.RockwellD.FinneganP.SchneidermanR.Unmanned drones are flying high in the military/aerospace sectorWolfR.HeinG. W.EissfellerB.LoehnertE.Integrated low cost GPS/INS attitude determination and position location systemProceedings of the 9th International Technical Meeting of the Satellite Division of the Institute of Navigation (ION GPS '96)September 19969759812-s2.0-0030381810LiY.DempsterA.LiB. H.WangJ. L.RizosC.A low-cost at t itude heading reference system by combinat ion of GPS and magnetometers and MEMS inert ial sensors for mobile applicat ionsMartinP.SalaünE.Design and implementation of a low-cost aided attitude and heading reference systemProceedings of the AIAA Guidance, Navigation, and Control Conference2008GeigerW.BartholomeyczikJ.BrengU.GutmannW.HafenM.HandrichE.HuberM.JäckieA.KempferU.KopmannH.KunzJ.LeinfelderP.OhmbergerR.ProbstU.RufM.SpahlingerG.RaschA.Straub-KalthoffJ.StrodaM.StumpfK.WeberC.ZimmermannM.ZimmermannS.MEMS IMU for AHRS applicationsProceedings of the IEEE/ION Position, Location and Navigation Symposium (PLANS '08)May 20082252312-s2.0-5534914693710.1109/PLANS.2008.4569973SukkariehS.GibbensP.GrocholskyB.WillisK.Durrant-WhyteH. F.Low-cost, redundant inertial measurement unit for unmanned air vehiclesSherryL.BrownC.MotazedB.VosD.Performance of automotive-grade MEMS sensors in low cost AHRS for general aviation2Proceeding of the 22nd Digital Avionics Systems ConferenceOctober 200312.C.2/112.C.2/52-s2.0-0344551243PsiakiM. L.Attitude-determination filtering via extended quaternion estimationMarinsJ. L.YunX.BachmannE. R.McGheeR. B.ZydaM. J.An extended Kalman filter for quaternion-based orientation estimation using MARG sensorsProceedings of the IEEE/RSJ International Conference on Intelligent Robots and SystemsNovember 2001200320112-s2.0-0035558199CrassidisJ. L.Sigma-point Kalman filtering for integrated GPS and inertial navigationVan MerweR. D.WanE. A.JulierS. I.Sigma-point kalman filters for nonlinear estimation and sensor-fusion—applications to integrated navigationProceedings of the AIAA Guidance, Navigation, and Control ConferenceAugust 2004173517642-s2.0-19944383162HideC.MooreT.SmithM.Adaptive Kalman filtering for low-cost INS/GPSCrassidisJ. L.Landis MarkleyF.ChengY.Survey of nonlinear attitude estimation methodsMarkleyF. L.Attitude error representations for Kalman filteringSpeyerJ. L.Computation and transmission requirements for a decentralized Linear-Quadratic-Gaussian control problemJungD. W.TsiotrasP.HigginsW. T.Jr.A comparison of complementary and Kalman filteringVasconcelosJ. F.SilvestreC.OliveiraP.BatistaP.CardeiraB.Discrete time-varying attitude complementary filterProceedings of the American Control Conference (ACC '09)June 2009405640612-s2.0-7044967501110.1109/ACC.2009.5159879LaiY.-C.JanS.-S.HsiaoF.-B.Development of a low-cost attitude and heading reference system using a three-axis rotating platformChenZ.