CNLN: Quadcopter (Crazyflie)

From:
da Cunha, A.E.C.: Benchmark: Quadrotor attitude control.
Dreossi T., Dang T., Piazza C.: Reachability Computation for Polynomial Dynamical Systems.

Dynamics:
pn' = u*(2*pow(q0v,2) + 2*pow(q1v,2) - 1) - v*(2*q0v*q3v - 2*q1v*q2v ) + w*(2*q0v*q2v + 2*q1v*q3v )
pe' = v*(2*pow(q0v,2) + 2*pow(q2v,2) - 1) + u*(2*q0v*q3v + 2*q1v*q2v ) - w*(2*q0v*q1v - 2*q2v*q3v )
h' = w*(2*pow(q0v,2) + 2*pow(q3v,2) - 1) - u*(2*q0v*q2v - 2*q1v*q3v ) + v*(2*q0v*q1v + 2*q2v*q3v )

u' = r*v - q*w - g*(2*q0v*q2v - 2*q1v*q3v )
v' = p*w - r*u + g*(2*q0v*q1v + 2*q2v*q3v )
w' = q*u - p*v -F/m + g*(2*pow(q0v,2) + 2*pow(q3v,2) - 1 )

q0v' = -(q1v/2)*p - (q2v/2)*q - (q3v/2)*r
q1v' = (q0v/2)*p - (q3v/2)*q + (q2v/2)*r
q2v' = (q3v/2)*p + (q0v/2)*q - (q1v/2)*r
q3v' = (q1v/2)*q - (q2v/2)*p + (q0v/2)*r

p' = (1/Jx)*tauphi + ((Jy - Jz)/Jx)*q*r
q' = (1/Jy)*tautheta - ((Jx - Jz)/Jy)*p*r
r' = (1/Jz)*taupsi + ((Jx - Jy)/Jz)*p*q

hI' = h - hr
uI' = u - ur
vI' = v - vr
psiI' = psi - psir

F = 0.0361*hI + 0.0694*h + 0.0603*w
tauphi = -0.0003*vI - 0.0005*v - 0.0018*phi - 0.0004*p
tautheta = 0.0003*uI + 0.0005*u - 0.0018*theta - 0.0004*q
taupsi = -0.0003*psiI - 0.0006*psi - 0.0003*r

phi = 2*q1v
theta = 2*q2v
psi = 2*q3v

Parameters:
M = 0.0015
mr = 0.001
R = 0.020
l = 0.045
g = 9.81
m = M + 4*mr
Jx = (2*M*pow(R,2))/5 + 2*pow(l,2)*mr
Jy = (2*M*pow(R,2))/5 + 2*pow(l,2)*mr
Jz = (2*M*pow(R,2))/5 + 4*pow(l,2)*mr

// Reference values
ur = 0
vr = 0
psir = 0
hr = 1

delta = 0.01 (for Euclid discretization)

Initial set:
h(0) \in [0.20, 0.21]
other vars = 0