A miniature integrated navigation system for rotary-wing unmanned aerial vehicles.
Due to the special abilities of hovering, vertical take-off and landing (VTOL), and low speed flying, rotary-wing unmanned aerial vehicles (UAVs), such as unmanned helicopters and multipropeller aerial robots, are particularly well suited for applications such as surveillance, search and rescue, aerial photography and video, security operation, traffic monitoring, and mapping. These applications lead to increasing demand for cheap and easy to operate rotary-wing UAVs. As rotary-wing UAVs are often inherently instable and highly dynamical, the permanent controls of attitude, velocity, and position are demanded. The precondition for such control system is the accurate, highly dynamical, reliable, and especially continuously available navigation information.
Until recently, most UAVs use MEMS gyros and accelerometers as inertial sensors due to limits of cost and payload. In many practical applications, these low accuracy sensors are usually integrated with aiding sensors such as GPS  or camera  to get drift-free and high dynamical navigation information. In the loosely coupled GPS/SINS integrated navigation system for unmanned helicopter , the continuous availability and dynamic performance of navigation information are assured by strapdown inertial sensors (SINS), and the SINS drift is corrected using GPS position and velocity. However, as the GPS measurements do not directly contain attitude information and the attitude is estimated from the coupling of velocity and attitude direction-cosine matrix (DCM), the observability of attitude is poor. Although observability of attitude can be improved by utilizing maneuver flights  or multiple GPSs [4, 5], it is not practical or economical for low cost rotary-wing UAVs flying in hovering mode. Meanwhile, the attitude estimation errors may diverge during GPS outage. Therefore, some systems use accelerometers' gravity observation and magnetometers' magnetic observation to guarantee attitude estimation accuracy, by either switching to attitude estimation mode during GPS outage  or adopting cascaded integrated structure [7, 8] instead of loosely coupled structure. In cascaded integrated structure, gyros, accelerometers, and magnetometers form an attitude heading reference system (AHRS), and the outputs of AHRS and GPS are used to estimate velocity and position. Compared to loosely coupled structure, although the estimated navigation information of cascaded structure is suboptimal, the attitude accuracy can be guaranteed at cost of little performance loss. Moreover, the cascaded low-dimensional filters can be independently designed and tuned, resulting in easier implementation and lower computational complexity. However, the Kalman filter (KF) and extended Kalman filter (EKF) adopted by these systems are still time-consuming for resource limited microcontrollers.
This study aims at developing a miniature navigation system for rotary-wing UAVs under limitations of cost and computing resource. To improve the accuracy of imprecise accelerometers and magnetometers, an ellipsoid hypothesis calibration algorithm which can effectively estimate sensor biases and scalefactors without any additional calibration equipment has been designed and implemented. Instead of KF and EKF, the motion states of vehicle are estimated by a series of linear complementary filters, leading to much easier implementation and less computation overheads. Besides, we implement a prototype of the navigation system and evaluate its performance in real environments, including ground tests and autonomous flight experiments of a miniature unmanned helicopter and a quadrotor. The experimental results show that the proposed system is sufficient for flight control of rotary-wing UAVs.
The rest of paper is organized as follows. Section 2 illustrates the hardware design of navigation system. The sensor calibration algorithm is presented in Section 3. Sections 4 and 5 elaborate on the attitude estimation algorithm and the velocity/position estimation algorithm, respectively. The experimental results are presented in Section 6. We conclude the work in Section 7.
2. Hardware Design
The navigation system we have developed is shown in Figure 1. The weight and dimension of the uncased system are 68 grams and 71mm x 46 mm x 36 mm, corresponding to 250 grams and 103 mm x 66 mm x 46 mm with aluminum casing. The tested power consumption is less than 2.4 W. The system consists of a triaxial solid state inertial/magnetic sensor unit (ADIS16405), a GPS receiver (OEMV-1G), and a microcontroller (STM32F103).
The ADIS16405 sensor unit is selected mainly due to its high density integration, low cost, well temperature and misalignment calibration, and miniature size. The sensor unit contains three orthogonally mounted MEMS gyroscopes ([+ or -] 300[degrees]/s), three orthogonally mounted MEMS accelerometers ([+ or -] 18 g), three orthogonally mounted solid state magnetometers ([+ or -] 3.5 Gauss), and one microcontroller. The sensitivity, bias, alignment, and linear acceleration effect of each sensor are factory calibrated within the temperature range of -40[degrees]C to 125[degrees]C. The microcontroller digitalizes, low-pass flitrates, and calibrates the sensor outputs. It also provides a SPI interface for data collection and configuration control.
For rotary-wing UAVs flying at low speed and hovering state, the accuracy and latency of velocity measurements are important issues. Therefore we choose the OEMV-1G card from NovAtel as GPS receiver. The velocity is computed from instantaneous Doppler measurements, resulting in lower latency and noise compared to velocity measurements based on delta phase.
The calibration and navigation algorithms are implemented on an ARM Cortex-M3 based microcontroller operating at 72 MHz. The microcontroller connects to the sensor unit via a SPI interface and to the GPS receiver via an UART interface. The computed navigation information is logged to flight computer via a RS232 interface.
To apply navigation system to rotary-wing UAVs, reliable vibration rejection is the first and foremost problem to be settled. For helicopter, vibration comes from main rotor (around 29.1 Hz for Hirobo 90), engine (around 230 Hz), and tail rotor (around 138.9 Hz). The amplitude is usually around 1~2g. For quadrotor, vibration comes from four rotors (around 40 Hz). Due to the smaller rotors and symmetrical structure, its amplitude is smaller than helicopter's amplitude. These vibrations will significantly degrade the performance of navigation system and make electronics more prone to failures. To reduce the influence of vibration, a two-level vibration rejection scheme has been adopted. The first level is an optional mechanical vibration damper (S type GEL-Bush from Taica corporation) with a cutoff frequency of 90 Hz. The main objective of this level is to protect electronics and connectors from vibrations. The second level is a digital scheme, which samples sensor outputs at high sample rate (819.2 Hz) and reduces sensor bandwidth to low frequency (16 Hz) using low-pass digital filters. As a result, the noises can be completely sampled by high frequency sampler and effectively reduced by low-pass digital filters. Figure 2 shows the outputs of x-axis gyro and z-axis accelerometer mounting on a vibrating table, which vibrates vertically with frequency of 40 Hz and amplitude of 1.5 g. The proposed system can effectively reduce vibration noises and outperforms the comparison system  which utilizes 18 Hz mechanical dampers, 50 Hz samplers, and 5 Hz low-pass digital filters.
3. Sensor Calibration
The errors of orthogonally mounted sensors can be classified into two kinds: the mechanical errors and the electrical errors. The mechanical errors consist of axis nonorthogonal (axis to axis, 90[degrees] ideal) errors and axis misalignment (axis to IC package) errors, which are constant during normal operation. For ADIS16405, these errors have been well factory calibrated.
The electrical errors consist of bias errors and scale factor errors. The biases and scale factors of gyroscopes are mainly temperature dependent. For the gyros of ADIS16405, the errors caused by sensitivity temperature coefficient ([+ or -] 40ppm/[degrees]C) are much smaller than those caused by bias temperature coefficient ([+ or -] 0.01[degrees]/sec/[degrees]C) and noises (0.05[degrees]/sec/[square root of Hz] rms). Therefore we only calibrate the biases of gyroscopes by averaging gyro outputs when UAVs are stationary. The biases and scale factors of accelerometers are also temperature dependent and need to be frequently calibrated.
The errors of magnetometers are mainly caused by hard iron, soft iron, and manufacturing process. Since UAVs are flying in the sky, the small and transient errors caused by hard iron and soft iron outside UAVs can be treated as high frequency noises, which can be significantly reduced by low-pass effect of attitude fusion algorithm. By not using soft iron materials in UAVs, the soft iron errors, which vary with respect to magnitude and direction of geomagnetic field, can be eliminated. The hard iron errors inside UAVs stem from airframe and avionics. These errors are constant after installation and may bias the outputs of magnetometers. The signal conditioning circuits and AD conversions also affect the biases and scale factors of magnetometers' outputs. In brief, the bias and scale factor errors dominate the errors of magnetometers, and calibration must be performed once the position of magnetometers with respect to airframe and avionics is changed.
For convenience of frequent calibrations, calibration method without additional equipment is demanded. Since the gravity and magnetism of earth measured by accelerometers and magnetometers are constant within area of few kilometers, the locus of outputs for an error-free accelerometer triad or magnetometer triad is a sphere, which is centered at origin with a radius equal to the magnitude of gravity or magnetism. The bias errors will shift the center of the basic locus, while the sensitivity errors will reshape the sphere into an ellipsoid. Hence the problem of accelerometer or magnetometer calibration is equivalent to the problem of estimating the center and radiuses of an ellipsoid that best fit the outputs of triaxial sensors . We rely on the recursive least square based ellipsoid hypothesis calibration algorithm  to estimate the center (biases) and radiuses (scale factors) of the ellipsoid. The calibration procedure is as follows.
(1) Initialize [c.sub.0] = [[0 1 0 1 0 -[R.sup.2]].sup.T] and [P.sub.0] = 1000 x [I.sub.6], where R is the magnitude of earth's gravity field or magnetic field and [I.sub.6] is the six-order identity matrix.
(2) Rotate the sensor triad along three axes and form [y.sub.n] = [-2[[xi].sub.rx] [[xi].sup.2.sub.ry] -2[[xi].sub.ry] [[xi].sup.2.sub.rz] -2[[xi].sub.rz] 1], [z.sub.n] = - [[xi].sup.2.sub.rx], where [[xi].sub.rx], [[xi].sub.ry], and [[xi].sub.rz] are the raw outputs of triaxial sensors. Compute K = [P.sub.n-1][y.sup.T.sub.n]/(1 + [y.sub.n][P.sub.n-1][y.sup.T.sub.n]), [P.sub.n] = ([I.sub.6] - K[y.sub.n])[P.sub.n-1], and [c.sub.n] = [c.sub.n-1] + K([z.sub.n] - [y.sub.n][c.sub.n-1]). Repeat Step 2 until [c.sub.n] converges.
(3) Compute sensor biases [b.sub.x] = [c.sub.n](1), [b.sub.y] = [c.sub.n](3)/[c.sub.n](2), and [b.sub.z] = [c.sub.n](5)/[c.sub.n](4), and scale factors [MATHEMATICAL EXPRESSION NOT REPRODUCIBLE IN ASCII]. Using the estimated bias and scale factor, the calibrated sensor output can be computed as [[xi].sub.i] = ([[xi].sub.ri] - [b.sub.i])/[s.sub.fi], where i = x, y, z.
The main advantages of the algorithm are the removal of additional calibration equipment and the convenience of frequent calibrations. Figure 3 shows that the locus of calibrated magnetometer outputs coincides well with the unit sphere, demonstrating the effectiveness of the calibration algorithm. As will be mentioned in Section 4.4, the calibration brings impressive improvement in yaw angle estimation compared with the well-known digital compass HMR3300.
4. Attitude Estimation
Attitude estimation is indispensable to attitude stabilization of rotary-wing UAV. Both static accuracy and bandwidth must be concerned. Although the outputs of rate gyros can be integrated to generate high bandwidth attitude estimation, the long-term accuracy is poor due to integration errors. The gyro-free attitude determination systems use multiantenna GPS  or inexpensive solid state sensors such as accelerometers and magnetometers  to derive error bounded attitude estimation. However, the bandwidth is limited. Hence the outputs of rate gyros are generally fused with the outputs of gyro-free attitude determination systems to guarantee both long-term accuracy and bandwidth of attitude. Many fusion algorithms have been developed, such as EKF , reduced vector observation model multiplicative extended Kalman filter (RMEKF) , quaternion Kalman filter (QKF) , unscented Kalman filter (UKF) , and particle filter (PF) . Although these algorithms can achieve satisfactory performance, they are impractical to be implemented on microcontroller due to high computational complexity. Hence we developed a linear fusion algorithm with low computational complexity for attitude estimation.
4.1. The Proposed Algorithm. The Euler angle and quaternion are commonly used attitude representations. However the computations of trigonometric function and root function are time consuming for microcontroller. In our algorithm, we use earth's gravity field and magnetic field in body fitted coordinate (b-coordinate) to represent attitude. When there is no translational acceleration, the triaxial accelerometers and magnetometers measure earth's gravity field and magnetic field in b-coordinate, which are constant in local geodetic coordinate (n-coordinate). Assuming [x.sup.n] is a constant vector expressed in n-coordinate, and [x.sup.b] is the same vector expressed in b-coordinate, we have
[x.sup.b] = [C.sup.b.sub.n][x.sup.n], (1)
[MATHEMATICAL EXPRESSION NOT REPRODUCIBLE IN ASCII], (2)
where [C.sup.b.sub.n] is the rotation matrix from n-coordinate to b-coordinate by following the rotation sequence of z, y, and x. The yaw angle [psi], pitch angle [theta], and roll angle [phi] are Euler angles. sx and cx are the abbreviations of sin x and cos x.
Since [x.sup.n] is constant, by derivation of [x.sup.b], we have
[MATHEMATICAL EXPRESSION NOT REPRODUCIBLE IN ASCII], (3)
where [[omega].sup.b] is the angular velocity of b-coordinate relative to n-coordinate expressed in b-coordinate. Since rate gyros offer high frequency attitude information and gyro-free attitude determination systems offer low frequency attitude information, we employ complementary filter  to blend the complementary characteristics of rate gyros and gyro-free attitude determination systems. Vector [x.sup.b] can be directly estimated by low-pass filtering the vector observations [x.sup.b.sub.m] and high-pass filtering the integration of gyro outputs [[omega].sup.b.sub.m] as follows:
[MATHEMATICAL EXPRESSION NOT REPRODUCIBLE IN ASCII], (4)
where k is the parameter that adjusts the cut-off frequency of filter. Equation (4) can be transformed into discrete time domain as follows:
[MATHEMATICAL EXPRESSION NOT REPRODUCIBLE IN ASCII], (5)
[MATHEMATICAL EXPRESSION NOT REPRODUCIBLE IN ASCII], (6)
where [T.sub.a] = 0.01 s is the filter operation cycle. Using (6), we can derive the gravity field complementary filter and the magnetic field complementary filter as follows:
[MATHEMATICAL EXPRESSION NOT REPRODUCIBLE IN ASCII], (7)
where [MATHEMATICAL EXPRESSION NOT REPRODUCIBLE IN ASCII] are the estimated gravity field and magnetic field in b-coordinate. [k.sub.g] and [k.sub.B] are the parameters that adjust cutoff frequency of filters. [f.sup.b.sub.m] = [a.sup.b.sub.m] - [g.sup.b.sub.m] is the calibrated output of triaxial accelerometers, [a.sup.b.sub.m] is the translational acceleration, and [g.sup.b.sub.m] is the gravity field measurement. Most attitude estimation systems [6-8, 13, 16] use accelerometer to measure gravity field. As accelerometer cannot distinguish gravitational acceleration from translational acceleration, the gravity field measurement may be corrupted by translational acceleration of UAV. We take two measures to reduce the neglect influence of translational acceleration. (1) We rely on the low-pass nature of complementary filter to reduce the influence of high frequency translational acceleration. (2) In flight control, we reduce the translational acceleration by smoothing flight action of UAV. [B.sup.b.sub.m] is the calibrated output of triaxial magnetometers.
Compared to EKF , RMEKF , QKF , UKF , and PF , the main advantages of complementary filter are the simpler structure, the lower computational complexity, and the easier filter tuning.
Once the gravity field and magnetic field in b-coordinate are estimated, the attitude can be computed as follows:
[MATHEMATICAL EXPRESSION NOT REPRODUCIBLE IN ASCII], (8)
where [alpha] is the magnetic declination computed from the international geomagnetic reference field (IGRF) or world magnetic model (WMM) using position information.
4.2. Robustness of the Algorithm. The previous complementary filter works well provided that we have perfect measurements of vectors and angular velocity. In this subsection it will be demonstrated that the filter continues to be valid with erroneous measurements. Assuming that the measurements of angular velocity [[omega].sup.b.sub.m] and vector [x.sup.b.sub.m] contain additive errors [delta][[omega].sup.b.sub.m] and [delta][x.sup.b.sub.m], we have
[MATHEMATICAL EXPRESSION NOT REPRODUCIBLE IN ASCII]. (9)
Defining V = [[??].sup.T][??], taking the time derivative of V, and substituting (3), (5), and (9) in it, we have
[MATHEMATICAL EXPRESSION NOT REPRODUCIBLE IN ASCII]. (10)
Expending the first and second term of (10) and using Young's inequality, we have
[MATHEMATICAL EXPRESSION NOT REPRODUCIBLE IN ASCII]. (11)
[??] [less than or equal to] -[lambda]V + [delta], (12)
where [lambda] = (5k - 2)/3, [delta] = 6[[parallel][delta][[omega].sup.b.sub.m][parallel].sup.2.sub.[infinity]] [[parallel][x.sup.b][parallel].sup.2.sub.2] + 3k[[parallel][delta][x.sup.b.sub.m][parallel].sup.2.sub.2]. Equation (12) can be rewritten as
[MATHEMATICAL EXPRESSION NOT REPRODUCIBLE IN ASCII], (13)
[MATHEMATICAL EXPRESSION NOT REPRODUCIBLE IN ASCII]. (14)
Equation (13) means if V(0) > [sup.sub.0 [less than or equal to] i [less than or equal to] t]([delta](i)/[lambda]), V(t) < V(0). Otherwise, if V(0) [less than or equal to] [sup.sub.0 [less than or equal to] i [less than or equal to] t]([delta](i)/[lambda]), V(t) [less than or equal to] [sup.sub.0 [less than or equal to] i [less than or equal to] t]([delta](i)/[lambda]). Therefore, the estimation error is bounded.
Besides, k is the key parameter that affects the error bound of filter. A larger k helps to reduce the error induced by [delta][[omega].sup.b.sub.m], but it also increases the bandwidth of the low-pass filter in (4), leading to poor suppression of accelerometer noises and magnetometer noises. Hence k should be tuned by comprehensively considering the precision of gyro, the noises of accelerometer and magnetometer, the vibration intensity of airframe, and the magnetic interference induced by avionics. In our system, [k.sub.g] and [k.sub.B] are set to 0.5, corresponding to cutoff frequency of 0.08 Hz.
4.3. Simulation Study. In order to evaluate the performance of the algorithm, extensive Monte-Carlo simulations were performed. In all simulations, the vehicle rotated with an angular velocity vector given by [0.3 sin([pi]t) 0.3 cos([pi]t) 0.2 sin(0.4[pi]t)] rad/s. The initial attitude was systematically taken as [[0.9975 -0.0358 0.0358 0.0492].sup.T] expressed in quaternion. The gyro bias was modeled as a first-order Gauss Markov process with time constant of 300 s and driving process noise deviation of 200 deg/h. The initial gyro biases were set to [0.1 -0.1 0.05] deg/s. The standard deviations of gyro, accelerometer, and magnetometer were set to 0.6 deg/s, 0.0981 m/[s.sup.2], and 4.8 mGauss.
We compare our algorithm with a novel quaternion Kalman filter (QKF for short) , which employs a special manipulation on measurement equation to eliminate usual linearization procedure, resulting in better performance than typical extended Kalman filter for quaternion estimation. The error quaternion angle [delta][PHI], which is defined as the angle of the small rotation that brings estimated attitude [??] to true attitude [q.sub.t], is employed to represent the estimation error. The angle is obtained as follows. First, the error quaternion [delta]q of estimated attitude [??] and true attitude [q.sub.t] is computed using equation [delta]q = [[??].sup.*] [cross product] [q.sub.t], where [[??].sup.*] is the conjugate of [??], and [cross product] is the quaternion product. Then, the rotation angle [delta][PHI] is computed from scalar component of [delta]q using equation [delta][PHI] = 2arccos([delta][q.sub.0]). Note that [delta][PHI] is always positive. Figure 4 shows the Monte-Carlo means of [delta][PHI] over 100 runs. The two algorithms possess similar accuracy, while the computation time of the proposed algorithm (16.89 seconds per run on a 3.0 GHz AMD Phenom II X4 PC) is much less than that of QKF (160.64 seconds per run). This shows the feasibility of the proposed algorithm for microcontroller implementation.
4.4. Implementation and Ground Tests. In this subsection the previous algorithm is implemented on the STM32F103 microcontroller. The measurements of rate gyros, accelerometers, and magnetometers onboard ADIS16405 are acquired via SPI bus with a period of 0.01 s. These measurements are fused using (7) to estimate earth's gravity field and magnetic field. Then the attitude angles are calculated using (8). The CORDIC algorithms  are adopted to accelerate the computations of the trigonometric and inverse trigonometric functions.
The ground tests of estimated pitch and roll angles were performed on a 3-axis flight motion simulator as shown in Figure 5. Figure 6 shows that the estimated pitch and roll angles possess high static accuracy and good repeatability. In the dynamic test, both pitch and roll angles of the simulator changed simultaneously by following the excitation signal of 10[degrees] sin(10t). Figure 7 shows that the estimated angles can well trace the high bandwidth reference angle, and the estimation errors fell within 0.5[degrees].
Due to ferromagnetic interference, the estimated yaw angle cannot be tested on the 3-axis flight motion simulator. So only the static precision test was performed on a circular disk which is marked every 45[degrees]. Figure 8 shows that the estimation errors are within 1[degrees] RMS under the conditions of [phi] = 0[degrees] and [phi] = 20[degrees], much smaller than the errors of well-known digital compass HMR3300 from Honeywell.
5. Position and Velocity Estimation
Due to the large integral error, the AHRS derived acceleration can only be used to improve the bandwidth of velocity and position. Therefore, sensor fusion techniques must be employed to bound the integral error using drift-free GPS velocity and position. The acceleration measurements from AHRS, in turn, are used to smooth the GPS velocity and position. In our approach, two complementary filters are employed to blend the complementary characteristics of AHRS derived acceleration and GPS derived velocity and position.
5.1. The Proposed Algorithm. Once the attitude is estimated, the translational acceleration can be computed as follows:
[a.sup.e.sub.A] = [C.sup.e.sub.n]([C.sup.n.sub.b][f.sup.b.sub.m] + [g.sup.n]) - 2[[omega].sup.e.sub.ie] x [v.sup.e.sub.A], (15)
[MATHEMATICAL EXPRESSION NOT REPRODUCIBLE IN ASCII], (16)
where [a.sup.e.sub.A] is the translational acceleration of accelerometer expressed in earth centered earth fixed Cartesian coordinate (e-coordinate). [C.sup.e.sub.n] is the rotation matrix from n-coordinate to e-coordinate. [lambda] and [phi] are longitude and latitude. [g.sup.n] = [[0 0 g].sup.T] is the gravity field expressed in n-coordinate, where gravity acceleration g can be computed using the International Gravity Formula. [[omega].sup.e.sub.ie] is the angular velocity of e-coordinate relative to inertial coordinate (i-coordinate) expressed in e-coordinate, which is equal to the earth's rotation rate. [v.sup.e.sub.A] is the velocity of accelerometer expressed in e-coordinate and can be approximated by the estimated [[??].sup.e.sub.A], for the Coriolis acceleration induced by the vehicle moving on the rotating earth is small.
The GPS measures the velocity and position of GPS antenna relative to e-coordinate. Since the accelerometers and the GPS antenna are not installed on the same position, the lever arm effect must be considered. The relationship between the position of accelerometers and GPS antenna can be expressed as follows:
[r.sub.OG] = [r.sub.OA] + [r.sub.AG], (17)
where [r.sub.OG] is the position vector from center of earth to GPS antenna. [r.sub.OA] is the position vector from center of earth to accelerometers. [r.sub.AG] is the position vector from accelerometers to GPS antenna. Taking time derivative of (17) and using relationship of absolute derivative and relative derivative, we have
[MATHEMATICAL EXPRESSION NOT REPRODUCIBLE IN ASCII], (18)
where (d[r.sub.OG]/dt)|[sub.e] = [v.sup.e.sub.G] is the velocity of GPS antenna in e-coordinate measured by GPS receiver. (d[r.sub.OA]/dt)|[sub.e] = [v.sup.e.sub.A] is the velocity of accelerometers in e-coordinate, which is used for sensor fusion. Since the relative position of GPS antenna and accelerometers in b-coordinate is constant, the term (d[r.sub.AG]/dt)|[sub.b] is zero. [[omega].sup.b] can be approximated by [[omega].sup.b.sub.m]. Considering the delayed measurements of GPS, the velocity measurements of accelerometers in e-coordinate can be expressed as
[v.sup.e.sub.A,m](t) = [v.sup.e.sub.G](t - d) + ([[omega].sup.e.sub.ie] - [C.sup.e.sub.b](t - d)[[omega].sup.b.sub.m] (t - d)) x [C.sup.e.sub.b](t - d)[r.sup.b.sub.AG] + [[integral].sup.t.sub.t-d][a.sup.e.sub.A]([tau])d[tau], (19)
where [C.sup.e.sub.b] = [C.sup.e.sub.n][C.sup.n.sub.b] can be computed using (2) and (16). d = 0.04 s is the time delay of GPS measurement, including the pseudo-range measurement delay and communication delay. [r.sup.b.sub.AG] is the position vector from accelerometers to GPS antenna expressed in b-coordinate. Based on (15), (19), and the integral relationship between acceleration and velocity, [v.sup.e.sub.A] can be estimated by a complementary filter as follows:
[MATHEMATICAL EXPRESSION NOT REPRODUCIBLE IN ASCII], (20)
where [k.sub.v] = 1 is the filter gain. [T.sub.v] = 0.01 s is the filter operation cycle. Similarly, the position can be estimated by another complementary filter as follows:
[MATHEMATICAL EXPRESSION NOT REPRODUCIBLE IN ASCII], (21)
[p.sup.e.sub.A,m](t) = [p.sup.e.sub.G](t - d) - [C.sup.e.sub.b](t - d)[r.sub.AG] + [[integral].sup.t.sub.t- d][[??].sup.e.sub.A]([tau])d[tau], (22)
where [p.sup.e.sub.G] is the position of GPS antenna measured by GPS receiver. [k.sub.p] = 0.5 is the filter gain. [T.sub.p] = 0.01 s is the filter operation cycle. Finally, the velocity and position for flight control can be computed as follows:
[MATHEMATICAL EXPRESSION NOT REPRODUCIBLE IN ASCII], (23)
where [[??].sup.e.sub.COG] and [[??].sup.e.sub.COG] are the estimated velocity and position of vehicle's COG (center of gravity). [r.sup.b.sub.ACOG] is the position vector from accelerometers to vehicle's COG expressed in b-coordinate.
5.2. Implementation and Ground Tests. In this subsection the previous algorithms are implemented to solve the problem of velocity and position estimation. As it has been previously said, [a.sup.e.sub.A], [v.sup.e.sub.A,m], and [p.sup.e.sub.A,m] are required. [a.sup.e.sub.A] is computed using (15) from AHRS measurements, and [v.sup.e.sub.A,m] and [p.sup.e.sub.A,m] are computed using (19) and (22). Since the update rate of AHRS is much higher than that of GPS, the complementary filters run at a rate equal to the update rate of AHRS, and the terms [p.sup.e.sub.A,m](n) - [[??].sup.e.sub.A](n) and [v.sup.e.sub.A,m](n) - [[??].sup.e.sub.A](n) are zero-order held when GPS is not updated.
Our objective is to increase the dynamic performance of GPS derived velocity and position by blending high bandwidth acceleration measurements. The dynamic performance was tested through the back and forth movement of navigation system. Figure 9 shows the evolution of the estimated velocity and position. Due to budget constraints, we cannot afford expensive RTK GPS or other ground truth measuring systems for comparison. However, by representing the velocity, position derived from acceleration integration, GPS, and the proposed fusion algorithm, one can observe that the proposed algorithm takes advantage from both short-term dynamic performance of acceleration integration and long-term precision of slowly updated GPS measurements.
6. Flight Test of Rotary-Wing UAVs
We conducted a series of autonomous flight experiments to illustrate the performance of the designed system. Generally, the accuracy of navigation system should be evaluated by high performance inertial navigation system or real-time kinematic differential GPS (RTK DGPS). However, the two prototype UAVs are not suitable to carry these systems for comparison due to their cost and payload capacity. Due to open loop unstable and high bandwidth dynamical characteristics of rotary-wing UAVs, the controlled UAVs are prone to oscillation and divergence, once the estimated motion states cannot reflect the real motion states. In other words, the flight control system has very high demand for both accuracy and bandwidth of the estimated attitude, velocity, and position. Therefore, although the ground truth information is not available for these experiments to evaluate the absolute accuracy, the effectiveness of the designed navigation system can still be verified by the control performance of rotary-wing UAVs in real flights.
6.1. The Test UAVs. The proposed navigation system has been equipped on two rotary-wing UAVs to evaluate its performance in real flights. One is the miniature unmanned helicopter shown in Figure 10, which is modified from a Hirobo Freya 90 RC helicopter. To reduce vibration induced from the single cylinder, 2-stroke engine and rotors, the proposed navigation system and flight computer are installed on four mechanical vibration dampers. The navigation information is transmitted to flight computer via RS232 interface. The other is the home-made prototype quadrotor shown in Figure 11. The PCB of navigation system is directly installed on the mainframe, as the vibration induced by four symmetrical rotors is small. And to reduce weight, the microcontroller also plays the role of flight computer.
6.2. Flight Test Results. Figures 12 and 13 show the autonomous hovering flight results of the miniature unmanned helicopter and the quadrotor, respectively. The position control errors are within 1.1 m RMS, and the controlled velocities are stable and small, demonstrating the effectiveness of the proposed navigation system in hovering flights.
Figure 14 shows the autonomous trajectory flight results of miniature unmanned helicopter. The helicopter took off manually and switched to autonomous flight mode at waypoint A and then successively flew over waypoints B, C, D, and E. The flight from current waypoint to next waypoint is divided into steady turn mode and forward flight mode, and the maximum forward flight speed was 5 m/s. Finally the human pilot took over the helicopter at waypoint E and performed a manual landing. The estimated attitude, velocity, and position in Figure 14 can well reflect the flight states of helicopter and the corresponding control errors are also small. These results do show the static accuracy and dynamic performance of the designed navigation system.
During the autonomous waypoint flight of quadrotor, the vehicle took off automatically and hovered at waypoint A, then flew over waypoints B, C, D, A, E, F, G, H, and E, successively, with maximum speed of 1.5 m/s, and finally landed automatically at waypoint E. Figure 15 shows the trajectory, velocity, and attitude during flight. These results also validate the performance of the designed navigation system.
In this paper, a low cost miniature navigation system for rotary-wing UAVs was designed and implemented. To obtain accurate and high bandwidth navigation information, a series of sensor calibrations and multisensor fusion algorithms were developed. First, the accelerometers and magnetometers were calibrated using the recursive least square based ellipsoid hypothesis calibration algorithm, which eliminates the requirement for additional calibration equipment. Then, a low computational linear attitude fusion algorithm was developed and proven to be error bounded with respect to additive sensor errors. Finally, two cascaded complementary filters were designed, and the translational acceleration in e-coordinate, the GPS velocity, and position are used to drive the filters for bandwidth improvement of velocity and position. The main advantages of the designed navigation system are its low cost, simplicity, miniaturization, and bounded estimation errors. The system shows remarkable static accuracy and dynamic performance in ground tests. The effectiveness of the system was further validated in autonomous flights of miniature unmanned helicopter and quadrotor, demonstrating its promise in UAV systems.
Conflict of Interests
The authors declare that there is no conflict of interests regarding the publication of this paper.
This work is supported by the NSFC under Grants 61303211, 60970118, and 61373166, the Zhejiang Provincial Natural Science Foundation of China under Grant LQ13F020010, and the Foundation of Zhejiang Educational Committee under Grant Y201223581. A preliminary version of this paper was presented at the 8th World Congress on Intelligent Control and Automation. This version is reorganized and includes new design on the navigation system, improved algorithm, more specific analyses, and more ground tests and flight experiments for system evaluation.
 R. van der Merwe and E. A. Wan, "Sigma-point kalman filters for integrated navigation," in Proceedings of the 60th Annual Meeting of the Institute of Navigation, vol. 14, pp. 641-654, Dayton, Ohio, USA, June 2004.
 X. Song, L. D. Seneviratne, and K. Althoefer, "A Kalman filter-integrated optical flow method for velocity sensing of mobile robots," IEEE/ASME Transactions on Mechatronics, vol. 16, no. 3, pp. 551-563, 2011.
 I. Rhee, M. F. Abdel-Hafez, and J. L. Speyer, "Observability of an integrated GPS/INS during maneuvers," IEEE Transactions on Aerospace and Electronic Systems, vol. 40, no. 2, pp. 526-535, 2004.
 F. Aghili and A. Salerno, "Driftless 3-D attitude determination and positioning of mobile robots by integration of IMU with two RTK GPSs," IEEE/ASME Transactions on Mechatronics, vol. 18, no. 1, pp. 21-31, 2013.
 M. Moore, C. Rizos, and J. Wang, "A GPS based attitude determination system for a UAV aided by low grade angular rate gyros," in Proceedings of the 16th International Technical Meeting of the Satellite Division of the U.S. Institute of Navigation, pp. 2417-2424, 2003.
 J. Wendel, O. Meister, C. Schlaile, and G. F. Trommer, "An integrated GPS/MEMS-IMU navigation system for an autonomous helicopter," Aerospace Science and Technology, vol. 10, no. 6, pp. 527-533, 2006.
 S. Tang, X. Lu, and Z. Zheng, "Platform and state estimation design of a small-scale UAV helicopter system," International Journal of Aerospace Engineering, vol. 2013, Article ID 524856, 13 pages, 2013.
 S. de La Parra and J. Angel, "Low cost navigation system for UAV's," Aerospace Science and Technology, vol. 9, no. 6, pp. 504-516, 2005.
 Y. Xu, Q. Ren, W. Sun, and P. Li, "Design and implementation of low cost integrated navigation system for mini autonomous helicopter," in Proceedings of the 8th World Congress on Intelligent Control and Automation (WCICA '10), pp. 6842-6847, July 2010, (Chinese).
 D. Gebre-Egziabher, G. H. Elkaim, J. D. Powell, and B. W. Parkinson, "Calibration of strapdown magnetometers in magnetic field domain,"
Journal of Aerospace Engineering, vol. 19, no. 2, pp. 87-102, 2006.
 Y. Xu, Q.-Y. Ren, W.-D. Sun, and P. Li, "A geomagnetic navigation algorithm for miniature unmanned helicopter," Acta Armamentarii, vol. 32, no. 3, pp. 337-342, 2011 (Chinese).
 D. Gebre-Egziabher, R. C. Hayward, and J. D. Powell, "Design of multi-sensor attitude determination systems," IEEE Transactions on Aerospace and Electronic Systems, vol. 40, no. 2, pp. 627-649, 2004.
 J. S. Jang and D. Liccardo, "Automation of small UAVs using a low cost MEMS sensor and embedded computing platform," in Proceedings of the 25th Digital Avionics Systems Conference, pp. 1-9, 2006.
 B. Liu, Z. Chen, X. Liu, and F. Yang, "An efficent nonlinear filter for spacecraft attitude estimation," International Journal of Aerospace Engineering, vol. 2014, pp. 1-11.
 D. Choukroun, I. Y. Bar-Itzhack, and Y. Oshman, "Novel quaternion Kalman filter," IEEE Transactions on Aerospace and Electronic Systems, vol. 42, no. 1, pp. 174-190, 2006.
 H. G. de Marina and F. J. Pereda, "UAV attitude estimation using unscented Kalman filter and TRIAD," IEEE Transactions on Industrial Electronics, vol. 59, no. 11, pp. 4465-4474, 2012.
 J. Lim and D. Hong, "Cost reference particle filtering approach to high-bandwidth tilt estimation," IEEE Transactions on Industrial Electronics, vol. 57, no. 11, pp. 3830-3839, 2010.
 R. Andraka, "A survey of CORDIC algorithms for FPGA based computers," in Proceedings of the 6thACM/SIGDA International Symposium on Field Programmable Gate Arrays (FPGA '98), pp. 191-200, February 1998.
Yu Xu, (1) Wenda Sun, (2) and Ping Li (2)
(1) College of Physics and Electronic Information Engineering, Wenzhou University, Wenzhou 325035, China
(2) School of Aeronautics and Astronautics, Zhejiang University, Hangzhou 310000, China
Correspondence should be addressed to Yu Xu; email@example.com
Received 18 March 2014; Revised 12 August 2014; Accepted 12 August 2014; Published 28 August 2014
Academic Editor: Christopher J. Damaren
|Printer friendly Cite/link Email Feedback|
|Title Annotation:||Research Article|
|Author:||Xu, Yu; Sun, Wenda; Li, Ping|
|Publication:||International Journal of Aerospace Engineering|
|Date:||Jan 1, 2014|
|Previous Article:||External aerodynamics simulations in a rotating frame of reference.|
|Next Article:||Boost full bridge bidirectional DC/DC converter for supervised aeronautical applications.|