This paper presents the development of a low cost miniature navigation system for autonomous flying rotary-wing unmanned aerial vehicles (UAVs). The system incorporates measurements from a low cost single point GPS and a triaxial solid state inertial/magnetic sensor unit. The navigation algorithm is composed of three modules running on a microcontroller: the sensor calibration module, the attitude estimator, and the velocity and position estimator. The sensor calibration module relies on a recursive least square based ellipsoid hypothesis calibration algorithm to estimate biases and scale factors of accelerometers and magnetometers without any additional calibration equipment. The attitude estimator is a low computational linear attitude fusion algorithm that effectively incorporates high frequency components of gyros and low frequency components of accelerometers and magnetometers to guarantee both accuracy and bandwidth of attitude estimation. The velocity and position estimator uses two cascaded complementary filters which fuse translational acceleration, GPS velocity, and position to improve the bandwidth of velocity and position. The designed navigation system is feasible for miniature UAVs due to its low cost, simplicity, miniaturization, and guaranteed estimation errors. Both ground tests and autonomous flight tests of miniature unmanned helicopter and quadrotor have shown the effectiveness of the proposed system, demonstrating its promise in UAV systems.

1. Introduction

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 [1] or camera [2] to get drift-free and high dynamical navigation information. In the loosely coupled GPS/SINS integrated navigation system for unmanned helicopter [1], 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 [3] 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 [6] 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 71 mm × 46 mm × 36 mm, corresponding to 250 grams and 103 mm × 66 mm × 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 (±300/s), three orthogonally mounted MEMS accelerometers (±18 g), three orthogonally mounted solid state magnetometers (±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 −40C to 125C. The microcontroller digitalizes, lowpass 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~2 g. 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 -axis gyro and -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 [9] 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 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 (±40 ppm/C) are much smaller than those caused by bias temperature coefficient (0.01/sec/C) and noises (0.05/sec/ 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 [10]. We rely on the recursive least square based ellipsoid hypothesis calibration algorithm [11] to estimate the center (biases) and radiuses (scale factors) of the ellipsoid. The calibration procedure is as follows.(1)Initialize and , where is the magnitude of earth’s gravity field or magnetic field and is the six-order identity matrix.(2)Rotate the sensor triad along three axes and form , , where , , and are the raw outputs of triaxial sensors. Compute , , and . Repeat Step 2 until converges.(3)Compute sensor biases , , and , and scale factors , , and . Using the estimated bias and scale factor, the calibrated sensor output can be computed as , where .

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 [5] or inexpensive solid state sensors such as accelerometers and magnetometers [12] 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 [13], reduced vector observation model multiplicative extended Kalman filter (RMEKF) [14], quaternion Kalman filter (QKF) [15], unscented Kalman filter (UKF) [16], and particle filter (PF) [17]. 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 is a constant vector expressed in n-coordinate, and is the same vector expressed in b-coordinate, we have where is the rotation matrix from n-coordinate to b-coordinate by following the rotation sequence of , , and . The yaw angle , pitch angle , and roll angle are Euler angles. and are the abbreviations of and .

Since is constant, by derivation of , we have where 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 [9] to blend the complementary characteristics of rate gyros and gyro-free attitude determination systems. Vector can be directly estimated by low-pass filtering the vector observations and high-pass filtering the integration of gyro outputs as follows: where is the parameter that adjusts the cut-off frequency of filter. Equation (4) can be transformed into discrete time domain as follows: where  s is the filter operation cycle. Using (6), we can derive the gravity field complementary filter and the magnetic field complementary filter as follows: where and are the estimated gravity field and magnetic field in b-coordinate. and are the parameters that adjust cut-off frequency of filters. is the calibrated output of triaxial accelerometers, is the translational acceleration, and is the gravity field measurement. Most attitude estimation systems [68, 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. We rely on the low-pass nature of complementary filter to reduce the influence of high frequency translational acceleration. In flight control, we reduce the translational acceleration by smoothing flight action of UAV. is the calibrated output of triaxial magnetometers.

Compared to EKF [13], RMEKF [14], QKF [15], UKF [16], and PF [17], 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: where 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 and vector contain additive errors and , we have Defining , taking the time derivative of , and substituting (3), (5), and (9) in it, we have Expending the first and second term of (10) and using Young’s inequality, we have And therefore where , . Equation (12) can be rewritten as Equation (13) means if , . Otherwise, if , . Therefore, the estimation error is bounded.

Besides, is the key parameter that affects the error bound of filter. A larger helps to reduce the error induced by , but it also increases the bandwidth of the low-pass filter in (4), leading to poor suppression of accelerometer noises and magnetometer noises. Hence 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, and are set to 0.5, corresponding to cut-off 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  rad/s. The initial attitude was systematically taken as 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  deg/s. The standard deviations of gyro, accelerometer, and magnetometer were set to 0.6 deg/s, 0.0981 m/s2, and 4.8 mGauss.

We compare our algorithm with a novel quaternion Kalman filter (QKF for short) [15], 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 , which is defined as the angle of the small rotation that brings estimated attitude to true attitude , is employed to represent the estimation error. The angle is obtained as follows. First, the error quaternion of estimated attitude and true attitude is computed using equation , where is the conjugate of , and is the quaternion product. Then, the rotation angle is computed from scalar component of using equation . Note that is always positive. Figure 4 shows the Monte-Carlo means of 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 [18] 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 . Figure 7 shows that the estimated angles can well trace the high bandwidth reference angle, and the estimation errors fell within 0.5.

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. Figure 8 shows that the estimation errors are within 1 RMS under the conditions of and , 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: where is the translational acceleration of accelerometer expressed in earth centered earth fixed Cartesian coordinate (e-coordinate). is the rotation matrix from n-coordinate to e-coordinate. and are longitude and latitude. is the gravity field expressed in n-coordinate, where gravity acceleration can be computed using the International Gravity Formula. 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. is the velocity of accelerometer expressed in e-coordinate and can be approximated by the estimated , 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: where is the position vector from center of earth to GPS antenna. is the position vector from center of earth to accelerometers. 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 where is the velocity of GPS antenna in e-coordinate measured by GPS receiver. 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 is zero. can be approximated by . Considering the delayed measurements of GPS, the velocity measurements of accelerometers in e-coordinate can be expressed as where can be computed using (2) and (16).  s is the time delay of GPS measurement, including the pseudo-range measurement delay and communication delay. 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, can be estimated by a complementary filter as follows: where is the filter gain.  s is the filter operation cycle. Similarly, the position can be estimated by another complementary filter as follows: where is the position of GPS antenna measured by GPS receiver. is the filter gain.  s is the filter operation cycle. Finally, the velocity and position for flight control can be computed as follows: where and are the estimated velocity and position of vehicle’s COG (center of gravity). 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, , , and are required. is computed using (15) from AHRS measurements, and and 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 and 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.

7. Conclusion

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.