An Adaptive Altitude Information Fusion Method for Autonomous Landing Processes of Small Unmanned Aerial Rotorcraft

Abstract

This paper presents an adaptive information fusion method to improve the accuracy and reliability of the altitude measurement information for small unmanned aerial rotorcraft during the landing process. Focusing on the low measurement performance of sensors mounted on small unmanned aerial rotorcraft, a wavelet filter is applied as a pre-filter to attenuate the high frequency noises in the sensor output. Furthermore, to improve altitude information, an adaptive extended Kalman filter based on a maximum a posteriori criterion is proposed to estimate measurement noise covariance matrix in real time. Finally, the effectiveness of the proposed method is proved by static tests, hovering flight and autonomous landing flight tests.

1. Introduction

With the ability to land vertically, small unmanned aerial rotorcraft (SUAR) have an irreplaceable role in civil applications [1]. Thus, they have been widely used in many areas, including road traffic monitoring, city building surveillance and power line inspection, etc. [2,3].

SUAR is a complex multi-input and multi-output (MIMO) system. Compared with the hovering and straight flight processes, there exists land disturbance in the landing process [4]. High performance altitude information is the basis factor for SUARs to realize stable landing control [5]. Due to the constraints of weight and size, sensors with low size and low performance are often used by SUARs, including micro-electronic mechanic system (MEMS) accelerometers and gyroscopes, barometers, the global positioning system (GPS) and ultrasonic sensors.

Integrated by the Euler equations, or quaternions, SUAR can get the corresponding aircraft attitude angles and position information, however, inertial sensors, especially gyroscopes, have fixed bias, drift bias, asymmetric scale factor errors and temperature-varying biases, causing the integration results to drift from true attitude [6]. GPS can provide absolute position and velocity information [7], but GPS information is easily affected by sources of interference [8]. Furthermore, GPS has low data frequency to get position and velocity information for a SUAR system [9]. Based on the relationship between the air pressure and the altitude, barometers can provide altitude information [10], but they are easily affected by wind disturbances, air fluctuations, and temperature [11]. Ultrasonic sensors are also often used in SUAR systems. Although they can provide high performance altitude measurements, they have measurement region limitations. When the altitude of SUAR surpasses the upper bound of ultrasound, the measurement results will have errors, therefore all the current sensors have limitation for SUAR to realize stable landing control.

Using filter methods, system can get high performance information based on different sensors. The most used filtering method is the extended Kalman filter (EKF) [12]. With the predict and update theory, Beard has used EKF to realize attitude acquisition for a unmanned aerial vehicle [13]. Nevertheless, poor performance or even divergence arising from the linearization implicit in EKF has led to the development of other filters [14]. The unscented Kalman filter (UKF) is also used in UAV systems [15]. Based on second or higher-order approximations of nonlinear functions, UKF can estimate the mean and covariance of state vectors [16]. With UKF, Seung realized target relative position and velocity determinations for follower UAV systems [17]. However, UKF is sensitive to the statistical distribution of the stochastic processes [18]. Based on the concept of sequential sampling and Bayesian theory, particle filtering (PF) is also used in dealing with nonlinear and non-Gaussian noise in SUAR systems. Kamrani used PF for efficient path planning of a UAV [19], but its computational demands are too complex. Wavelet analysis has also been widely used for its time and frequency domain convenience and it can effectively eliminate high frequency noise. Tsiotras used a wavelet transform to construct an approximation of the environment at different levels for small UAVs [20].

Inspired by the discussion above, an adaptive extended Kalman filter (AEKF) method based on the wavelet filter is proposed to get high performance altitude information for a SUAR during the autonomous landing process. The wavelet decomposition and reconstruction method is used to restrain the high frequency noise in the barometer, ultrasonic and GPS sensor information. Since the measurement noise is greatly changed after wavelet filtering, an AEKF based on a maximum a posteriori criterion is proposed to estimate the measurement noise matrix in real time to get high performance altitude information.

The paper is organized as follows: the dynamic model of the SUAR system is described in Section 2. The wavelet decomposition and reconstruction method is presented in Section 3. In Section 4, an AEKF based on a maximum a posteriori criterion is proposed to improve altitude information. The simulation and test results confirm the effectiveness of the proposed method in Section 5. Finally, conclusions are drawn in Section 6.

2. The Dynamic Model of SUAR

2.1. The State Model

For SUAR, altitude information is mainly controlled by the main rotor speed and longitudinal cyclic input. Therefore, the simple altitude dynamic model for SUAR can be defined as:

$$\{\begin{array}{c}{\dot{x}}_1=f_1=x_2\hfill \\ {\dot{x}}_2=f_2=a_0+a_1{x}_2+a_2{x}_2^2+(a_3+a_4{x}_4-\sqrt{a_5+a_6{x}_4})x_3^2\hfill \\ {\dot{x}}_3=f_3+u_1=a_7+a_8{x}_3+(a_{9}sin{x}_4+a_{10})x_3^2+u_1\hfill \\ {\dot{x}}_4=f_4=x_5\hfill \\ {\dot{x}}_5=f_5+u_2=a_{11}+a_{12}{x}_4+a_{13}{x}_3^{2}sin{x}_4+a_{14}{x}_5+u_2\hfill \end{array}$$

(1)

where x_i = (h, ḣ, w, θ, θ̇)^T, i = 1,⋯,5. The term h is the estimated altitude measured by barometer, GPS, and ultrasonic sensors, w is rotor speed of main rotor, θ is the longitudinal angle of rotor blade speed, u = (u₁, u₂)^T is the corresponding input, u₁ and u₂ are throttle and collective input respectively, playing an important role in longitudinal cyclic input, lateral cyclic input and blade speed. a_j(j = 1,2,⋯,15) are unknown identification parameters, obtained by the adaptive genetic method [21]. Therefore, the altitude state model of SUAR can be defined as follows:

$$\dot{h}=f(h,w,\theta ,\dot{\theta })$$

(2)

2.2. The Measurement Model

The output accuracy of a barometer is mainly affected by the high frequency noise and constant error which is related to air pressure and temperature. The high frequency noise can be restrained largely by a wavelet filter. Thus, the barometer output h_b mainly includes a constant error ε_b and measurement noise v₁. The function of the h_b can be defined as follows:

$$h_b=h+{\varepsilon }_b+v_1$$

(3)

where h is the altitude of the SUAR system.

With the location method of the ranging interchange theory, DGPS can provide position information for SUAR systems with sub-meter performance. The output of DGPS can be defined as follows:

$$h_g=h+v_2$$

(4)

where h_g is the output of DGPS, and v₂ is measurement noise

Ultrasonic sensors can provide high performance altitude information from 0.15 m to 6.05 m, and the error is less than 1 millimeter. When the altitude surpasses the upper limitation, the output of ultrasonic sensor fluctuates greatly. Therefore, the output of the ultrasonic sensor can be defined as follows:

$$h_u=\{\begin{array}{ll}{w}_{lu}\hfill & h\ge 6.05m\hfill \\ w_u\hfill & 0\le h<6.05\hfill \end{array}$$

(5)

where h_u is the output of ultrasonic sensor, w_u is the random error within the bounds and w_lu is random error without bounds.

When a SUAR finishes a certain task at low altitude, there exists land disturbance causing an increase of barometer error. Ultrasonic sensors can provide high precision altitude information for SUARs at low altitude, therefore the measurement matrix can be constructed with inputs from different sensors.

Case 1

If the integrated navigation altitude is larger than 6 m, the SUAR is beyond the range of the ultrasonic sensor. The output of barometer and DGPS are fused. Thus, the constant error of barometer sensor can be revised by the DGPS. The measurement equation can be defined as:

$$\left[\begin{array}{c}{h}_b\\ h_g\end{array}\right]=\left[\begin{array}{ll}1\hfill & 1\hfill \\ 1\hfill & 0\hfill \end{array}\right]\left[\begin{array}{c}h\\ {\varepsilon }_b\end{array}\right]+\left[\begin{array}{c}{v}_1\\ v_2\end{array}\right]$$

(6)

Case 2

If the integrated navigation altitude is less than 6 m, the barometer is easily affected by land disturbance. The output of DGPS and ultrasonic sensor are used to construct the measurement vector. The measurement equation can be defined as follows:

$$\left[\begin{array}{c}{h}_g\\ h_u\end{array}\right]=h+\left[\begin{array}{c}{v}_2\\ w_u\end{array}\right]$$

(7)

Therefore, the measurement equation can be expressed as:

$$Z_k=H_k{X}_k+V_k$$

(8)

where H_k is the state vector, Z_k is the measurement vector, H_k is the measurement matrix and V_k is the measurement noise vector. With the wavelet filter, the high frequency noises can be largely eliminated. Then, using the AEKF based on maximum a posteriori criterion, the altitude h and the constant error ε_b can be estimated unbiasedly. The whole procedure is shown in Figure 1.

3. The Wavelet Decomposition and Reconstruction Method

To get high precision altitude information, it is necessary to use a data filter to deal with high frequency noises in the output of barometer sensor, DGPS, and ultrasonic sensor. Wavelet analysis is a time and frequency domain method, having good representation for partial signal characteristics, therefore, a wavelet filter is used here as a tool to reduce high frequency noises in the sensor information. Lifting-based wavelet transform implementation has shown high potential in reducing the number of computations, so it is used to reduce computation burden in real tasks. It includes three steps:

• Split: splitting the original signal s^j = {s^j,k|0 ≤ k < 2^j} into even and odd ones. That is:

  $$\{\begin{array}{l}\mathit{\text{Split}}(s^j)=(E_{j-1},O_{j-1})\hfill \\ E_{j-1}=\{E_{j-1,k}=s_{j,2k}\},O_{j-1}=\{O_{j-1,k}=s_{j,2k+1}\}\hfill \end{array}$$

  (9)

• Predict: defining the detailed representation characteristics by choosing a predictor:

  $$O_{j-1}-=P(E_{j-1})$$

  (10)

• Update: averaging the signal of rough representation against original signal:

  $$E_{j-1}+=U(O_{j-1})$$

  (11)

The basic principle of lifting scheme is to factorize the polyphase matrix of a wavelet filter into a sequence of alternating upper, lower triangular matrices and a diagonal matrix with constants. The factorization is obtained by using an extension of the Euclidean algorithm. The resulting formulation can be implemented by means of banded matrix multiplications.

Suppose that the z-transform of wavelet filter h = {h_k, k ∈ Z} can be defined as h(z). Let ĥ(z) and ĝ(z) be the low and high pass analysis filters, and h(z), g(z) be the low and high pass synthesis filters. ĥ(z), ĝ(z), h(z) and g(z) are biorthogonal filters. The filters can be divided into even and odd parts as:

$$\{\begin{array}{l}\widehat{h}(z)={\widehat{g}}_e(z^2)+z^{-1}{\widehat{g}}_o(z^2)\hfill \\ \widehat{g}(z)={\widehat{g}}_e(z^2)+z^{-1}{\widehat{g}}_o(z^2)\hfill \\ h(z)=h_e(z^2)+z^{-1}{h}_o(z^2)\hfill \\ g(z)=g_e(z^2)+z^{-1}{g}_o(z^2)\hfill \end{array}$$

(12)

The polyphase matrices are then defined as:

$$\{\begin{array}{c}\widehat{P}(z)=\left[\begin{array}{ll}{\widehat{h}}_e(z)\hfill & {\widehat{h}}_o(z)\hfill \\ {\widehat{g}}_e(z)\hfill & {\widehat{g}}_o(z)\hfill \end{array}\right]\\ P(z)=\left[\begin{array}{ll}{h}_e(z)\hfill & h_o(z)\hfill \\ g_e(z)\hfill & g_o(z)\hfill \end{array}\right]\end{array}$$

(13)

If the (ĥ, ĝ) is a complementary filter pair, then P̂(z) can be factored as follows:

$$P(z)=\prod _{i=m}^m\left[\begin{array}{cc}1& s_i(z)\\ 0& 1\end{array}\right]\left[\begin{array}{cc}1& 0\\ t_i(z)& 1\end{array}\right]\left[\begin{array}{cc}K& 0\\ 0& K^{-1}\end{array}\right]$$

(14)

where K is a constant value.

Therefore, the low pass samples are multiplied by the time domain equivalent of s_i(z), and are added to the high pass samples. Then, the updated high-pass samples are multiplied by the time domain equivalent of t_i(z) and are added to the low-pass samples. If a diagonal matrix is present in the factorization, the low pass coefficients are multiplied by K and the high-pass coefficients are multiplied by K⁻¹. The polyphase-based wavelet transform in lifting scheme is shown in Figure 2.

In this paper, the wavelet “db4” is utilized to construct the wavelet method. The coefficients of the filter are shown in

Table 1. The coefficients of the “db4” filter.

h0	0.48296291314453	g0	0.12940952255126
h1	0.83651630373780	g1	0.22414386804201
h2	0.22414386804201	g2	−0.83651630373780
h3	−0.12940952255126	g3	0.48296291314453

.

The comparisons of original data and the wavelet filtered data of barometer and DGPS are shown in Figure 3. Obviously, the wavelet method can filter out the high frequency noise effectively.

4. The Adaptive Extended Kalman Filter

Since the measurement noise structure has changed greatly after wavelet filtering, experiential value or the statistics of partial noise cannot be used to provide a good description of measurement noise covariance, therefore, an AEKF is proposed to estimate the measurement noise covariance in real time to improve altitude information.

Since the nonlinear dynamic equation of SUAR is continuous and the measurements are a discrete series, a continuous-discrete EKF is proposed to fuse altitude sensor information. In EKF, the state equation and measurement equation can be expressed as:

$$\{\begin{array}{l}\dot{X}(t)=f(X(t),t)+B(t)u(t)+W(t)\hfill \\ Z_k=H_k{X}_k+V_k\hfill \end{array}$$

(15)

where, k is the number of time step. X(t) and Z_k are the state vector and measurement vector respectively. f(X(t), t) is nonlinear ordinary differential equations, and H_k is the measurement matrix. B(t) is the input matrix, u(t) is the input vector, W(t), V_k are the system noise and measurement noise vector respectively. Besides, the system noise and the measurement noise are uncorrelated, and the system noise can be treated as Gaussian white noise.

In EKF, measurement noise covariance matrix R plays an important role in obtaining a converged filter result. If the value of R is small, unreliable results will be obtained, and a big value of the diagonal elements of R can lead to filter divergence. In traditional EKF, the measurement noise is treated as Gaussian white noise, however, the measurement noise structure has changed greatly after wavelet filtering. Using traditional experiment values or partial statistics of sampling data as measurement noise matrix, the filtering speed will become slow, and filtering performance will become bad, therefore, the sub-optimal and unbiased maximum a posteriori method is proposed to estimate R in real time. As shown in Equation (21), the current R is updated by the innovation ε_k and R at previous time. The filter consists of the following stages:

• The prediction stage:

  $${\widehat{X}}_{k/k-1}={\widehat{X}}_{k-1}+{\left\{f\left[{\widehat{X}}_{k-1},t_{k-1}\right]+B(t_{k-1})u(t_{k-1})\right\}}^T$$

  (16)
  $$P_{k/k-1}={\varnothing }_{k,k-1}{P}_{k-1}{\varnothing }_{k,k-1}^T+Q_{k-1}$$

  (17)
  $${\widehat{Z}}_{k/k-1}=H_k{\widehat{X}}_{k-1}$$

  (18)

• The update stage:

  $${\varepsilon }_k=Z_k-{\widehat{Z}}_{k/k-1}$$

  (19)
  $${\widehat{X}}_k={\widehat{X}}_{k/k-1}+K_{k-1}{\varepsilon }_{k-1}$$

  (20)
  $$R_k=\left(1-\frac{1}{k}\right)R_{k-1}+\frac{1}{k}({\varepsilon }_k{\varepsilon }_k^T-H_k{P}_k{H}_k^T)$$

  (21)
  $$K_k=P_{k/k-1}{H}_k^T\left[H_k{P}_{k/k-1}{H}_k^T+R_k\right]$$

  (22)
  $$P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T$$

  (23)
  where X̂_k/k−1 is the predicted measurement vector for the next epoch, Ẑ_k/k−1 and P_k/k−1 are the predicted measurement vector and the predicted state covariance matrix respectively. Ø_k,k−1 is the transition matrix after discretization. The innovation ε_k is the difference between the real observations and its estimated values. T is the sampling time. Q_k is the system noise covariance matrix. K_k is the gain matrix, P_k is the estimated state covariance matrix, and R_k is the covariance matrix of measurement noise based on the maximum a posteriori adaptive method. Using ε_k, $H_k{P}_{k/k-1}{H}_k^T$ and initial experiment value R₀, the measurement noise covariance matrix can be estimated in real time to improve filtering performance.

5. Experiment

5.1. Hardware System

Experiments were conducted on a radio-controlled Raptor 90 helicopter, shown in Figure 4. The SUAR is 1.3 m length and 1.46 m span. The total weight is 5 kg, including two liter fuels, a light weight DGPS receiver, and radio telemeter system. The SUAR is powered by a piston engine running on a mixture of methanol and oil. Five servos are used to control the tail, the longitudinal cyclic input, the lateral cyclic input, the collective and the throttle. The longitudinal vertical direction can be stabilized by using the collective and pitch cyclic. Meanwhile, the lateral direction can be controlled by using the roll-cyclic and collective. The heading can be controlled by the tail.

For SUAR, there exist weight and size constraints for onboard control components. Thus, a micro guidance navigation control (MGNC) system with little weight was self-developed to realize stable control. The MGNC is only 207 g in weight, with a size of 120 mm × 61 mm × 48 mm. It consists of a horizontal main board, housing three angular rate sensors, two 2-axis accelerometers and a barometer. The barometer, DGPS, and ultrasonic sensor are used to provide altitude information for the SUAR system. The MPXA6115 barometer, produced by Freescale Semiconductor Company, has a range of 15 kPa∼115 kPa. The DGPS module employs the Novatel RTK, whose position accuracy is about 0.02 m, and the range of the Mini-S electrostatic ultrasonic transducer is from 0.15 m to 6.05 m.

5.2. Static Distance Test

To test the effectiveness of the proposed information fusion method, a static distance test has been done on the stairs. A six-floor building is chosen as the basis for its high precision measurement. Three marking points are selected on the stairs. The distances between points and ground have been tested by flexible rulers and the distances are 5.12 m (first point), 9.32 m (second point) and 13.52 m (third point). The SUAR is stretched to measure the distance between the current point and the ground. Besides, the sampling time is 60 s per point.

The comparison result of the proposed method and the real altitude, the output of the barometer and the real altitude, the output of the DGPS and the real altitude, the output of the ultrasonic sensor and the real altitude are shown in Figure 5. The measurement results of each sensor are shown in

Table 2. Altitude accuracies for AEKF and each sensor (in meters).

	AEKF	Barometer	Ultrasonic	DGPS
Max. absolute error	0.31	0.32	4.94	1.49
Mean error	0.08	0.14	0.44	0.49
Standard deviation	0.098	0.16	0.94	0.41

. It is easy to see that the proposed AEKF method has the best performance. Since there exist barriers for GPS in the building, the maximum error reaches to 1.49 m. Without airflow disturbance, the barometer can provide good measurement results, and the standard deviation is 26 percent of the DGPS after initial alignment. The ultrasonic sensor can provide high performance altitude information under 6 m. The mean error at first point is below 0.11 m. When altitude surpasses the 6 m, the performance of ultrasonic is decreased greatly.

5.3. Hovering Flight Test

To test the dynamic performance of the proposed method, a hovering flight test has been done on the SUAR system. Under a 3.4 m/s wind disturbance, the SUAR hovers in the air at 10 m altitude. The LQR control method has been used to adjust altitude and position in real time [22]. Since the planned altitude surpasses the upper limit of ultrasonic sensor, DGPS and barometer are used to provide altitude information for the SUAR system. The altitude generated by the AEKF method, barometer and DGPS are shown in Figure 6. The mean error of the adaptive EKF is only 0.214 m, and the standard deviation is 0.169 m. With the proposed AEKF, the SAUR can realize stable hovering control. Furthermore, it is easy to see that the altitude measured by the barometer fluctuates greatly. Since there exists wind disturbance, the fluctuation of barometer surpasses 1 m. Without shelter, the output of DGPS can provide high performance measurement in short periods. With the fluctuation of the star number, the DGPS output is not so reliable.

5.4. Autonomous Landing

To test the effectiveness of the proposed method, a series of autonomous landing tests have been done on the SUAR system with the adaptive radial basis function neural network and pilot model. When the SUAR received an autonomous landing command, it changed work station, and flew to the planned hovering point (0,0,10). To satisfy the criteria for position error, speed error and heading error, the SUAR hovered at the planned hovering point. With the constant adjustment for the planned hovering altitude, SAUR descends with hovering stations. Finally, the SUAR landed on the ground. Ten landing tests were conducted from different altitudes, while the wind velocity was less than 3 m/s. The landing results are shown in Figure 7. With the proposed adaptive altitude information fusion method, the SUAR can realize stable autonomous landings, and the average Euclidean distance from the landing target is about 0.67 m. Compared with the navigation system with camera [23,24], the SUAR can get achieve similar landing performance.

The comparison of landing performance with AEKF and KF [13] which fuses SINS and DGPS is shown in Figure 8. Using the AEKF, SUAR realized a stable autonomous landing with 0.75 m and 0.45 m error in the East and North directions from the planned landing point. Compared with the KF, the AEFK has much better performance in the autonomous landing process. The altitude, attitude and velocity of the autonomous landing using AEKF are shown in Figure 8(b–d) respectively.

6. Conclusions

In this paper, an adaptive information fusion method based on wavelet decomposition and reconstruction is proposed to improve the accuracy and reliability of altitude measurement information in the landing process for a SUAR. With the proposed method, the high frequency noises in sensors can be eliminated greatly, and then high performance altitude information can be fused to provide support for SUAR in the autonomous landing process. The effectiveness of the proposed method has been demonstrated by static tests, hovering tests and a series of autonomous landing tests.