In-flight model parameter and state estimation using gradient descent for high-speed flight

Abstract

High-speed flight in GPS-denied environments is currently an important frontier in the research on autonomous flight of micro air vehicles. Autonomous drone races stimulate the advances in this area by representing a very challenging case with tight turns, texture-less floors, and dynamic spectators around the track. These properties hamper the use of standard visual odometry approaches and imply that the micro air vehicles will have to bridge considerable time intervals without position feedback. To this end, we propose an approach to trajectory estimation for drone racing that is computationally efficient and yet able to accurately estimate a micro air vehicle’s state (including biases) and parameters based on sparse, noisy observations of racing gates. The key concept of the approach is to optimize unknown and difficult-to-observe state variables so that the observations of the racing gates best fit with the known control inputs, estimated attitudes, and the quadrotor dynamics and aerodynamics during a time window. It is shown that a gradient-descent implementation of the proposed approach converges ∼4 times quicker to (approximately) correct bias values than a state-of-the-art 15-state extended Kalman filter. Moreover, it reaches a higher accuracy, as the predicted end-point of an open-loop turn is on average only ∼20 cm away from the actual end-point, while the extended Kalman filter and the gradient descent method with kinematic model only reach an accuracy of ∼50 cm. Although the approach is applied here to drone racing, it generalizes to other settings in which a micro air vehicle may only have sparse access to velocity and/or position measurements.

Introduction

Quadrotors have received considerable attention in recent years, thanks to their mechanical simplicity and good maneuverability combined with hover properties. They have offered new possibilities in a variety of fields like aerial photography, inspection and even transportation. With recent advances in on-board computation and sensor technology, aggressive maneuvering has come within reach of many applications. To further stimulate aggressive and fast flight, autonomous drone racing is gaining interest. The first ever autonomous drone race was held by the International Conference on Intelligent Robots and Systems (IROS) in 2016. ¹ A track consisting of gates had to be flown autonomously in a pre-specified order. The robot had to achieve this as fast as possible, while only relying on onboard sensors and processing. Figure 1 illustrates the setup of the 2016 indoor track.

Figure 1.

The map of the IROS 2016 drone race. In this drone race, the UAVs have to fly through orange gates in a pre-specified order as fast as possible.

Autonomous indoor drone racing brings many new challenges to the fields of quadrotor navigation and control. One initial challenge is the navigation without any external positioning system like VICON, Optitrack or GPS. Typical approaches to this problem make use of on-board cameras and use Visual Inertial Odometry to integrate position. This type of algorithms rely on integrating inertial information, tracking visual features over several frames and solving an optimization problem to retrieve the most likely solution. In autonomous drone racing, on top of this position estimation algorithm, gate detection is often needed when the position of gates is not precisely known, or when gates contain moving parts—as is the case in the IROS competitions. With the limited computational resources of small indoor drones, to achieve the fast speeds needed in drone racing, this paper proposes a navigation solution based solely on gate detection, augmented with inertial measurements and an aerodynamic model. To cope with the sometimes sparse and noisy non-Gaussian visual observations, we formulate the navigation solution as an optimization problem. We then solve it using a gradient descent method. The resulting method provides online estimation of the quadrotor position, velocity and inertial biases using less computational resources than traditional Visual Inertial Odometry. The proposed approach also estimates aerodynamic properties of the quadcopter—which become increasingly important in the case of fast aggressive control. Finally, the approach scales favorably with increasing flight speeds as it keeps performing well even with very few position updates. As a comparison, we use the Kalman filter, which is currently still the default choice for navigation. Since the extended Kalman filter (EKF) is significantly less computationally complex than the unscented Kalman filter (UKF), ² in this paper we select the EKF as a benchmark. We compare the results with EKF, which is shown to be much more sensitive to visual outliers or other non-Gaussian effects.

In the section Related work, an overview of studies on aerodynamics modeling and state estimation methods is given. The section Quadrotor model will describe the quadrotor model parameters that will be solved. The section State estimation proposes two different approaches for the visual state estimation. First a classic 15-state EKF is developed as benchmark. Then the novel FMINCON-based gradient descent optimization method is proposed to solve the model parameters and states. In the section Experiment setup and result, both algorithms are compared on flight test data and Conclusion summarizes the conclusions.

Related work

Several researchers have already proposed aerodynamics models for quadrotors.^3–7 The main object of their studies is to derive a nonlinear quadrotor aerodynamics model to improve the control performance by compensating for the nonlinear terms. In some studies, a detailed aerodynamic model is analyzed through theory and fitted by experimental data. ³ Simplified aerodynamic models are also established from experiments.^5,6 It should be noted that their models are all obtained off-line using external measurements, such as GPS, VICON and thrust test beds. Aerodynamic models can also be combined with on-board measurements, for instance from computer vision, ⁸ in order to better estimate the velocity of the drone on-line. In this article, we employ a simplified aerodynamic model in the trajectory estimation exactly for this purpose.

Quadrotor control heavily relies on attitude estimation from an attitude and heading reference system (AHRS). This system is typically based on inertial sensors (accelerometers and gyroscopes), but also relies on orientation sensors (magnetometer) and/or positioning sensors (GPS, VICON) to estimate inertial sensor biases and compensating for long term drift. Sensor biases become increasingly important as the drone will have to fly longer or temporarily perform feedforward control maneuvers in the absence of sensor measurements. Hence, for drone racing, it is important to estimate them accurately. Here we briefly discuss the sensors and then the filtering employed in estimating both attitude and position or velocity on micro air vehicles (MAVs).

Most systems intended for outdoor environments utilize the magnetometer and GPS-measurements.^9–13 The indoor equivalent is the use of a motion tracking system such as VICON or Optitrack. ¹⁴ In many applications—like autonomous drone racing, it is required to have accurate state estimation without the help of external systems. The necessary position or velocity measurements can be obtained from multiple sensors. One early option is to use laser scanners.^15,16 But a laser scanner contains sensitive optics and mirrors, which are susceptible to shock and vibration problems. ¹⁷ Another choice for on-board navigation is RGB-D devices.^18–20 The main drawback of these RGB-D devices is that their maximum depth perception range is limited to a few meters.^21,22 This is why light-weight and inexpensive on-board cameras which are more robust to vibration and shock, have attracted interest of researchers for the navigation of drones. Generally, visual odometry (VO) algorithms ²³ using a stereo camera or monocular camera are used for estimation of the MAV’s translation and rotation between frames.^24–28 However, generic visual odometry approaches necessitate detecting features, matching corresponding features and estimating motions, which leads to a heavy demand for on-board computational resources and low-frequency estimation. In the meantime, aggressive maneuvers may introduce blur into generic visual odometry and seriously affect the accuracy of estimation. Moreover, in complicated environments like drone racing, dynamic spectators may also interfere visual odometry. Less generic but computationally efficient methods are employed in some specific environments, for instance, using detection of known visual markers to determine position.^29,30 However, these methods cannot cope with other generic environments.

Concerning filtering, with white-Gaussian position measurement, Kalman filter and its variants are widely used. It is well-known that nonlinearities in the state update or observation equations can be handled by an EKF^11,12,22,31 and that heavy nonlinearities are often handled better by a UKF.^30,32,33 Also, there are factor graph-based smoothing methods which can handle nonlinearity and allows multi-rate, asynchronous, and possibly delayed measurements, which have similar performance with an EKF.^34,35 We hypothesize that when these measurements get sparser, and their noise distribution moves further away from the Gaussian distribution, it will be better to estimate the attitude, heading, and trajectory in general as an optimization problem that uses more data at a time. In particular, we want to optimize the trajectory and parameters such as sensor biases, given a specified time-window with the corresponding sensor measurements, control inputs, and knowledge of the aerodynamic model. Our approach will be explained below, starting with our dynamic quadrotor model.

Quadrotor model

Dynamic model of quadrotor

Before deriving the dynamic model for quadrotor, two reference frames are introduced (Figure 4).

Earth frame E. The origin of the local tangent earth frame is on the ground, the x-axis x^E points to north, the y-axis y^E points east and the z-axis z^E points down.

Body frame B. The origin of the body frame is at the center of mass. Its x-axis x^B is in the symmetry plane of the drone and points forward. Its z-axis z^B also lies in the symmetry plane and points downward. The y-axis y^B is directed to the right, perpendicular to the symmetry plane.

The relative relation between two frames can be expressed by three successive rotations along three axes. In this paper, we use z–y–x sequence to rotate one frame to the other. The corresponding angle of rotation is defined by ${\varphi }_E^B,{\theta }_E^B$ , and ${\psi }_E^B$ which are also called Euler angles. Given the Euler angles between the two frames, the rotation matrix between two frames can be expressed by

$${\mathfrak{R}}_E^B=\left[\begin{array}{c}\begin{array}{ccc}{C}_{\theta }{C}_{\psi }& C_{\theta }{S}_{\psi }& -S_{\theta }\\ S_{\varphi }{S}_{\theta }{C}_{\psi }-C_{\varphi }{S}_{\psi }& S_{\varphi }{S}_{\theta }{S}_{\psi }+C_{\varphi }{C}_{\psi }& S_{\varphi }{C}_{\theta }\\ C_{\varphi }{S}_{\theta }{C}_{\psi }+S_{\varphi }{S}_{\psi }& C_{\varphi }{S}_{\theta }{S}_{\psi }-S_{\varphi }{C}_{\psi }& C_{\varphi }{C}_{\theta }\end{array}\end{array}\right]$$

(1)

where C_X and S_X denote the cosine and sine of X, respectively. The control of the quadrotor is often divided in to two loops which can be independently developed, namely a high level translation loop and a faster low-level attitude loop. For the attitude loop, the inputs of the system are the four rotor speeds and the output consists of the three Euler angles. For the translation loop, the inputs of system are three Euler angles and the output is position. Since quadrotor attitude control is a well-developed topic, in this work we only derive the translational model and have used INDI from Smeur et al. ³⁶ as innerloop.

According to Newton’s laws of motion, the motion of quadrotor can be described as

$$m\dot{\mathbf{V}}=m\mathbf{g}+\mathbf{F}$$

(2)

where m is mass of the drone, g is gravity vector and F is the specific force vector. The change in position can be described by the kinematic equation

$$\dot{\mathbf{X}}=\mathbf{V}$$

(3)

In equation (2), the specific force F can be expressed in Body frame B as

$${\mathbf{F}}^B=\left[\begin{array}{l}{F}_x^B\hfill \\ F_y^B\hfill \\ F_z^B\hfill \end{array}\right]$$

(4)

Gravity acting on the center of mass and expressed in Earth frame is

$$m{\mathbf{g}}^E=m\left[\begin{array}{l}0\hfill \\ 0\hfill \\ g\hfill \end{array}\right]$$

(5)

Combining all forces yields the equations of motion in inertial frame

$$\begin{array}{l}\left[\begin{array}{l}\dot{x}\\ \dot{y}\\ \dot{z}\end{array}\right]=\left[\begin{array}{l}{v}_x\\ v_y\\ v_z\end{array}\right]\\ \left[\begin{array}{l}{\dot{v}}_x\\ {\dot{v}}_y\\ {\dot{v}}_z\end{array}\right]=\left[\begin{array}{l}0\\ 0\\ g\end{array}\right]+{\mathfrak{R}}_B^E\left[\begin{array}{l}{a}_x^B\\ a_y^B\\ a_z^B\end{array}\right]\end{array}$$

(6)

where

$$\left[\begin{array}{l}{a}_x^B\hfill \\ a_y^B\hfill \\ a_z^B\hfill \end{array}\right]=\left[\begin{array}{l}{F}_x^B\hfill \\ F_y^B\hfill \\ F_z^B\hfill \end{array}\right]/m$$

(7)

In the system above, we have six states $\mathbf{x}={[x,y,z,v_x,v_y,v_z]}^{\text{T}}$ and four inputs $\mathbf{u}={[\varphi ,\theta ,\psi ,a_z^B]}^{\text{T}}$ . In equation (6), the specific force is a nonlinear function of velocity, attitude, angular rates and other factors. It can be expressed as $\mathbf{F}={\mathbf{f}}_a(\mathbf{V},\varphi ,\theta ,\psi ,\dots )$ . This system is a multiple input multiple output nonlinear system.

IMU misalignment

Equation (6) reveals that rotation matrix ${\mathfrak{R}}_E^B$ is an essential part of the model. However, in the real world, many aspects can contribute to attitude estimation errors. A first reason is the misalignment of the IMU (see Figure 2). Assembly inaccuracy can cause the measurements of the IMU to differ from the real states in body frame. Rotor misalignment can also affect the performance of quadrotor. In an ideal quadrotor, the four rotors should be perpendicular to x_BOy_B plane. In practice however, due to installation errors or deformation of rotors or axes, the thrust produced by the rotors is not perfectly perpendicular to the x_BOy_B plane.

Figure 2.

When a quadrotor hovers, usually the average attitude of the quadrotor and reading of the AHRS are not zero. This is caused by the misalignment of both the IMU and the rotors.

Both factors lead to non-zero required attitude during hover: ${\varphi }_E^B\ne 0°$ and ${\theta }_E^B\ne 0°$ . In order to model this misalignment error, we introduce a new frame. The IMU frame I is an orthogonal frame whose three axis coincide with three axes of the accelerometers. The rotation between the IMU frame I and the body frame B can be described by Euler angles ${\mathbf{\Phi }}_I^B={[{\varphi }_I^B,{\theta }_I^B,{\psi }_I^B]}^{\text{T}}$ . The rotation matrix between the IMU frame I and the body frame B is ${\mathfrak{R}}_I^B({\mathbf{\Phi }}_I^B)$ . Since the IMU frame is physically attached to the body frame, we have the assumption

$$\{\begin{array}{l}{\dot{\varphi }}_I^B(t)=0\hfill \\ {\dot{\theta }}_I^B(t)=0\hfill \\ {\dot{\psi }}_I^B(t)=0\hfill \end{array}$$

(8)

Aerodynamic model

There are many factors that can affect the quadrotor’s aerodynamics. Some examples are the quadrotor’s velocity V, its angle of attack $\mathit{\alpha }$ , the thrust T, the rotor speed ω, the angular velocity q and so on. Accurate and complete quadrotors models can be complicated and nonlinear.^37,38 Moreover, accurate modeling also requires many more parameters to be estimated and this leads to heavier computations. In the context of autonomous drone racing we opted for a faster approach using a minimal model that covers the most important aerodynamic effects, hereby maximizing the yield for a given computational load. In particular, many drag factors—such as induced drag, translation drag and blade flapping drag—can be approximated as linear functions of body velocity $v_x^b$ and $v_y^b$ with the assumption that wind is still. ⁶ This results in the following simple lumped parameter model

$$\{\begin{array}{l}{a}_x^B=K_x{v}_x^B\hfill \\ a_y^B=K_y{v}_y^B\hfill \end{array}$$

(9)

where

$$\left[\begin{array}{l}{v}_x^B\hfill \\ v_y^B\hfill \end{array}\right]={\mathfrak{R}}_E^B(3;3)\left[\begin{array}{l}{v}_x\hfill \\ v_y\hfill \end{array}\right]$$

(10)

$a_x^B,a_y^B$ are the acceleration caused by drag in the body frame. K_x, K_y are first-order drag coefficients in body frame coordinates B and have units 1/s.

AHRS bias model

When positioning information is available, the mainstream approach for estimating attitude is merging information from gyro, accelerometer and the positioning system. For instance, the classic 15-state Kalman filter uses accelerometer and gyro measurements to predict states along with GPS measurement updates. It can provide non-biased optimal attitude by estimating the gyro and accelerometer biases as states.

When no continuous external positioning information is available, like in our experiment, a compromise is to neglect kinematic accelerations in the attitude filter. In this case, the biases of accelerometers cannot be estimated.

In the case of attitude determination with constant sensor biases and small angles, the Kalman gain in the Kalman filter typically converges to an almost constant value. To avoid the computational overhead of computing the Kalman gain, complementary filters can be used with very similar results. The structure of the complementary attitude determination filter implemented in this work can be found in Figure 3. In Figure 3, ${\mathbf{\Omega }}_m=[p_m,q_m,r_m]$ are the gyro measurements. ${\mathbf{a}}_m=[a_x^m,a_y^m,a_z^m]$ contains the accelerometer measurements and

$$\begin{array}{l}{\mathbf{R}}^{\mathbf{\prime }}=\left(\begin{array}{lll}1\hfill & \tan \theta \sin \varphi \hfill & \tan \theta \cos \varphi \hfill \\ 0\hfill & \cos \varphi \hfill & -\sin \theta \hfill \\ 0\hfill & \frac{\sin \varphi }{\cos \theta }\hfill & \frac{\cos \varphi }{\cos \theta }\hfill \end{array}\right)\hfill \end{array}$$

(11)

Figure 3.

Complementary filter for attitude determination.

Figure 3 shows that the gyroscopes are integrated and the accelerometer is used as feedback to determine attitude. The high-frequency vibrations and centripetal forces which are measured by the accelerometers cancel out on the long term when no constant non-zero accelerations are present. On the long term, the resulting attitude estimation therefore converges to

$$\left[\begin{array}{l}{\widehat{\varphi }}_a(t)\hfill \\ {\widehat{\theta }}_a(t)\hfill \end{array}\right]=\left[\begin{array}{l}\arctan \frac{-a_x^m}{-a_z^m}\hfill \\ \arctan \frac{-\cos {\widehat{\varphi }}_a(t)a_x^m}{-a_z^m}\hfill \end{array}\right]$$

(12)

where $a_x^m,a_y^m$ , and $a_z^m$ are measurements of the accelerometer in three axes.

The gyroscopes measure angular velocity in the three axes of the body frame. Because they are integrated, even small biases cause drift over time, and in this filter the gyro biases ${\mathbf{b}}_g={[b_p,b_q,b_r]}^T$ are accounted for by the k_I/s term in the filter.

Accelerometers unfortunately also suffer from biases, which is denoted by ${\mathbf{b}}_a={[b_{a_x},b_{a_y},b_{a_z}]}^T$ , for instance caused by temperature changes. Fortunately, the biases of the accelerometers only change slowly. Everything combined, the AHRS has an erroneous representation of where earth is, which is referred to as coordinate frame $E^{\prime }$ and is shown in Figure 4. The AHRS attitude is then defined as the rotation between $E^{\prime }$ and I and is denoted as ${\mathbf{\Phi }}_{E^{\prime }}^I={[{\varphi }_{E^{\prime }}^I,{\theta }_{E^{\prime }}^I,{\psi }_{E^{\prime }}^I]}^{\text{T}}$ . The corresponding rotation matrix is written as ${\mathfrak{R}}_{E^{\prime }}^I({\mathbf{\Phi }}_{E^{\prime }}^I)$ .

Figure 4.

AHRS estimation errors can be represented by an erroneous Earth reference frame $E^{\prime }$ .

The rotation between the real earth E and $E^{\prime }$ can be expressed by three Euler angles ${\mathbf{\Phi }}_E^{E^{\prime }}={[{\varphi }_E^{E^{\prime }},{\theta }_E^{E^{\prime }},{\psi }_E^{E^{\prime }}]}^{\text{T}}$ . Based on the assumption that the AHRS error changes slowly, we can assume

$$\{\begin{array}{l}{\dot{\varphi }}_E^{E^{\prime }}(t)\approx 0\hfill \\ {\dot{\theta }}_E^{E^{\prime }}(t)\approx 0\hfill \\ {\dot{\psi }}_E^{E^{\prime }}(t)\approx 0\hfill \end{array}$$

(13)

With this assumption, on the short term the rotation matrix ${\mathfrak{R}}_E^{E^{\prime }}({\mathbf{\Phi }}_E^{E^{\prime }})$ is a constant matrix.

Four reference frames have been introduced, namely E, $E^{\prime }$ , I and B. The rotation matrix ${\mathfrak{R}}_E^B$ in equation (6) can now be expressed as

$${\mathfrak{R}}_E^B({\mathbf{\Phi }}_E^B)={\mathfrak{R}}_I^B({\mathbf{\Phi }}_I^B){\mathfrak{R}}_{E^{\prime }}^I({\mathbf{\Phi }}_{E^{\prime }}^I){\mathfrak{R}}_E^{E^{\prime }}({\mathbf{\Phi }}_E^{E^{\prime }})$$

(14)

where ${\mathfrak{R}}_I^B({\mathbf{\Phi }}_I^B)$ and ${\mathfrak{R}}_E^{E^{\prime }}({\mathbf{\Phi }}_E^{E^{\prime }})$ are constant matrices and ${\mathfrak{R}}_{E^{\prime }}^I({\mathbf{\Phi }}_{E^{\prime }}^I)$ represents the attitude as determined by the AHRS.

Full model

Combining equations (6), (9), and (14), we obtain the full model as

$$\dot{\mathbf{x}}=\{\begin{array}{l}{v}_x\hfill \\ v_y\hfill \\ v_z\hfill \\ \left[\begin{array}{c}0\\ 0\\ g\end{array}\right]+{\mathfrak{R}}_B^E\left[\begin{array}{c}0\\ 0\\ a_z^B\end{array}\right]+\left[\begin{array}{ccc}{K}_x& 0& 0\\ 0& K_y& 0\\ 0& 0& 0\end{array}\right]{\mathfrak{R}}_E^B\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right]\hfill \end{array}$$

$$\begin{array}{l}{\mathfrak{R}}_E^B({\mathbf{\Phi }}_E^B)={\mathfrak{R}}_I^B({\mathbf{\Phi }}_I^B){\mathfrak{R}}_{E^{\prime }}^I({\mathbf{\Phi }}_{E^{\prime }}^I){\mathfrak{R}}_E^{E^{\prime }}({\mathbf{\Phi }}_E^{E^{\prime }}){\mathfrak{R}}_B^E={\mathfrak{R}}_E^B\text{T}\\ a_z^B=a_z^m-b_{a_z}\end{array}$$

(15)

The model in equation (15) contains the following parameters, which are assumed to be constant over short periods of time

$$\mathbf{\Theta }={[K_x,K_y,b_{a_z},{\varphi }_E^{E^{\prime }},{\theta }_E^{E^{\prime }},{\psi }_E^{E^{\prime }},{\varphi }_I^B,{\theta }_I^B,{\psi }_I^B]}^{\text{T}}$$

(16)

State estimation

To estimate the states of the model from the section Quadrotor model, two approaches are derived. As a benchmark, an EKF is developed. Secondly, a novel gradient descent based optimization method to estimate the states is proposed.

Vision-based EKF

The attitude determination Kalman filter uses the inertial sensors as inputs to predict the states of the system, then uses different observations to revise the predictions. When the system is linear, observable and the noise is white Gaussian, then it can be mathematically proven that the Kalman filter provides the optimal solution. If the system is nonlinear, it can be linearized at every time step, which is referred to as the EKF. A classic 15-state EKF is implemented as found in Gross’s work, ² the difference being that we use vision measurements instead of GPS as positioning information. The following states are used

$$\begin{array}{l}\mathbf{X}={[x,y,z]}^{\text{T}}\\ \mathbf{V}={[v_x,v_y,v_z]}^{\text{T}}\\ \mathbf{\Phi }={[\varphi ,\theta ,\psi ]}^{\text{T}}\\ {\mathbf{b}}_a={[b_{a_x},b_{a_y},b_{a_z}]}^{\text{T}}\\ {\mathbf{b}}_g={[b_p,b_q,b_r]}^{\text{T}}\end{array}$$

(17)

with as inputs

$$\begin{array}{l}{\mathbf{\Omega }}_m={[p^m,q^m,r^m]}^{\text{T}}\\ {\mathbf{a}}_m={[a_x^m,a_y^m,a_z^m]}^{\text{T}}\end{array}$$

(18)

and as observation

$$\mathbf{y}=\mathbf{h}(\mathbf{x})=\left[\begin{array}{l}x\hfill \\ y\hfill \\ z\hfill \end{array}\right]$$

(19)

The process equation is

$$\{\begin{array}{l}\dot{\mathbf{X}}=\mathbf{V}\hfill \\ \dot{\mathbf{V}}=\mathbf{g}+{\mathfrak{R}}_B^E({\mathbf{a}}_m+{\mathbf{b}}_a)\hfill \\ \dot{\mathbf{\Phi }}={\mathfrak{R}}^{\prime }({\mathbf{\Omega }}_m+{\mathbf{b}}_g)\hfill \\ {\dot{\mathbf{b}}}_a=0\hfill \\ {\dot{\mathbf{b}}}_g=0\hfill \end{array}$$

(20)

This forms a standard nonlinear system expression

$${\dot{\mathbf{x}}}^{\prime }=\mathbf{f}({\mathbf{x}}^{\mathbf{\prime }},\mathbf{u})$$

(21)

where ${\mathbf{x}}^{\prime }={[\mathbf{X},\mathbf{V},\mathbf{\Phi },{\mathbf{b}}_a,{\mathbf{b}}_g]}^{\text{T}}$ and

$$\mathbf{f}({\mathbf{x}}^{\mathbf{\prime }},\mathbf{u})=\left[\begin{array}{l}\mathbf{V}\\ \mathbf{g}+{\mathfrak{R}}_E^B({\mathbf{a}}_m+{\mathbf{b}}_a)\\ {\mathfrak{R}}^{\prime }({\mathbf{\Omega }}_m+{\mathbf{b}}_g)\\ 0\\ 0\end{array}\right]$$

(22)

The EKF follows five steps:

(1) Predict the states based on equation (20)

$${\widehat{\mathbf{X}}}_{k|k-1}={\widehat{\mathbf{X}}}_{k-1}+\mathbf{f}({\widehat{\mathbf{X}}}_{k-1},{\mathbf{u}}_{k-1})\text{T}$$

(23)

where T is sampling time.

(2) Linearize and discretize the system

$$\begin{array}{l}{\mathbf{F}}_{k-1}=\frac{\partial }{\partial \mathbf{x}}\mathbf{f}(\mathbf{x}(t),\mathbf{u}(t)){|}_{\mathbf{x}(t)={\widehat{\mathbf{x}}}_{k-1}}\\ {\Phi }_{k|k-1}\approx \hspace{0.17em}\mathbf{I}+{\mathbf{F}}_{k-1}\text{T}\\ {\mathbf{H}}_k=\frac{\partial }{\partial \mathbf{x}}\mathbf{h}(\mathbf{x}(t)){|}_{\mathbf{x}(t)={\widehat{\mathbf{x}}}_{k-1}}\end{array}$$

(24)

(3) Propagate the covariance matrix ${\mathbf{P}}_{k|k-1}$

$${\mathbf{P}}_{k|k-1}={\mathbf{\Phi }}_{k|k-1}{\mathbf{P}}_{k-1}{\mathbf{\Phi }}_{k|k-1}^{\text{T}}+{\mathbf{Q}}_{k-1}$$

(25)

where ${\mathbf{Q}}_{k-1}$ is system noise covariance matrix.

(4) Calculate the Kalman gain and update the prediction.

$$\begin{array}{l}\delta {\widehat{\mathbf{X}}}_k={\mathbf{K}}_k\left\{{\mathbf{Z}}_k-\mathbf{h}[{\widehat{\mathbf{X}}}_{k|k-1},k]\right\}\\ {\mathbf{K}}_k={\mathbf{P}}_{k|k-1}{\mathbf{H}}_k^{\text{T}}{[{\mathbf{H}}_k{\mathbf{P}}_{k|k-1}{\mathbf{H}}_k^{\text{T}}+{\mathbf{R}}_k]}^{-1}\\ {\widehat{\mathbf{X}}}_k={\widehat{\mathbf{X}}}_{k|k-1}+\delta {\widehat{\mathbf{X}}}_k\end{array}$$

(26)

where ${\mathbf{R}}_k$ is sensor noise covariance matrix.

(5) Update the covariance matrix of the state estimation error

$${\mathbf{P}}_k=(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k){\mathbf{P}}_{k/k-1}{(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k)}^{\text{T}}+{\mathbf{K}}_k{\mathbf{R}}_k{\mathbf{K}}_k^{\text{T}}$$

(27)

Vision-based gradient descent method

According to the gate detection algorithm we used in IROS 2016 autonomous drone race, the vision-based position used as observation in the Kalman filter (equation (19)) has very non-Gausian noise, which can significantly affect the estimation accuracy of Kalman filters. The vision measurement model will be discussed later. Therefore the state prediction is rewritten as a parameter optimization problem in the form of a trajectory matching problem.

Unlike the Kalman filter which estimates continuously varying states like pitch and roll for any moment in time, the proposed gradient descent using the model from equation (15) in essence estimates corrections on top of attitude estimates provided by an external complementary attitude filter.

Since most model parameters like drag and AHRS error are integrated twice to arrive at position, observing the trajectory over a period of time allows for extremely fine observations of these parameters. For instance, a sub-degree attitude error is hard to identify in noisy raw accelerometer measurements. However integrating the consequence of this small angle error, which causes a percentage of gravity to be erroneously double-integrated in the lateral position after several seconds, becomes very easily observable.

The observed trajectory is obtained from the vision pipeline and expressed as a list of n noisy measurements. The predicted trajectory is based on integrating the model presented in equation (15) using attitude from the AHRS and given a set of model parameters $\widehat{\mathbf{\Theta }}$ . The resulting trajectory becomes

$$\mathbf{F}(\mathbf{\Theta })={\displaystyle {\int }_0^t\mathbf{f}}(\mathbf{\Theta },\mathbf{u}(t),t)\text{d}t=\left[\begin{array}{l}\widehat{x}(\mathbf{\Theta },\mathbf{u}(t),t)\hfill \\ \widehat{y}(\mathbf{\Theta },\mathbf{u}(t),t)\hfill \\ \widehat{z}(\mathbf{\Theta },\mathbf{u}(t),t)\hfill \\ {\widehat{v}}_x(\mathbf{\Theta },\mathbf{u}(t),t)\hfill \\ {\widehat{v}}_y(\mathbf{\Theta },\mathbf{u}(t),t)\hfill \\ {\widehat{v}}_z(\mathbf{\Theta },\mathbf{u}(t),t)\hfill \end{array}\right]$$

(28)

The error between the predicted integrated trajectory and the vision measurements is found as

$$J(\mathbf{\Theta })={\displaystyle \sum _{i=i}^n\Vert \left[\begin{array}{l}\widehat{x}(\mathbf{\Theta },\mathbf{u}(t_i),t_i)\hfill \\ \widehat{y}(\mathbf{\Theta },\mathbf{u}(t_i),t_i)\hfill \\ \widehat{z}(\mathbf{\Theta },\mathbf{u}(t_i),t_i)\hfill \end{array}\right]-\left[\begin{array}{l}{x}_i^m\hfill \\ y_i^m\hfill \\ z_i^m\hfill \end{array}\right]}\Vert$$

(29)

where $x_i^m,y_i^m,z_i^m$ are position measurements obtained from onboard computer vision. Now the state estimation has become a nonlinear parameter optimization problem that finds a set of optimal parameters ${\mathbf{\Theta }}^{*}$ to minimize the value of $J(\mathbf{\Theta })$ which can be expressed as

$$\begin{array}{l}{\min }_{\mathbf{\Theta }}J(\mathbf{\Theta })\\ s.t.\dot{\mathbf{x}}(t)=\mathbf{f}(\mathbf{x}(t),\mathbf{u}(t))\end{array}$$

(30)

To solve the problem formulated by equation (30), we can apply many types of nonlinear optimization methods to find the optimal parameters ${\mathbf{\Theta }}^{*}$ . In this paper, we propose the gradient descent method, which is iteratively searching for optimal values in negative gradient direction until it finds the minimum point

$${\mathbf{\Theta }}_{k+1}={\mathbf{\Theta }}_k+\alpha \nabla J\left({\mathbf{\Theta }}_k\right)$$

(31)

where α is learning rate and

$$\nabla J({\mathbf{\Theta }}_k)={\left[\begin{array}{lll}\frac{\partial }{\partial {\Theta }_1}J(\mathbf{\Theta })& \cdots & \frac{\partial }{\partial {\Theta }_n}J(\mathbf{\Theta })\end{array}\right]}^{\text{T}}{|}_{\mathbf{\Theta }={\mathbf{\Theta }}_k}$$

(32)

is the gradient of $J(\mathbf{\Theta })$ .

Figure 5 shows an example of the gradient descent approach. The propagation in time of the model from equation (15) for various parameters $\mathbf{\Theta }$ is compared to the ground-truth measured by a passive external positioning system. The gradient descent starts with an initial guess of ${\mathbf{\Theta }}_0$ , and gradually gets the predicted trajectory closer to the real trajectory until an optimal set ${\mathbf{\Theta }}^{*}$ is found. In this example, we directly use Optitrack data as measurements which better illustrate how the predicted trajectories converge to the ground-truth trajectory (measured by Optitrack).

Figure 5.

A gradient descent method optimizes a set of parameters $\mathbf{\Theta }$ to best fit a predicted trajectory through a measured trajectory (blue). During the fitting phase, the gradient descent method converges to the ground-truth trajectory.

Experiment setup and result

Experiment setup

In order to study the performance of state estimation methods, a hippodrome shaped track is used with end circles with radius of 1.5 m and straights of 3 m as shown in Figure 6. Onboard flight data are recorded while flying without computer vision but based on Optitrack position. The data are then analyzed in MATLAB. A Bebop 1 (Figure 7) from Parrot is used as experiment platform. It is equipped with three gyros, three accelerometers, one sonar, one barometer, a front camera and a bottom camera. Only the front camera and IMU are used and the original stock flight-code in the drone is replaced by open-source software from the Paparazzi-UAV project. ³⁹ The AHRS runs on-board and consists of the complementary filter discussed in previous section. The flight time of the test runs is about 100s and the average flight velocity is about 1.8m/s, resulting in about 15 circles of the hippodrome. An overview of data gathered is presented in Table 1.

Figure 6.

The top view of the experiment track.

Figure 7.

The Parrot Bebop 1 hardware is used as experiment platform. All flight code is replaced with open-source Paparazzi-UAV flight code.

Figure 8.

Based on the vision measurement model (equation (33)), simulated vision measurement points (red) are generated around the real trajectory (blue). During the autonomous drone race, only the visual measurement points are available.

Figure 9.

When vision measurements are not available, the quadrotor can only rely on model predictions based on model information and inertial data. This prediction will diverge in time. The better the model prediction is, the smaller the end point prediction error Δ _f becomes.

Table 1.

Data gathered during the experiment.

Parameter	Symbol	Frequency (Hz)	Source
Acceleration	${\widehat{\mathbf{a}}}^m$	512	IMU
Angular velocity	${\widehat{\mathbf{p}}}^m$	512	IMU
Attitude	${\widehat{\mathbf{\Phi }}}^m$	512	AHRS
Position	${\widehat{\mathbf{x}}}^m$	120	Optitrack
Velocity	${\widehat{\mathbf{v}}}^m$	120	Optitrack
Altitude	zm	512	Sonar

During the IROS 2016 autonomous drone race, we used the bebop 1 onboard camera to detect the gates and provide the position measurements for navigation. In this work, however, noisy vision measurements are generated simulating on-board vision-based gate detections with various levels of accuracy. Along the straight part trajectory, n random points P _i are randomly sampled ( $15<n<20,i\in [1,n]$ ). For each sampled point P _i , we calculate the distance between P _i and the gate which is denoted by ${\widehat{x}}_i-x_g$ . Then, the noise $\Delta {\mathbf{P}}_i$ is generated depending on the distance to the gate that $\Delta {\mathbf{P}}_i$ is larger when the gate is further away. Finally, $\Delta {\mathbf{P}}_i$ is added to P _i to get the simulated measurements ${\mathbf{P}}_i^v$ . This process can be described by equation (33) (Figure 8)

$$\begin{array}{l}{\mathbf{P}}_i^v={\mathbf{P}}_i^m+\Delta {\mathbf{P}}_i\\ \Delta {\mathbf{P}}_i\sim \mathbf{N}(0,{\mathbf{S}}_i)\\ {\mathbf{S}}_i=\left[\begin{array}{lll}{\sigma }_i^2\hfill & 0\hfill & 0\hfill \\ 0\hfill & {\sigma }_i^2\hfill & 0\hfill \\ 0\hfill & 0\hfill & {\sigma }_i^2\hfill \end{array}\right]\\ {\sigma }_i=0.1({\widehat{x}}_i-x_g)\end{array}$$

(33)

The test flights consist of two distinct phases which are shown in Figure 6.

During the straight part (blue line), the gates are in the field of view of the quadrotor and vision-based position measurements are available. The vision-based EKF (VEKF) can run both prediction and update loops. The vision-based gradient descent method (VGD) searches for parameters $\mathbf{\Theta }$ that make the prediction best fit the noisy measurements.

During the arc (purple line), no position measurements are available but an open-loop coordinated turn is performed. The VEKF can only rely on model prediction and the gradient descent method uses the last estimated parameters and on-board inertial data to propagate the states of the quadrotor. This phase must be limited in time as the open-loop integration is diverging as can be seen in Figure 9.

The test track is designed to resemble an autonomous drone race track, where it is not possible to keep gates in sight at all times. When using fast gate detection as sole means of position information, some maneuvers need to be performed open-loop. But even when gates are in-sight, better model prediction allows the estimation of more accurate trajectories through the noisy visual data. Therefore, as a performance index we selected the prediction error Δ _f at the final point of the open-loop arc to evaluate the performance of both algorithms.

$${\Delta }_f=\Vert \left[\begin{array}{l}{x}_f\\ y_f\end{array}\right]-\left[\begin{array}{l}{\widehat{x}}_f\\ {\widehat{y}}_f\end{array}\right]\Vert$$

(34)

where x_f and y_f, which are from Optitrack, form the ground truth of the end point of the arc, while ${\widehat{x}}_f$ and ${\widehat{y}}_f$ are the filter prediction of the end point.

Analysis of VGD

In this section, we use the on-board flight data and generated vision measurements to analyze the VGD using a MATLAB implementation of gradient descent, FMINCON.

The performance of the gradient descent method is affected by the size of the training data. It is important to investigate how the size of the dataset used to search for ${\mathbf{\Theta }}^{*}$ affects the estimation performance. We use the notation γ ( $1\le \gamma \le 5$ ) to represent the size of the history used by FMINCON. In other words, γ is the number of straight lines whose corresponding vision measurement is used by FMINCON. Too short γ will contain very few visual measurements and the approach is at risk of over-fitting the gate detection noise. Too long γ will violate the constant parameter constraint like for instance equation (13). Figure 10 shows an example where γ = 2. For each step, we use an array of flight data and vision measurements of size γ in FMINCON to search for ${\mathbf{\Theta }}^{*}$ . Then, ${\mathbf{\Theta }}^{*}$ is used to estimate the trajectory of next arc, which is given by id τ ( $1\le \tau \le 15$ ). Finally the final point error ${}_{\gamma }^{\tau }{\Delta }_f$ can be calculated using equation (34)

$$\gamma \tau {\Delta }_f=\Vert \left[\begin{array}{l}{x}_f\hfill \\ y_f\hfill \end{array}\right]-\left[\begin{array}{l}{\widehat{x}}_f\hfill \\ {\widehat{y}}_f\hfill \end{array}\right]\Vert$$

(35)

Figure 10.

Example test flight data showing the x position in function of time and illustrating the prediction strategy when γ = 2. First, the data of straight lines 1 and 2 are used to estimate ${\mathbf{\Theta }}^{*}$ . Then the identified model parameters are used to predict the second turn. Finally, the final point error after the second arc ${}_2^2{\Delta }_f$ is calculated. Here, subscript 2 means the data from 2 straight lines are used and superscript 2 means second arc’s trajectory prediction is used. This procedure is repeated by using data of straight lines 2 and 3 and predicting the trajectory of third arc and so forth. (a) Final point error ${}_{\gamma }^{\tau }{\Delta }_f$ in function of γ for various parts of the run τ. (b) Number of FMINCON iterations based on stopping criteria (equation (36)) in function of γ for various parts of the run τ.

The stopping criteria used in the FMINCON optimization is

$$\frac{\Vert J\left({\mathbf{\Theta }}_k\right)-J\left({\mathbf{\Theta }}_{k-1}\right)\Vert }{\Vert J\left({\mathbf{\Theta }}_k\right)\Vert }\le {10}^{-4}$$

(36)

With different combinations of τ and γ, a set of 70 ${}_{\gamma }^{\tau }{\Delta }_f$ is gathered. The prediction accuracy results, ${}_{\gamma }^{\tau }{\Delta }_f$ , and the number of iterations based on the stopping criteria from equation (36) are shown in Figure 11.

Figure 11.

Influence of the history length γ on the prediction accuracy ${}_{\gamma }^{\tau }{\Delta }_f$ and required number of iterations.

Figure 11(a) shows the prediction accuracy ${}_{\gamma }^{\tau }{\Delta }_f$ as a function of the history length γ. Each gray dot represents an individual arc estimation τ on another part of the data while the blue dots give the average for a given γ. Similarly, in Figure 11(b) the required number of iterations based on the stopping criteria is shown. The figures show that the prediction error ${}_{\gamma }^{\tau }{\Delta }_f$ keeps decreasing up to γ = 4. This means that fitting more than one straight part helps improving the accuracy of state estimation. Figure 11(b) shows that the average number of iterations is about 19 and the maximum is only 25, which means this VGD quickly converges and is not very computationally expensive.

Comparison between VEKF, VGD and VGD-kinematic

In this section, in order to show the different performance of the gradient descent between the kinematic model and model from equation (15), we introduce a new method called Vision-based gradient descent method with kinematic model (VGD-kinematic). This method has the same principle as VGD except that it is using a kinematic model 38 as prediction model.

$$\{\begin{array}{l}\dot{\mathbf{X}}=\mathbf{V}\hfill \\ \dot{\mathbf{V}}=\mathbf{g}+{\mathfrak{R}}_B^E\left({\mathbf{a}}_m+{\mathbf{b}}_a\right)\hfill \\ \dot{\mathbf{\Phi }}={\mathfrak{R}}^{\prime }({\mathbf{\Omega }}_m+{\mathbf{b}}_g)\hfill \end{array}$$

(37)

In this case, the parameters to be estimated are the bias of accelerometers and gyros, which can be written as

$$\mathbf{\Theta }={[b_{a_x},b_{a_y},b_{a_z},b_p,b_q,b_r]}^{\text{T}}$$

(38)

To compare the performance of the of three methods, all three methods are tested using the same on-board data and the same generated vision measurements. In both VGD and VGD-kinematic, γ was set to 3, which means that the flight data of the last 3 straights is used in the estimation of ${\mathbf{\Theta }}^{*}$ . Note that during the first two arcs of the flight, there is not yet enough flight data, and γ will be smaller than 3.

The resulting full flight is shown in Figure 13. In Figure 13, the orange dots are the generated vision measurements from the straight parts of the track. The magenta curve is the estimation result of the VEKF. In the VEKF, $\mathbf{R}=\mathit{diag}([{2.5}^2,{2.5}^2,{2.5}^2]),\mathbf{Q}=\mathit{diag}([(2e-6,2e-6,5e-6,e-5,5e-6,3e-5,3e-8,3e-9,3e-9,0,0,0,0,0,0])$ and ${\mathbf{P}}_0=10\times I_{15\times 15}$ . The blue curve is the estimation result of the VGD and the red curve is the result of VGD-kinematic. To test the sensitivity of the VGD and the VGD-kinematic algorithm, the initial parameters ${\mathbf{\Theta }}_0$ are selected randomly within some ranges which can be found in Table 2. It can be seen that while the VEKF clearly converges to the measurements. The long prediction horizon combined with few and noisy measurement updates challenges the filter to its limit. On the other hand, the VGD managed to find parameters that fit the model very well through the noisy measurements and is not sensitive to the initial parameters. Even large measurement noise does not affect the prediction too much as the dynamics of the quadrotor cannot explain them.

Figure 12.

The final point error $\gamma \tau {\Delta }_f$ when using the VEKF, VGD and VGD-kinematic. The VGD has the most stable performance and least $\gamma \tau {\Delta }_f$ compared to the EKF and VGD-kinematic. (a) Position estimation; (b) Velocity estimation; (c) Estimation of accelerometer bias; (d) Estimation of gyro bias.

Figure 13.

Comparison of the position and velocity estimation results of the EKF and the FMINCON-based gradient descent method using inertial sensors and discrete low frequency noise vision based position measurements from gate detections.

Table 2.

The range of ${\mathbf{\Theta }}_0$ in VGD and VGD-kinematic.

${\mathbf{\Theta }}_0$	Range	${\mathbf{\Theta }}_0$	Range
$K_{*}^0$	$[-1,0]$	${\varphi }_{*}^{*}0$	$[-3^{°},3^{°}]$
${\mathbf{b}}_a^0$	$[-1m/s^2,1m/s^2]$	${\theta }_{*}^{*}0$	$[-3^{°},3^{°}]$
${\mathbf{b}}_g^0$	$[-3^{°}/s,-3^{°}/s]$	${\psi }_{*}^{*}0$	$[-3^{°},3^{°}]$

The final point prediction error ${}_{\gamma }^{\tau }{\Delta }_f$ after each turn of the three algorithms is shown in Figure 12. The VEKF requires several laps (3rd arc, or about 20 s of flight) to converge to sub-meter prediction accuracy. During the rest of the flight, the EKF can predict the 180° turns with a final point prediction error of around 0.5 m. The VGD-kinematic uses the derived kinematic model as prediction model and utilizes multiple vision measurements for parameter estimation. It has similar performance when compared with the VEKF. Overall, the VGD, which uses the same measurements as the VGD-kinematic but performs a bias and aerodynamics model estimation, is shown to find the best estimates of all parameters. It even finds good model parameters for the first arc, using only 1 straight line’s flight data. During the whole flight, ${}_{\gamma }^{\tau }{\Delta }_f$ of the VGD is kept around 0.2 m.

Conclusion

Accurate state and parameter estimation is essential for quadrotor control, especially when they perform aggressive maneuver. However, in the environment where only sparse and noisy position measurements are available, a classic Kalman filter can struggle to provide accurate state and model parameter estimation results. In this paper, we presented a novel method that only uses sparse vision measurements to estimate the AHRS error and select aerodynamic parameters of the quadrotor using a gradient descent method. The experiment result shows that our VGD could increase the accuracy of state estimation when compared to a classic Kalman filter in environments where only sparse noisy position measurements are available.