Augmenting Ultra-Wideband Localizationwith Computer Vision for Accurate Flight

David Hoeller ∗ Anton Ledergerber ∗ Michael Hamer ∗

Raffaello D’Andrea ∗

∗ Institute for Dynamic Systems and Control, ETH Zurich(e-mail: {dhoeller,antonl,hamerm,rdandrea}@ethz.ch).

Abstract: Ultra-wideband radio networks enable low-cost, low-computation robot localizationin semi-structured environments; however, previous results have shown that these localizationsystems suffer from spatially-varying measurement biases, leading to a spatially-varying offsetbetween the physical and the estimated position. In tasks where absolute positioning or hightracking accuracy is required, this offset can lead to failure of the task. This paper proposesaugmenting ultra-wideband-based localization with visual localization to improve estimationaccuracy for critical tasks. It also presents a control strategy that takes the camera measurementprocess into account, and allows the ultra-wideband system’s measurement biases to be learnedand compensated over multiple executions of the task. This bias compensation can be usedto improve the accuracy of the task in the case of visual impairment. The effectiveness of theproposed framework is demonstrated by accurately flying a quadrocopter to a landing platformusing on-board estimation and control.

Keywords: Flying robots, Perception and sensing, Information and sensor fusion, Motioncontrol systems, Non-linear predictive control

1. INTRODUCTION

Recent progress in low-cost, low-computation indoor local-ization using ultra-wideband (UWB) radio networks hasenabled the use of drones in indoor environments. How-ever, UWB localization systems have been shown to sufferfrom spatially-varying measurement biases (Ye et al., 2010;Prorok and Martinoli, 2014), which can lead to a spatially-varying offset between the physical and the estimatedposition (Mueller et al., 2015; Ledergerber et al., 2015).In applications requiring drones to accurately performmaneuvers, for example, a maneuver to land on a charging-station or on a package in a warehouse, this offset can leadto failure.

In this paper, we augment a quadrocopter flying in aUWB localization system with an onboard camera anddemonstrate that the camera enables the accurate exe-cution of a landing maneuver. The combination of UWBlocalization and onboard vision is advantageous: absoluteposition, in our system with an accuracy of ±25cm, canbe obtained from UWB measurements quickly and witha low computational cost; meanwhile, the onboard visionsensor provides improved local accuracy if the task requiresit. This is in comparison with existing methods of indoorlocalization, such as visual localization, which require sig-nificant processing, and may not provide accurate posi-tioning information in situations where the configurationand visual state of the environment is constantly changing,for example in modern, robotic warehouses (e.g. Wurmanet al. (2007)).

Using computer vision to assist with drone maneuvershas been the focus of considerable research. One example

H

Fig. 1. An overview of the system presented in this paper.The quadrocopter localizes itself based on UWB rangemeasurements with an accuracy of ±25cm. This is tooinaccurate to successfully perform a landing maneu-ver. The quadrocopter therefore relies on camera mea-surements for additional accuracy during the land-ing maneuver. A control strategy was developed thattakes into account the camera measurement process.This control strategy further allows the UWB biasesto be learned over multiple maneuvers, allowing anaccurate trajectory to be tracked by the UWB systemin the case of temporary visual impairment.

of early work on this topic is Amidi et al. (1999), whoapplied visual odometry to improve the performance ofautonomous helicopters. More recent publications, e.g.Faessler et al. (2014); Lange et al. (2009); Yang et al.(2013); Wenzel and Zell (2009) extend this work specifi-cally for take-off and landing maneuvers. In addition, manyrecent consumer drones (e.g. the Parrot AR.Drone and theDJI Mavic drone) use computer vision to stabilize motion.

This paper leverages this research, employing an approachbased on planar artificial markers for vision-based state

estimation during a landing maneuver. While such anapproach was also chosen by Benini et al. (2013) to aug-ment a UWB localization system with visual localization,the main contributions of this paper are found in theextensions of their work, namely the control strategy devel-oped specifically for this kind of state estimation, as wellas the framework used to compensate the biases of theUWB localization system during a maneuver. This biascompensation can be used to improve the accuracy of thelanding maneuver in the case of visual impairment. Fig. 1illustrates the landing procedure discussed in this paper,where trajectories are generated such that the onboardcamera faces a known pattern next to the landing site andthe UWB biases are learned along these trajectories.

The paper is structured as follows: Section 2 provides abrief overview of the camera-based state estimator andSection 3 lays out the dynamics of the quadrocopter. InSection 4, a control strategy is presented which takesthe camera measurement process into account. Section 5explains how the sensor measurements can be combinedto learn and compensate for the biases of the UWB local-ization system, allowing a trajectory to be tracked in thecase of temporary visual impairment. Finally, Section 6presents details of the implementation and discusses ex-perimental results.

2. VISION-BASED POSE ESTIMATION

This section describes the estimation of the quadrocopter’spose from the image of a known pattern placed near thelanding platform.

Camera model A pinhole model is used to describe theimage formation process, as shown in Fig. 2. Assuming thecamera has been calibrated (e.g. as described by Zhang(2000)), a point p = (x, y, z) in the world projected ontothe camera’s sensor plane results in the pixel measurement[

uv

]= Φ (p, TCW) , (1)

where TCW ∈ SE(3) is the pose of the camera’s centerrelative to the inertial frame, and where Φ(·, ·) is thecalibrated, non-linear projection function as described inHartley (2000) and Szeliski (2010).

Fig. 2. Representation of the pinhole camera model. Apoint p is projected onto the sensor plane resulting inthe pixel measurement (u, v). The pose of the camerais encoded via the transformation TCW.

Pose estimation Given a set of points P, where pointi ∈ P has the known location pi relative to the iner-tial frame, and measured pixel coordinates (ui, vi), thecamera’s pose TCW relative to the inertial frame can beestimated by solving the following non-linear least squaresproblem

minTCW

∑i∈P

∥∥∥∥[uivi]− Φ (pi, TCW)

∥∥∥∥2

, (2)

where ‖ · ‖ is the Euclidean norm. If the points are inthe same plane, an initial guess for the pose can be foundby calculating the parameters of the homography relatingthe points in the world and the pixel measurements. Thepose of the quadrocopter in the inertial frame can thenbe estimated as TBW = TBCTCW, where TBC ∈ SE(3) isthe known transformation from the camera’s center to thequadrocopter’s body frame.

For simplicity, our implementation (Section 6) is based ona blob-detection approach, whereby the landing platformis marked by a pattern (as shown in Fig. 1), whose knownlayout and projection on the camera’s sensor providesthe information required for (2). The main limitation isthat the pattern must be visible at all times and themethod is therefore sensitive to occlusions. However, as welater discuss in Section 5, by learning and compensatingfor the UWB network’s spatially-varying biases along thelanding trajectory, position estimates derived from theUWB network can help ensure accurate tracking duringperiods of temporary visual impairment, e.g. due to markerocclusion or motion blur.

The accuracy of the resulting pose estimate can be quanti-fied by means of backward transport of covariance (Hart-ley, 2000, pp. 138-150). For the planar pattern depicted inFig. 1, the accuracy improves when the angle with respectto the pattern normal is increased.

3. DYNAMICS AND STATE ESTIMATION

We assume the quadrocopter is a rigid body of mass mable to produce a positive thrust f along its body’s z-axis.The body’s angular rates are defined in the body frameas ω = (ω1, ω2, ω3), where the notation (x, y, z) is usedto succinctly express elements of a vector. We define thequadrocopter to have position x(t) ∈ R3, relative to theinertial frame, and an orientation RWB(t) ∈ SO(3), whichexpresses a rotation from the quadrocopter’s body frameto the inertial frame. The continuous-time quadrocopterdynamics are then given by

x = RWBe3f

m+ g

RWB = RWB[[ω]]×,(3)

where g = (0, 0,−g) is the acceleration due to gravity,e3 = (0, 0, 1) and [[ω]]× is the matrix form of the crossproduct, defined as

[[ω]]× =

[0 −ω3 ω2

ω3 0 −ω1

−ω2 ω1 0

]. (4)

We use the angular rates ω and the thrust f as controlinputs (see Fig. 3) and assume that they can be controlledwithout delay. This assumption is reasonable, since the

motors’ time constants are much smaller than those of theother dynamics.

In an approach similar to Augugliaro et al. (2012), thedynamics (3) are discretized for state estimation andtrajectory generation (Section 4). We define the discrete-time state of the quadrocopter at discrete-time index kas q[k] = (x[k],v[k],RWB[k]), where x[k] ∈ R3 andv[k] ∈ R3 are respectively the position and velocity ofthe quadrocopter expressed in the inertial frame, andwhere RWB[k] ∈ SO(3) expresses a rotation from thequadrocopter’s body frame to the inertial frame. Notethat the quadrocopter’s body rates ω are not includedin the state, since they are used as inputs to the system.Discretization for the fixed sampling period ∆t, assuminga zero-order input hold, then yields

x[k + 1] ≈ x[k] + v[k]∆t

+ RWB[k]∆t2(

1

2I +

1

6∆t[[ω[k]]]×

)e3f [k]

m

+1

2g∆t2,

v[k + 1] ≈ v[k] + RWB[k]∆t

(I +

1

2∆t[[ω[k]]]×

)e3f [k]

m

+ g∆t,

RWB[k + 1] = RWB[k]exp([[ω[k]]]×∆t),(5)

where the first order approximation

exp([[ω[k]]]×∆t) ≈ I + [[ω[k]]]×∆t (6)

was used to discretize the updates of x and v. Thediscrete-time acceleration a[k] ∈ R3 in the inertial frameis therefore given by

a[k] = RWB[k]e3f [k]

m+ g. (7)

An extended Kalman filter, similar to the filter describedin Mueller et al. (2015), is used to track the state ofthe quadrocopter. Gyroscope and accelerometer measure-ments enter directly as inputs in the process update step,while camera pose estimates are used as measurementupdates.

4. LANDING MANEUVER

As previously discussed, the state estimator relies on thecamera’s pose estimate, calculated based on the image ofthe blob pattern. This results in an additional constrainton the planned trajectory in order to ensure that thecamera faces the pattern. In this section, we formulatethe generation of landing maneuvers as an optimizationproblem including the quadrocopter’s dynamics and thecamera orientation constraint, and discuss how these tra-jectories can be tracked by means of a non-linear modelpredictive control (MPC) problem.

4.1 Trajectory Generation and Tracking

We express the trajectory generation problem as an op-timization problem over the finite time-horizon N , andsolve for the control inputs ω[k] and f [k] at each timestep k = 0, . . . , N − 1. This formulation allows a smoothtrajectory to be obtained by minimizing the jerk of thetrajectory, while ensuring that the discrete-time dynamics(5) are fulfilled.

Fig. 3. Model of the quadrocopter. The inputs are theangular rates ω1, ω2, ω3, and thrust f . The vectorecam (blue) describes the orientation of the rigidly-mounted camera with respect to the quadrocopter’sbody frame, and the vector epat (red) is defined topoint from the center of the pattern to the center ofthe camera.

Furthermore, defining ecam as the unit-vector expressingthe orientation of the rigidly-mounted camera with respectto the quadrocopter’s body frame, and epat as the unitvector pointing from the pattern to the camera’s center(see Fig. 3), we additionally penalize deviation of thecamera’s orientation from the pattern’s center by includingthe dot-product RWB[k]ecam and epat. This term reachesits minimum of -1 when the vectors are facing each other.This results in the optimization problem

minimize α

N−2∑k=0

∣∣∣∣∣∣∣∣a[k + 1]− a[k]

∆t

∣∣∣∣∣∣∣∣2+ (1− α)

N∑k=1

(RWB[k]ecam)T

epat[k]

subject to fmin ≤ f [k] ≤ fmax, k = 0, . . . , N − 1

ωmin ≤ ω[k] ≤ ωmax, k = 0, . . . , N − 1

q[0] = qinit, q[N ] = qfinal

(8)

with α ∈ [0, 1] a weight factor and qfinal the desired finalstate defined up to the quadrocopter’s yaw, which is leftas a free state. The control inputs are subject to boxconstraints. This results in 4N optimization variables withnine equality constraints and 8N inequality constraints.Note that due to the problem formulation, the discretedynamics of Section 3 are contained in the objectivefunction and are not used as constraints. Also note thatthe initial conditions are fulfilled by definition.

The initial position of the maneuvers is chosen to ensurea sufficient angle with respect to the pattern normal. Asdescribed in the previous section, this results in moreaccurate camera pose estimates throughout the trajectory.

This non-linear optimization problem can be solved usingsequential quadratic programming (Nocedal and Wright,2006) where the non-convex objective function and con-straints are iteratively approximated by quadratic andaffine functions, resulting in a convex optimization prob-lem that can be solved in real time by the quadrocopter’sonboard processor (Section 6).

The resulting optimal control inputs define a trajectorywhich could be supplied as a reference to a standardtrajectory-tracking controller. However, instead of focus-ing on the camera pointing constraint, too much controleffort might be dedicated to tracking the trajectory, which

could result in the camera losing sight of the pattern inthe case of a deviation from the planned trajectory.

In order to maintain camera-pattern alignment through-out the trajectory, a non-linear model predictive controlapproach was chosen, in which the optimization problem issolved every 20ms starting from the current state estimate.The resulting control inputs are then applied to the systemuntil a new solution becomes available (as in, e.g. Hehnand D’Andrea (2011)). While the number of time stepsN is held constant throughout the maneuver to keep thecomputational cost similar, the time allocated for eachnew trajectory is reduced by the time elapsed since thebeginning of the maneuver resulting in a decreasing timestep length ∆t along the maneuver.

5. UWB BIAS ESTIMATION

While the camera provides a good pose estimate whenfacing the pattern, it is sensitive to illumination conditionsand visual impairment. In the previous sections we havediscussed the generation of landing maneuvers based onvisual localization. In this section we derive a frameworkto learn the spatially-varying biases of the UWB measure-ments. These biases can be used to compensate the mea-surements during future maneuvers. Thus, the accuracy ofthe UWB localization system is improved, adding a layerof robustness against temporary visual impairment.

Mueller et al. (2015) present a UWB localization systemcapable of measuring the distance between an antenna onthe quadrocopter and fixed antennas on the ground, knownherein as anchors. Mueller et al. (2015) note that eachrange measurement appears to have a systematic, spatiallyvarying bias; however, as this bias is unobservable withouta secondary localization system, it is not included in theirmeasurement model. Having augmented the quadrocopterwith a visual localization system able to provide accurateand absolute pose estimates, this paper is able to extendthe system of Mueller et al. (2015) by additionally consid-ering the bias in the range measurement model.

Fig. 4. Overview of the bias estimation framework. TheUWB range measurements and the estimated posi-tions of the antenna are logged along a trajectory.The bias to each anchor is assumed to be constant ineach cube.

5.1 UWB Bias Modelling

As depicted in Fig. 4, in order to estimate the bias foreach anchor in each cube, the camera, IMU, and UWBmeasurements should be logged when the quadrocopterperforms a maneuver and an optimization run after com-pleting the maneuver in order to estimate the biases thatmaximize the measurement likelihood along the trajectory.Due to the large amount of data, this high dimensionalproblem quickly becomes computationally intractable onthe embedded platform. Forster et al. (2015) approach asimilar problem in visual-inertial odometry by factorizingthe IMU measurements, thus reducing the problem’s di-mensionality. In our case, dimensionality is reduced bylogging the UWB range measurements when they arrive,as well as the estimated position of the quadrocopter’sUWB antenna (derived from the fusion of camera and IMUmeasurements) whenever a measurement update with acamera pose estimate is performed (i.e. at a frequency of60 Hz).

We denote the set of antenna position measurements asM.These measurements mark the times and locations alongthe trajectory at which a camera update was performed.Given a measurement m ∈M, the position of the quadro-copter’s UWB antenna in the inertial frame is measuredas

zant[m] = pant[m] + ηant, (9)

where pant[m] denotes the actual position of the antenna,and where the measurement of this position is corruptedby an assumed white and normally-distributed measure-ment noise ηant ∼ N (0,Σant) with zero mean and covari-ance Σant.

As shown in Fig. 4, UWB range measurements to ananchor are corrupted by a systematic bias based on thequadrocopter’s position, and the anchor to which the rangeis measured. The range bias for each anchor is assumedto be constant in time and spatially varying. In order tomodel the range biases, the world is segmented into cubes,in which the range bias for each anchor is assumed tobe constant. This approach is similar to the approach ofProrok and Martinoli (2014), who use spatially anchorederror models.

As the quadrocopter flies the trajectory, range measure-ments are collected at a rate of 200 Hz. Since this is morefrequent than measurements of the antenna’s position arerecorded (60 Hz, since this occurs upon updating with acamera image), multiple range measurements will occurbetween two position measurements. For a range mea-surement r, we define pinterp[r] to be the position of thequadrocopter’s antenna in the inertial frame at the timeof recording the range measurement r, and relate it tothe antenna position of the previous measurement pant[m]and next measurement pant[m + 1] using linear interpo-lation based on the measurement times. Although it ispossible to record the quadrocopter’s state estimate uponthe reception of every range measurement, by relatingthe position of a range measurement to the position ofa camera measurement, we significantly reduce the num-ber of optimization variables, hence making this problemcomputationally tractable on the embedded system.

We denote the set of all range measurements R, and fora range measurement r ∈ R define: ar to be the anchorindex and panch[ar] to be the known position of the anchorto which the range was measured; cr to be the cube indexin which the range was measured; and b[ar, cr] to be thebias of a measurement to anchor ar in cube cr. Note thatmultiple range measurements made to the same anchor inthe same cube will have the same bias. We further definethe value of the range measurement to be

zrange[r] = ‖panch[ar]− pinterp[r]‖+ b[ar, cr] + ηrange,(10)

where the measurement of this range is corrupted byan assumed white and normally-distributed measurementnoise ηrange ∼ N (0,Σrange) with zero mean and covarianceΣrange.

The problem of estimating the biases and path flownduring a maneuver can be expressed as the follow-ing non-linear least squares minimization problem overpant[m] ∀m ∈M and b[ar, cr] ∀r ∈ R:

min∑r∈R

∥∥Σ−1rangeρrange[r]

∥∥2+∑

m∈M

∥∥Σ−1antρant[m]

∥∥2(11)

where

ρrange[r] = zrange[r]− (‖panch[ar]− pinterp[r]‖+ b[ar, cr])

ρant[m] = zant[m]− pant[m],(12)

and recalling that pinterp[r] is a function of the opti-mization variables pant[m]. This problem formulation isattractive: it is computationally tractable on the em-bedded system (e.g. it can be solved quickly using theLevenberg-Marquardt algorithm (More, 1978)); and, un-der the prior assumption of Gaussian measurement noise(ηant and ηrange), results in the maximum likelihood esti-mate (Bishop, 2006). To maintain reasonable solving timeson the embedded system, longer trajectories are segmentedinto sub-trajectories and the problem is solved for eachportion.

The main disadvantage of this technique is that only thespatially varying bias is learned; whereas, as shown by Yeet al. (2010), the bias also appears to depend on the orien-tation of the quadrocopter. However, since the maneuverstraverse similar cubes with a similar orientation, this doesnot appear to have a significant effect on the situationdescribed in this paper, as is shown in Section 6.

6. EVALUATION

This section describes the implementation details and theexperimental results assessing the camera-based estimator,the bias estimation framework, and the landing maneu-vers.

6.1 System Architecture

The quadrocopter used for the experiments is an Ascend-ing Technologies Hummingbird that was modified to runwith a Snapdragon Flight computer, which is equippedwith both a Quad-core 2.26 GHz Krait CPU and a DSP.The camera used to record images at a resolution of1920x1080 (1080p) is a Sony IMX135 camera. The IMU

is a low-cost Invensense MPU-9250 and the UWB radiosare Decawave DWM1000 modules.

The algorithms presented in this paper are implementedbased on the Flying Machine Arena framework (see Lu-pashin et al. (2014)), which is used to issue high levelcommands, and additionally to provide accurate motioncapture data for comparison purposes. Every algorithmpresented in this paper runs onboard the quadrocopter andwithout the help of the external motion capture system.

Fig. 5. Block diagram of the system. The estimator ob-tains measurements from the IMU, UWB radios, andcamera. Based on the state estimate and the desiredend state and time, the MPC generates a controlinput that is tracked by the angular rate and thrustcontroller.

The non-linear optimization problem of the trajectory gen-eration step is solved with the open source library NLopt(Johnson (2007)) implementing the algorithm described inKraft (1988) and Kraft (1994).

The block diagram of the system is depicted in Fig. 5. Theestimator obtains measurements from the IMU at 1 kHz,from the UWB anchors at 200 Hz, and from the cameraat 60 Hz. Given the current state estimate, the desiredend state and the time to reach it, the MPC calculatesa series of control inputs at a frequency of 50 Hz. Thesecommands are tracked with an angular rate and thrustcontroller which runs at 1 kHz and uses the gyroscopicsensor for feedback. This controller is further describedin Appendix B of Lupashin et al. (2014).

6.2 Setup

The experimental area was equipped with four UWBanchors arranged in a rhombus at heights varying from0 to 3m. The landing platform is a 0.38 m× 0.38 m squarewhose center is placed 0.7 m in front of the pattern.

In order to estimate UWB biases along the trajectory, thespace was divided into cubes with a side length of 0.2 m.

6.3 Results

Slow Triangular Maneuver The first experiment wasdesigned to assess the performance of the camera-basedestimator and the bias estimation framework. A slow,horizontal trajectory at the same height as the pattern’scenter was repeatedly flown, after which the bias estima-tion framework was run. In order to assess the impactof the bias compensation step, the trajectory was thenreflown and two estimators fusing only the IMU and UWB

measurements were run simultaneously. The first used rawUWB range measurements, while the second also correctedthe range with the estimated UWB bias. This mimicsthe impact of an occlusion in the vision system during atrajectory. Fig. 6 shows range measurements in the reflowntrajectory to two different anchors with and without biascompensation. Spikes in the range measurements to thefirst anchor (as seen in Fig. 6) arise when the UWB radiosignal has to traverse the quadrocopter. Due to the as-sumption that the bias is constant in a cube, the estimatorcannot handle such outliers well.

The mean absolute error of the IMU-Camera EKF positionis 0.036 m in the reflown trajectory.

Fig. 6. Range measurements for two anchors in the firstexperiment. The ground truth measurements are inblue, the raw range measurements in red, and the biascompensated measurements in yellow.

The position estimates of the two estimators in the secondtrajectory are presented in Fig. 7. The mean absolute errorof the position estimate is reduced from 0.30 m to 0.12 mwhen compensating for range biases.

Fig. 7. This figure shows the effect of bias compensationon the position estimate. Shown in red, the position isestimated based on raw UWB ranges, while in yellow,the position is estimated based on bias-compensatedrange measurements. The position as measured by themotion capture system is shown in blue.

Landing Maneuver The goal of the second experimentis to assess the landing maneuvers. For that purpose, 3 mlong maneuvers with T = 3.4 s, N = 8 (see Section 4) wereperformed. The optimization problem (8) used to generatethe control inputs for these maneuvers can be solved fordifferent values of the weighting factor α, resulting indifferent trajectories. Two such trajectories for α = 0and α = 0.1 respectively are shown in Fig. 8. While anα = 0 leads to a better alignment of the camera and thepattern, it is not clear that this also leads to a bettercamera pose estimate. Weighting also the jerk leads to

smoother trajectories with lower peak velocities, hence thisalso reduces the motion blur impairing the pose estimates.Therefore, the value of α should be adapted to the timeavailable for a maneuver and the exposure time. For ourlanding maneuver a value of α = 0.1 was chosen.

Fig. 8. Two trajectories generated by solving the optimiza-tion problem (8). The upper one with α = 0 betterkeeps the camera aligned whereas the lower one withα = 0.1 leads to smoother trajectories and hence alsoless motion blur.

The landing maneuver was flown multiple times, resultingin a mean absolute position estimation error across allmaneuvers of 0.063 m. Very similar results are obtainedwhen including bias-compensated UWB measurements,due to the accuracy of the camera’s pose estimates incomparison to the estimates from the UWB system.

To simulate the effect of occlusions, UWB range biaseswere learned based on the initial camera-based flights,after which the maneuver was flown multiple times withthe camera disabled and the estimator fusing only the IMUand UWB measurements. When compensating for UWBbiases, the absolute position error across all maneuverswas 0.14 m. Without bias estimation, the position errorwas 0.21 m.

These results support the idea that bias estimation is nec-essary for accurate flight of dynamic maneuvers without acamera. It should be noted that these experiments corre-spond to the extreme case of visual impairment, where thecamera is unable to provide pose estimates for the entireduration of the trajectory.

Fig. 9 shows the first trajectory that is calculated bythe MPC in blue along with the ground truth trajectoryfinally flown in green. The red rectangles are the patternand the landing platform. The discrepancy between thefirst calculated trajectory and the one that was performedhighlights the limitations of the simple motion model usedfor the quadrocopter. This model does not include thepropeller dynamics and also neglects drag. The advantageof such a formulation is its simplicity, making it usable formost multirotors since it does not depend on the vehicle’sgeometry.

7. CONCLUSION

This paper first discusses a suitable control strategy fora quadrocopter estimating its state by taking images of a

-3

1.2

1.4

1.6

-2.5

1.8

2

2.2

z(m

)

-2

y (m)

1.5-1.5

x (m)

1 -10.5 -0.5

Fig. 9. Trajectory generated by the MPC during a landingmaneuver. The red rectangles represent the patternand the landing platform. The blue curve correspondsto the first trajectory calculated by the MPC, wherethe vectors represent the thrust at each time step. Thegreen trajectory is the ground truth trajectory.

known pattern with a rigidly mounted onboard camera.While we only applied this control strategy to land thequadrocopter on a landing platform, the general problemformulation also allows its usage for tracking objects, e.g.for outdoor filming. The second part of the paper showshow the precise state estimate resulting from this controlstrategy can be used to learn the biases of the UWBlocalization system such that also under visual impairmentprecise maneuvers can be flown.

ACKNOWLEDGEMENTS

The authors would like to thank Marc-Andre Corzilliusfor the design of the electrical hardware and MichaelEgli for construction of the platform. Many people havecontributed to the Flying Machine Arena in which thisresearch was conducted. A list of these people can be foundat: http://flyingmachinearena.org/people.

This work was supported by the Swiss National ScienceFoundation (SNSF).

REFERENCES

Amidi, O., Kanade, T., and Fujita, K. (1999). A visualodometer for autonomous helicopter flight. Journal ofRobotics and Autonomous Systems, 28, 185 – 193.

Augugliaro, F., Schoellig, A.P., and D’Andrea, R. (2012).Generation of collision-free trajectories for a quadro-copter fleet: A sequential convex programming ap-proach. In 2012 IEEE/RSJ International Conferenceon Intelligent Robots and Systems (IROS), 1917–1922.

Benini, A., Mancini, A., and Longhi, S. (2013). AnIMU/UWB/vision-based extended Kalman filter formini-UAV localization in indoor environment using802.15.4a wireless sensor network. Journal of Intelligent& Robotic Systems, 70(1-4), 461–476.

Bishop, C.M. (2006). Pattern recognition and machinelearning. Springer.

Faessler, M., Mueggler, E., Schwabe, K., and Scaramuzza,D. (2014). A monocular pose estimation system basedon infrared leds. In 2014 IEEE International Conferenceon Robotics and Automation (ICRA), 907–913.

Forster, C., Carlone, L., Dellaert, F., and Scaramuzza, D.(2015). On-manifold preintegration theory for fast andaccurate visual-inertial navigation. CoRR.

Hartley, R. (2000). Multiple view geometry in computervision. Cambridge University Press.

Hehn, M. and D’Andrea, R. (2011). Quadrocopter trajec-tory generation and control. IFAC Proceedings Volumes,18th IFAC World Congress.

Johnson, S.G. (2007). The NLopt nonlinear-optimizationpackage. URL http://ab-initio.mit.edu/nlopt.

Kraft, D. (1988). A software package for sequentialquadratic programming. Technical report, Institut furDynamik der Flugsysteme, Oberpfaffenhofen.

Kraft, D. (1994). Algorithm 733: TOMP-Fortran modulesfor optimal control calculations. ACM Transactions onMathematical Software, 20(3), 262–281.

Lange, S., Sunderhauf, N., and Protzel, P. (2009). Avision based onboard approach for landing and positioncontrol of an autonomous multirotor UAV in GPS-denied environments. In 2009 International Conferenceon Advanced Robotics (ICAR), 1–6.

Ledergerber, A., Hamer, M., and D’Andrea, R. (2015).A robot self-localization system using one-way ultra-wideband communication. In 2015 IEEE/RSJ Inter-national Conference on Intelligent Robots and Systems(IROS), 3131–3137.

Lupashin, S., Hehn, M., Mueller, M.W., Schoellig, A.P.,Sherback, M., and D’Andrea, R. (2014). A platform foraerial robotics research and demonstration: The FlyingMachine Arena. Mechatronics, 24(1), 41 – 54.

More, J.J. (1978). The Levenberg-Marquardt algorithm:Implementation and theory. In Numerical analysis, 105–116. Springer.

Mueller, M.W., Hamer, M., and D’Andrea, R. (2015). Fus-ing ultra-wideband range measurements with accelerom-eters and rate gyroscopes for quadrocopter state estima-tion. In IEEE International Conference on Robotics andAutomation, (ICRA), 1730–1736.

Nocedal, J. and Wright, S. (2006). Numerical optimization.Springer Science & Business Media.

Prorok, A. and Martinoli, A. (2014). Accurate indoorlocalization with ultra-wideband using spatial modelsand collaboration. International Journal of RoboticsResearch, 33(4), 547–568.

Szeliski, R. (2010). Computer Vision: Algorithms andApplications. Springer.

Wenzel, K.E. and Zell, A. (2009). Automatic take off,hovering and landing control for miniature helicopterswith low-cost onboard hardware, 73–80. Springer BerlinHeidelberg.

Wurman, P.R., D’Andrea, R., and Mountz, M. (2007).Coordinating hundreds of cooperative, autonomous ve-hicles in warehouses. In 2007 National Conference onInnovative Applications of Artificial Intelligence, vol-ume 2, 1752–1759.

Yang, S., Scherer, S.A., and Zell, A. (2013). An onboardmonocular vision system for autonomous takeoff, hov-ering and landing of a micro aerial vehicle. Journal ofIntelligent & Robotic Systems, 69(1), 499–515.

Ye, R., Redfield, S., and Liu, H. (2010). High-precisionindoor UWB localization: Technical challenges andmethod. In 2010 IEEE International Conference onUltra-Wideband, volume 2, 1–4.

Zhang, Z. (2000). A flexible new technique for cameracalibration. IEEE Transactions on pattern analysis andmachine intelligence, 22(11), 1330–1334.