Robust Adaptive Model Predictive Control of Quadrotors
Alexandre Didier, Anilkumar Parsi, Jeremy Coulson, Roy S. Smith
RRobust Adaptive Model Predictive Control of Quadrotors
Alexandre Didier, Anilkumar Parsi , Jeremy Coulson, Roy S. Smith Abstract — Robust adaptive model predictive control(RAMPC) is a novel control method that combines robustnessguarantees with respect to unknown parameters and boundeddisturbances into a model predictive control scheme. However,RAMPC has so far only been developed in theory. The goalof this paper is to apply RAMPC to a physical quadrotorexperiment. To the best of our knowledge this is the first timethat RAMPC has been applied in practice using a state spaceformulation. In doing so, we highlight important practicalchallenges such as computation of λ -contractive polytopes anddealing with measurement noise, and propose modifications toRAMPC so that it can be applied on a quadrotor. We firstsimulate quadrotor flight with a direct and a decoupled controlarchitecture in different scenarios. The scenarios include: (i)an unknown mass of the quadrotor as a package deliveryscenario with wind as a bounded disturbance; and (ii) allrotor efficiencies drop as a power delivery problem. We thenimplement these scenarios on a physical quadrotor and presentthe experimental results. I. INTRODUCTIONModel Predictive Control (MPC), as presented in [1],is an optimisation based control scheme, which guaranteesstate and input constraint satisfaction at every time stepfor a discrete-time, nominal system. Robust MPC, see [1],integrates robustness guarantees with respect to bounded dis-turbances and parametric uncertainties in the dynamics intothe optimisation problem. However, robust MPC controllersresult in poor closed loop performance when the uncertaintysets are large. This is addressed by Robust Adaptive MPC(RAMPC), a novel control technique as proposed in [2] foran impulse response model and in [3], [4] and [5] for astate space model. In RAMPC, set-membership identificationis used to update uncertainty sets online. This methodguarantees that the true parameter is always included in theupdated uncertainty set, and thus is suitable for guaranteeingrobust constraint satisfaction.Though using state space models in RAMPC improvesthe overall control performance by reducing the number ofparameters, it has never been applied in practice. The goalof this paper is to implement RAMPC on a quadrotor inorder to robustly guarantee a safe flight while achievinggood performance. We propose solutions to practical issueswhich arise in the application of RAMPC, notably a steady-state input depending on the uncertain parameter, whichlead to steady-state errors and infeasibility for robust MPC,
All authors are with the Department of Information Tech-nology and Electrical Engineering at ETH Zurich, Switzerland { adidier,aparsi,jcoulson,rsmith } @ethz.ch. Affiliation: This work is supported by the Swiss National ScienceFoundation under grant number 200021 178890. The authors are with theAutomatic Control Laboratory, ETH Zurich, Switzerland. and measurement noise. To resolve these issues, we presentsolutions with theoretical guarantees which showed goodperformance during practical implementation. In addition, wepropose a novel algorithm to compute contractive polytopeswith a desired contraction rate, which are necessary toimplement RAMPC.To test the effectiveness of RAMPC as a control schemefor quadrotors, we first simulate different scenarios of aquadrotor flight: (i) The first scenario involves an uncertainquadrotor mass as part of a package delivery. If the massof the package the drone is delivering is unknown, it maylead to an unsafe flight resulting in a crash of the quadrotor.Additionally, an unknown wind force is acting on the quadro-tor which is modelled as an additive disturbance. Wind is acommon factor during an outdoor quadrotor flight and itsimpact on quadrotor flight is studied in [6] and [7]. (ii) Inthe second scenario we consider a loss of efficiency in allrotors as a power delivery problem, as similarly studied in [8]and [9]. The simulations were performed with two differentcontrol architectures for the quadrotor. The first is a directthrust control and the second is a decoupled control structurefrom [10].The paper is structured as follows. Section II contains adescription of the RAMPC method used, a description ofthe set-membership estimation, and a discussion on practicalimplementation issues and their solutions. In Section III, weshow the results of simulations of different quadrotor flightscenarios and in Section IV we discuss the results of theexperiments on the physical quadrotor.
Notation.
The set of integers { , . . . , n } is given as N n and the set of positive reals is R > . A (cid:23) denotes that thematrix A is positive semi-definite and A (cid:23) B is equivalentto A − B (cid:23) . The i -th row of the matrix A is given as [ A ] i .The diagonal matrix with entries a, b, c on its main diagonalis denoted as diag ( a, b, c ) and the convex hull of a set A isdefined as co ( A ) . The identity matrix is denoted as I and is the vector of ones. The Euclidean norm of b is is given by (cid:107) b (cid:107) and (cid:107) x (cid:107) A represents x T Ax . A ⊕ B denotes Minkowskiset addition and B n = { x ∈ R n | (cid:107) x (cid:107) ∞ ≤ . } is a unithypercube of dimension n .II. R OBUST A DAPTIVE M ODEL P REDICTIVE C ONTROL
A. System description
The Robust Adaptive Model Predictive Control schemeproposed in [5] is used in this paper due to its computa-tional efficiency. The method considers linear, discrete-time,parameter dependent dynamics with an additive disturbance x k +1 = A ( θ ) x k + B ( θ ) u k + w k , (1) a r X i v : . [ ee ss . S Y ] F e b ith the state vector x k ∈ R n , the disturbance w k ∈ R n , theinput vector u k ∈ R m and the uncertain parameter θ ∈ R p ,whose true value is θ = θ ∗ . The additive disturbance w k andthe uncertain parameter θ fulfil the following assumptions,made in [3], [4] and [5]: Assumption 1.
1) The disturbance w k is bounded by a convex polytope w k ∈ W = { w ∈ R n | H w w ≤ h w } , (2) with H w ∈ R n w × n and h w ∈ R n w .2) The system matrices A ( θ ) and B ( θ ) depend affinely onthe parameter vector θ ∈ R p , such that ( A ( θ ) , B ( θ )) = ( A , B ) + p (cid:88) i =1 ( A i , B i )[ θ ] i . (3)
3) The uncertain parameters θ are bounded in a convexpolytope θ ∈ Θ = { θ ∈ R p | H θ θ ≤ h θ } , (4) with H θ ∈ R n θ × p and h θ ∈ R θ , which is knownand contains the true parameter vector θ ∗ , which isunknown. The states and inputs are constrained using polytopic stateand input constraints ( x k , u k ) ∈ Z = { ( x, u ) ∈ R n × R m | F x + Gu ≤ } , (5)with F ∈ R n z × n and G ∈ R n z × m . B. Parameter Identification
To estimate the parameter θ , set-membership identificationis used, see [11]. In this identification method, a set ofpossible parameters that is guaranteed to contain the trueparameter is recursively updated. This gives robustness guar-antees with respect to the uncertain parameter. In order tocompute this recursive update, the time dependent matrix D k ∈ R n × p and vector d k ∈ R n are introduced D k = D ( x k , u k )= [ A x k + B u k , A x k + B u k , . . . , A p x k + B p u k ] ,d k = A x k − + B u k − − x k , which are affine in the states and inputs.At every time step k , the set of possible parametersconsistent with the evolution of the system given the un-known, bounded disturbance, can be computed. This setof parameters ∆ k , called the non-falsified parameter set, isgiven as ∆ k = { θ ∈ R p | x k − ( A ( θ ) x k − + B ( θ ) u k − ) ∈ W } = { θ ∈ R p | − H w D k − θ ≤ h w + H w d k } . (6)The set of feasible parameters is denoted by Θ k and isrecursively updated using the non-falsified set ∆ k , startingwith Θ as the initial set. In order to efficiently describe theparameter set, the parameter sets are restricted to a boundinghypercube, with centre ¯ θ k ∈ R p and length η k ∈ R ≥ , as in Θ k = { ¯ θ k } ⊕ η k B p ⊇ Θ k − ∩ ∆ k (7) As proposed in [5], the computation time of parameter setupdates is improved by parameterizing Θ k using hypercubesinstead of a more generic set description used in [3], althoughconservatism is introduced by restricting the shape of Θ k .This is also suitable for practical implementation of theRAMPC algorithm.Additionally, a point estimate is required for the costfunction evaluation within the RAMPC scheme. This pointestimate ˆ θ k is chosen as a least mean squares estimate ofthe true parameter θ ∗ and is updated recursively using theupdate formula ˜ θ k = ˆ θ k − + µD Tk − ( x k − ( A (ˆ θ k − ) x k − + B (ˆ θ k − ) u k − )) , ˆ θ k = Π Θ k (˜ θ k ) , (8)where Π Θ k ( · ) denotes the projection onto the set Θ k and µ ∈ R > is a constant filter parameter. C. Tube Model Predictive Control
Tube model predictive control, see [12], predicts the statepropagation of a system by using polytopes within whichthe states are guaranteed to be contained, given a boundedadditive disturbance and bounded uncertain parameters. Inorder to guarantee robustness with respect to this boundeddisturbance w k and the uncertain parameters θ , a state tube { X l | k } l ∈ N N is introduced at time step k for the predictedtime step l . This tube consists of N + 1 predicted polytopes X l | k ⊂ R n in the state space at time step k , where N is theprediction horizon of the RAMPC scheme. At each predictedtime step l | k , it can be ensured that the states are inside ofthe polytope X l | k for all w ∈ W and θ ∈ Θ k . It follows thatrobustness is guaranteed if the polytopes do not violate theconstraints in (5). This can thus be reduced to the followingconstraints that must hold ∀ l ∈ N N x k ∈ X | k , (9a) A ( θ ) x + B ( θ ) u l | k ( x ) + w ∈ X l +1 | k , ∀ x ∈ X l | k , w ∈ W , θ ∈ Θ k , (9b) ( x, u l | k ( x )) ∈ Z , ∀ x ∈ X l | k , (9c)for some input mapping u l | k : R n → R m . The polytopes X l | k = { ¯ x l | k } ⊕ α l | k X are defined as translations anddilations of a predefined polytope X = { x ∈ R n | H x x ≤ } , with H x ∈ R n x × n . The translations ¯ x l | k are computedaccording to the dynamics (1) with θ = ¯ θ k and the dilations α l | k are decision variables. Finally, in order to guaranteerecursive feasibility of the scheme, the following terminalconstraint is applied on the final polytope of the tube X N | k : X N | k ⊆ X f , (10)where X f is a terminal set.Given the point estimate of the uncertain parameter ˆ θ k ,the cost function of the RAMPC optimisation problem is ofthe form N − (cid:88) l =0 (cid:18)(cid:13)(cid:13)(cid:13) ˆ x l | k (cid:13)(cid:13)(cid:13) Q + (cid:13)(cid:13)(cid:13) ˆ u l | k (cid:13)(cid:13)(cid:13) R (cid:19) + (cid:13)(cid:13)(cid:13) ˆ x N | k (cid:13)(cid:13)(cid:13) P , (11)here Q ∈ R n × n , R ∈ R m × m are positive definite costmatrices, P ∈ R n × n represents a positive definite terminalcost. The state estimate ˆ x l | k is defined recursively as ˆ x l +1 | k = A (ˆ θ k )ˆ x l | k + B (ˆ θ k )ˆ u l | k , with ˆ x | k = x k . (12)An input parametrisation u l | k ( x ) = Kx + v l | k is used, with aprestabilising feedback matrix K ∈ R m × n and the decisionvariables { v l | k } l ∈ N N − , where v l | k ∈ R m . The prestabilisa-tion matrix K needs to fulfil the following assumption whichfacilitates proving robust constraint satisfaction and (cid:96) -gainstability. This assumption is used in [3] and [5]. Assumption 2.
The feedback gain K is chosen such that A cl ( θ ) = A ( θ ) + B ( θ ) K is stable for all θ ∈ Θ and itholds that A cl ( θ ) T P A cl ( θ ) + Q + K T RK (cid:23) P, ∀ θ ∈ Θ , (13) where Q ∈ R n × n , R ∈ R m × m and P ∈ R n × n are positivedefinite cost matrices of the cost function defined in (11) . Assumption 2 is standard for tube MPC methods, andis also used in [3] and [5]. The prestabilizing gain K andthe terminal cost P can be computed using a semi-definiteprogram, for example as proposed in [5].The RAMPC method in [5] uses the contractivity of thepolytope X for any θ ∈ Θ k , given the prestabilisation K ,in order to rewrite the constraints (9). The definition of a λ -contractive polytope is given as follows. Definition 3.
A polytopic set X = { x ∈ R n | H x x ≤ } is λ -contractive for some λ ∈ [0 , , with respect to some θ ∈ Θ and a feedback gain K , if H x ( A ( θ ) + B ( θ ) K ) x ≤ λ , ∀ x ∈ X . In order to compute the contractivity of a polytope X forall θ ∈ Θ k ⊆ Θ , the following upper bound is used λ ( θ ) ≤ max i,x ∈ X [ H x ] i A cl (¯ θ k ) x + η k max i,j,x ∈ X [ H x ] i D ( x, Kx )˜ e j , Algorithm 1
Computation of a λ -contractive polytope. Initialise H x = (cid:104) F T ( GK ) T (cid:105) T i ← while i ≤ H i − x do H ix ← H i − x for j ∈ N n v,θ do e j ← max x [ H ix ] i A cl ( θ j ) − λ s.t. H ix x ≤ if e j > then H ix = (cid:104) H iTx ( λ [ H ix ] i A cl ( θ j )) T (cid:105) T end if end for i ← i + 1 end while X = { x ∈ R n | H i − x x ≤ } where ˜ e j represents the j -th vertex of the unit hypercube B p . In order to reduce the computation time in the RAMPCscheme, λ (¯ θ k ) is not updated during real-time control inSection IV. As this results in a conservative upper bound onthe contractivity rate, the algorithm from [13] can be adaptedto construct a polytope X with a desired contractivity rate λ for θ ∈ Θ and a given prestabilisation feedback K and isgiven in Algorithm 1. By reducing the initial conservatismon the contractivity, better initial feasibility is achieved.The RAMPC optimisation problem which is solvedat every time step, using α ·| k = { α l | k } l =0 ,...,N and v ·| k = { v l | k } l =0 ,...,N − is: min v ·| k ,α ·| k N − (cid:88) l =0 (cid:13)(cid:13)(cid:13) ˆ x l | k (cid:13)(cid:13)(cid:13) Q + (cid:13)(cid:13)(cid:13) ˆ u l | k (cid:13)(cid:13)(cid:13) R + (cid:13)(cid:13)(cid:13) ˆ x N | k (cid:13)(cid:13)(cid:13) P (14a)s.t. ∀ i ∈ N n x , ∀ j ∈ N p , l ∈ N N − , ¯ x | k = ˜ x | k = x k , α | k = 0 , (14b) ¯ x l +1 | k = A (¯ θ k )¯ x l | k + B (¯ θ k )¯ u l | k , (14c) ¯ u l | k = K ¯ x l | k + v l | k , (14d) ˆ x l +1 | k = A (ˆ θ k )ˆ x l | k + B (ˆ θ k )ˆ u l | k , (14e) ˆ u l | k = K ˆ x l | k + v l | k , (14f) ( F + GK )¯ x l | k + Gv l | k + cα l | k ≤ , (14g) λα l | k + ¯ w + η k [ H x ] i D (¯ x l | k , ¯ u l | k )˜ e j ≤ α l +1 | k , (14h) ( α N | k + [ H x ] i ¯ x N | k ) max i [ c ] i ≤ (14i)with ¯ w = max i max w ∈ W [ H x ] i w and [ c ] i = max x ∈ X [ F + GK ] i x for i ∈ N n z . The optimisation problem consistsof the cost function (14a), the initial condition (14b), thepropagation of the state ¯ x for the centre ¯ θ k of Θ k in (14c) and(14d) and of ˆ x for the point estimate ˆ θ in (14e) and (14f), thestate and input constraints (14g), the tube inclusion constraint(14h) which implies (9b) and finally the terminal constraint(14i). Under the assumption that λ +max i c i ¯ w ≤ holds, it isshown in [5] that (14i) can be used as a terminal constraint toshow recursive feasibility. Thus, with the given assumptionsthe constraints (9) hold and recursive feasibility, stability andconsistency of the parameter estimation are proven in [5]. D. Practical Issues
Steady-state input error and measurement noise affectmany real systems. For the quadrotor simulations and imple-mentations in Sections III and IV, an uncertain mass is used,which results in a steady-state input dependent on the uncer-tain parameter θ for a linearisation of the dynamics aroundthe hover position. This practical issue is not considered inthe methods presented in [3], [4] and [5]. Given a systemwith a true, non-zero steady state input u ss ( θ ∗ ) the steady-state input which is applied is u ss ( θ ) = u ss ( θ ∗ ) + u ss,err ( θ ) with the steady-state input error u ss,err ( θ ) .This steady-state input error affects the dynamics as fol-lows x k +1 = A cl ( θ ∗ ) x k + B ( θ ∗ ) v k + B ( θ ∗ ) u ss,err ( θ ) + w k . In order to be able to guarantee robustness with respectto this steady-state input error, the term B ( θ ∗ ) u ss,err cane considered as an additional disturbance that affects thedynamics. By using [˜ u ] i = max θ ∈ Θ [ H x ] i B ( θ ) u ss,err , thetube inclusion constraint can be rewritten as H x (( A ( θ ) + B ( θ ) K ) x l | k + B ( θ ) v l | k ) + ˜ w + ˜ u ≤ α l +1 | k , ∀ x ∈ X l | k , θ ∈ Θ k . By including this additional term ˜ u , the theoreticalrobustness guarantees of the method are preserved. Inpractice however, this leads to conservatism, as a largeuncertainty set Θ is used for the scenarios, which inturn leads to a large steady-state error compared to thedisturbance. Instead, as the estimate of θ improves over timeby using the set-membership estimation, the steady-stateinput is recomputed as u ss (¯ θ k ) , with ¯ θ k the centre of Θ k .This significantly improves performance at the cost of theloss of robustness guarantees. Compared to directly using themethods in [3], [4] and [5], updating the steady-state inputwith respect to ¯ θ k resulted in reference tracking with smallor no steady-state error, as can be seen in Sections III and IV.The second practical issue to be considered is measure-ment noise on the state ˜ x k = x k + m k . Such noisy mea-surements can lead to the true parameter θ ∗ being removedfrom Θ k in the set-membership identification. Under theassumption of bounded noise m k ∈ M ⊂ R n , with a convexpolytope M , consistency for the set-membership estimationcan still be guaranteed. It must hold that if there existsa noise m ∈ M , that could explain a parameter choice θ ∈ Θ k − given the disturbance w ∈ W , then this parameter θ cannot be eliminated from Θ k . This can be achieved byintroducing dilation factors in the calculation of the non-falsified parameter set ˜∆ k = { θ ∈ R n | − H w D k − θ ≤ h w + H w d k + max m k ∈ M H w m k + max θ ∈ Θ k − ,m k − ∈ M − H w A ( θ ) m k − } , (15)with the set update Θ k ⊇ Θ k − ∩ ˜∆ k . As M is known apriori and and the sets Θ k are restricted to hypercubes, thedilation factors can be easily computed.III. S IMULATION S TUDIES
A. Quadrotor Dynamics
The dynamics of a quadrotor are nonlinear and are repre-sented by 12 states and 4 inputs and their description can befound in [10]. The states and inputs are x = ∆ p ∆ ˙ p ∆ ψ ∆ ˙ ψ , u = ∆ f ∆ f ∆ f ∆ f , (16)where ∆ p is the x -, y -, z -positional deviation from the steady-state position, ∆ ψ = (cid:2) ∆ γ ∆ β ∆ α (cid:3) T are the roll-pitch-yaw angles and ∆ f i are the deviations of the thrusts gener-ated by rotors i from a steady-state input. For the RAMPCscheme described in Section II, the linearisation around the hover position of the quadrotor dynamics is used. Thissteady-state input for the quadrotor is computed by solving y y y y − x − x − x − x c c c c f f f f = mg , (17)with the position of rotor i with respect to the centre ofgravity ( x i , y i ) , a constant of proportionality from rotortorque to thrust force c i , the quadrotor mass m and thegravitational acceleration g . The linearised dynamics are inthe form (1) and are given in [10]. For the discrete timedynamics, an Euler discretisation with a sample time of T s = 0 . s is used. The chosen discretisation and samplingthe simulation showed good flight performance. B. Unknown Mass
The first scenario considered is of a package delivery,where the mass of the quadrotor m is unknown. For this,we use RAMPC in a receding horizon fashion for a controlarchitecture where the control inputs are the individualthrusts of each rotor as described in Section III-A, referredto as direct thrust control in [10]. Note that as m is requiredin the computation of the steady-state input (17), u ss ( θ ) isupdated at every time step as discussed in Section II-D. Theinverse of the mass appears in the dynamics. Thus, θ = m and Θ = [ . , . ] kg − with θ ∗ = . . A constantwind disturbance with a velocity of up to ms in x, y and z -direction is considered. The quadrotor is restricted to operatein a hypercube in space of { ∆ p ∈ R | (cid:107) ∆ p (cid:107) ∞ ≤ . m } .The roll, pitch and yaw angles are restricted to ± π/ . Asinput constraints, the generated rotor thrust for each rotorneeds to lie within [0 , . N. The cost matrices, whichare used are Q=diag(10,10,100,1,1,1,2,2,30,1,1,1)/100 andR=diag(1,1,1,1)/100. All optimisation problems are solvedusing YALMIP [14] with MOSEK [15] and OSQP [16] assolvers. The results of this scenario can be seen in Figure 1,where after 10 time steps, Θ ≈ [35 . , . kg − , whichcorresponds to m ∈ [27 , . g. Note that by using thedirect thrust control mode, robust flight can be ensured inthe lateral directions as well as the altitude. The averagesolve time for solving the optimisation problem as well asupdating the uncertain parameter was ms on a 3.1 GHzIntel i5 CPU. The computation time of ms was achieved byusing Algorithm 1 in order to find a λ -contractive polytope X ⊂ R with a low number of half-spaces n x , as thenumber of constraints in (14) is dependent on n x .In the next simulation, the performance of RAMPC iscompared to a robust MPC controller while performing thepackage delivery task in Figure 2. Note that the mass ofthe quadrotor is assumed to be g for the robust MPCscheme, which lies in Θ and is not updated during theflight. As opposed to the results shown in Figure 1, therobust MPC controller is tracking only the altitude referenceas tracking the x - and y -position at the same time resulted inunstable flight. The robust MPC problem is infeasible nearthe constraints and a steady-state error exists near the origin,s the steady-state input can not updated according to SectionII-D as the mass is not estimated. Position x [m]
Position y [m]
Time [s]
Position z [m]
Fig. 1. Simulation of RAMPC applied to a quadrotor with an uncertainmass and constant wind as a disturbance with position states ( red ) andreference ( blue ). Time [s] -0.100.10.20.30.40.50.60.7
Position z [m]
Fig. 2. Comparison of robust MPC and RAMPC applied to a simulatedquadrotor with an uncertain mass and wind as a disturbance with the altitudefor robust MPC ( black ), RAMPC ( red ) and the reference ( blue ). C. Decoupled Quadrotor Dynamics
The linearisation in Section III-A represents one possiblemeans of controlling a quadrotor via direct rotor control, i.e.the desired thrust for each rotor is computed individuallyand applied directly. This control mode proved to be difficultto implement, as discussed in Section IV. Another possiblemode of control decouples the quadrotor system into an x -position system, a y -position system, a z -position systemand a yaw system which are controlled in an outer loop.The inputs in the decoupled system are the total thrust forcedeviation ∆ f total and ∆ ω x, ref , ∆ ω y, ref and ∆ ω z, ref are thedesired body rates about the x , y , z body axes, respectively.The decoupled system thus needs 3 different controllersfor each subsystem. Since the scenarios considered in oursimulation studies affect the z -direction the most, a RAMPCcontroller was chosen for z -position control while LQRcontrollers were used for the yaw and x - and y -position. Thismeans that for this architecture, the subsystem controlled by RAMPC has 2 states and 1 control input, as given by (cid:20) ∆ ˙ p z ∆¨ p z (cid:21) = (cid:20) (cid:21) (cid:20) ∆ p z ∆ ˙ p z (cid:21) + (cid:34) m (cid:35) ∆ f total . (18)The desired body rates for the x -, y - and yaw-control arecontrolled in a linearised inner loop. This inner controlloop is controlled at a faster sampling rate by using a PIDcontroller. D. Unknown Mass with Decoupled Control
Using the decoupled control architecture, we first studythe package delivery scenario described as in Section III-B. RAMPC is applied to the altitude control of a quadro-tor, where the mass is unknown and its inverse lies in Θ = [ . , . ] kg − with θ ∗ = . . The altitude isconstrained to be within . m of the origin and the resultsof this simulation can be seen in Figure 3 as the dashed line. E. Power Delivery Failure
The final scenario which is considered consists of a suddenpower delivery failure for the decoupled altitude control. Therotor efficiency of all rotors γ ∈ [0 . , can drop at anygiven moment within these bounds and enters the dynamicsas follows x k +1 = Ax k + B (cid:18) m (cid:19) γu k . (19)As the decoupled altitude control (18) is used, the uncertainparameter is θ = γm , (20)with Θ = [ . . , . ] . In order to guarantee robustnessfor this failure at any time step, the lower bound θ k, min of Θ k is dilated at every time step after the parameter updateaccording to θ k, min = min { . θ k, min , θ , min } , where θ , min = . . . The result of such a failure is shown with a dashedline in Figure 4, where the failure occurs at 2 seconds. Theapplied RAMPC scheme manages to keep the quadrotor safeduring the failure and with only a small steady-state error.This small steady-state error persists for the altitude due tothe fact that the parameter set is dilated at every time stepas we require robustness at every time step.IV. E XPERIMENTS
A. Experimental Configuration
A Crazyflie quadrotor is used in the experiments. The massof the Crazyflie is g and it has a size of × × mm.It has a built in IMU consisting of 3 accelerometers andgyroscopes with up to a kHz data rate. It also comes withradio functionality, which is used to send control commandsfrom a laptop, which computes the RAMPC solution, to theCrazyflie.In order to accurately track the position and rotation of theCrazyflie, a Vicon tracking system is used, which consists of6 cameras placed in a room that track the motion of reflectivesurfaces attached to the Crazyflie. The data of the trackingsystem is collected using a separate computer and is sent tothe laptop at a rate of Hz.n the laptop that runs RAMPC, at every time step whennew measurements from the motion tracking system arereceived, the optimisation problem is solved before updatingthe estimates of θ in order to reduce the time delay. Thisoptimisation is solved using OSQP, see [16], and takes lessthan ms. It was observed that the direct thrust controlmode is difficult to implement on the Crazyflie, not only forRAMPC, but also with an LQR controller. Thus, the exper-iments were performed only with RAMPC in the decoupledaltitude control mode in (18). While any wind disturbancewas not experimentally applied, the same disturbance boundsused in simulation were used to account for any linearisationerror. Additionally, due to small measurement noise, the set-membership update with the dilated non-falsified parameterset (15) is used. B. Unknown Mass Experiment
The unknown mass experiment is identical to the sim-ulation configuration described in Section III-D for thedecoupled system. The RAMPC scheme is used for thealtitude control of the quadrotor, while LQR controllers areused for the x and y -position and yaw control. The IMU ofthe Crazyflie stabilises the inner control loop (18) using aPID controller. The quadrotor altitude is initially controlledusing an LQR controller with an assumed mass of g untilRAMPC is activated at t = 3 s. RAMPC is able to identifythe unknown mass of the quadrotor and follows the altitudereference as can be seen in Figure 3. However, comparedto the simulation, the Crazyflie reaches the reference slowerand a small steady-state error exists due to using the dilatednon-falsified parameter set (15), which results in a slowersize reduction of the set of estimates and the differencesbetween the theoretical model and the Crazyflie. Time [s] -0.6-0.4-0.200.20.40.6
Position z [m]
Fig. 3. Comparison of a simulation and implementation of RAMPC for theunknown mass scenario. In simulation, RAMPC is applied to the altitudecontrol of a quadrotor with an uncertain mass and constant wind as adisturbance with position states ( red dashed ) and reference ( blue ). In theimplementation, RAMPC is applied to the altitude control of the Crazyfliewith an uncertain mass with position measurements ( red ), reference ( blue )and the RAMPC activation time ( black ). C. Power Delivery Failure
This experiment is again the same as the experimentdescribed in III-E for the decoupled system. The failure for this practical implementation, which can occur at any giventime step, occurs at around t = 17 s. The occurrence ofthe failure is implemented by lowering the requested thrustsfrom the RAMPC scheme by on the quadrotor. Ascan be seen in Figure 4, the quadrotor recovers successfullyfrom the failure and is able to track the given reference.Similarly to Section IV-B, small discrepancies exist in thetracking performance between simulation and experiment forthe altitude control. Time [s] -0.6-0.4-0.200.20.40.6
Position z [m]
Fig. 4. Comparison of a simulation and implementation of RAMPC forthe power delivery failure scenario. In simulation, RAMPC is applied to thealtitude control of a quadrotor with an uncertain mass and a power deliveryfailure after t = 17 s ( black dashed ) for all rotors, with γ = 0 . and theposition states ( red ) and reference ( blue ). In the implementation, the samefailure is considered with position measurements ( red ), reference ( blue ) andthe RAMPC activation time ( black ). V. C
ONCLUSION