diff --git a/mavion/mavion.py b/mavion/mavion.py
new file mode 100644
index 0000000000000000000000000000000000000000..65326433f6b440945254606b0f3c974515a60895
--- /dev/null
+++ b/mavion/mavion.py
@@ -0,0 +1,433 @@
+import matplotlib.pyplot as plt
+import numpy as np
+import gymnasium as gym
+from gymnasium.envs.classic_control import utils
+import yaml
+from scipy.optimize import fsolve
+from scipy.integrate import solve_ivp
+
+
+deg2rad = 2 * np.pi / 360
+
+def hat(x):
+ mat = np.array([[0, -x[2], x[1]], [x[2], 0, -x[0]], [-x[1], x[0], 0]])
+ return mat
+
+def quat2dcm(q):
+ dcm = np.array(
+ [
+ [
+ q[0] ** 2 + q[1] ** 2 - q[2] ** 2 - q[3] ** 2,
+ 2 * (q[1] * q[2] + q[0] * q[3]),
+ 2 * (q[1] * q[3] - q[0] * q[2]),
+ ],
+ [
+ 2 * (q[1] * q[2] - q[0] * q[3]),
+ q[0] ** 2 - q[1] ** 2 + q[2] ** 2 - q[3] ** 2,
+ 2 * (q[2] * q[3] - q[0] * q[1]),
+ ],
+ [
+ 2 * (q[1] * q[3] + q[0] * q[2]),
+ 2 * (q[2] * q[3] - q[0] * q[1]),
+ q[0] ** 2 - q[1] ** 2 - q[2] ** 2 + q[3] ** 2,
+ ],
+ ]
+ )
+ return dcm
+
+def eul2quat(a):
+ c = np.cos(a/2)
+ s = np.sin(a/2)
+ q = np.array(
+ [
+ c[0] * c[1] * c[2] + s[0] * s[1] * s[2],
+ -c[0] * s[1] * s[2] + c[0] * c[1] * s[2],
+ c[0] * s[1] * c[2] + s[0] * c[1] * s[2],
+ c[0] * c[1] * s[2] - s[0] * s[1] * c[2],
+ ]
+ )
+ return q
+
+def quat2eul(q):
+ phi = np.arctan2(2 * (q[0] * q[1] + q[2] * q[3]), 1 - 2 * (q[1] * q[1] + q[2] * q[2]))
+ theta = np.arcsin(2 * (q[0] * q[2] - q[1] * q[3]))
+ psi = np.arctan2(2 * (q[0] * q[3] + q[1] * q[2]), 1 - 2 * (q[3] * q[3] + q[2] * q[2]))
+ return np.array([phi, theta, psi])
+
+def eul2dcm(a):
+ Rx = np.array([[1, 0, 0],
+ [0, np.cos(a[0]), np.sin(a[0])],
+ [0, -np.sin(a[0]), np.cos(a[0])]])
+
+ Ry = np.array([[np.cos(a[1]), 0, -np.sin(a[1])],
+ [0, 1, 0],
+ [np.sin(a[1]), 0, np.cos(a[1])]])
+
+ Rz = np.array([[np.cos(a[2]), np.sin(a[2]), 0],
+ [-np.sin(a[2]), np.cos(a[2]), 0],
+ [0, 0, 1]])
+
+ return Rx @ Ry @ Rz
+
+
+def s2q(x):
+ quat = eul2quat(x[3:6])
+ x = np.concatenate([x[0:3], quat, x[6:12]])
+ return x
+
+def q2s(x):
+ eul = quat2eul(x[3:7])
+ x = np.concatenate([x[0:3], eul, x[7:13]])
+ return x
+
+
+class Mavion:
+
+ def __init__(self, file="mavion.yaml"):
+ stream = open(file, "r")
+ drone = yaml.load(stream, Loader=yaml.Loader)
+
+ self.NAME = drone['NAME']
+ self.RHO = drone['RHO']
+ self.G = drone['G']
+ self.MASS = drone['MASS']
+ self.CHORD = drone['CHORD']
+ self.WINGSPAN = drone['WINGSPAN']
+ self.WET_SURFACE = drone['WET_SURFACE']
+ self.DRY_SURFACE = drone['DRY_SURFACE']
+ self.PROP_RADIUS = drone['PROP_RADIUS']
+
+ self.Cd0 = 0.1
+ self.Cy0 = 0.1
+ self.dR = -0.1 * self.CHORD
+ self.PHI_fv = np.diag([self.Cd0, self.Cy0, (2 * np.pi + self.Cd0)])
+ self.PHI_mv = np.array(
+ [
+ [0, 0, 0],
+ [0, 0, -1 / self.CHORD * self.dR * (2 * np.pi + self.Cd0)],
+ [0, 1 / self.WINGSPAN * self.dR * self.Cy0, 0],
+ ]
+ )
+ self.PHI_mw = 1 / 2 * np.diag([0.5, 0.5, 0.5])
+ self.PHI = np.block([[self.PHI_fv, self.PHI_mv], [self.PHI_mv, self.PHI_mw]])
+ self.PHI_n = drone["PHI_n"]
+
+ # acording to back-of-the-envelope computations
+ self.INERTIA = np.diag(drone["INERTIA"])
+
+ # geometric parameters
+ self.P_P1_CG = np.array(drone["P_P1_CG"]) * self.CHORD
+ self.P_P2_CG = np.array(drone["P_P2_CG"]) * self.CHORD
+ self.P_A1_CG = np.array(drone["P_A1_CG"]) * self.CHORD
+ self.P_A2_CG = np.array(drone["P_A2_CG"]) * self.CHORD
+
+ self.ELEVON_MEFFICIENCY = np.array(drone['ELEVON_MEFFICIENCY'])
+ self.ELEVON_FEFFICIENCY = np.array(drone['ELEVON_FEFFICIENCY'])
+ self.PROP_KP = drone['PROP_KP']
+ self.PROP_KM = drone['PROP_KM']
+ self.INERTIA_PROP_X = drone['INERTIA_PROP_X']
+ self.INERTIA_PROP_N = drone['INERTIA_PROP_N']
+
+ self.pos = np.zeros(3)
+ self.vel = np.zeros(3)
+ self.ang = np.zeros(3)
+ self.rot = np.zeros(3)
+ self.quat = eul2quat(self.ang)
+
+ def thrust(self, dx):
+ """
+ THRUST thrust forces and moments modeling
+ this function computes the propeller thrust by means of simple
+ prop formulas commonly applied in quadrotor literature.
+
+ INPUTS:
+ motor rot. : dx in R(1x1)
+ + required self specs:
+ |---- PROP_KP in R(1x1)
+ |---- PROP_KM in R(1x1)
+
+ OUTPUTS:
+ Prop force : T (N) (body axis) in R(3x1)
+ Prop moment: N (Nm) (body axis) in R(3x1)
+
+ vl: vehicle velocity in NED axis (m/s) [3x1 Real]
+ wb: vehicle angular velocity in body axis (rad/s) [3x1 Real]
+ q: quaternion attitude (according to MATLAB convention) [4x1 Real]
+
+ NOTE1: notice that motor rotation's sign depend on which section we are due to
+ counter-rotating propellers;
+ NOTE2: elevon sign convention is positive pictch-up deflections.
+ NOTE3: gyroscopic effects are implemented in the caller function!
+
+ refer to [1] for further information.
+
+ REFERENCES:
+ [1] Lustosa L.R., Defay F., Moschetta J.-M., "The Phi-theory
+ approach to flight control design of tail-sitter vehicles"
+ @ http://lustosa-leandro.github.io
+
+ """
+
+ kp = self.PROP_KP
+ km = self.PROP_KM
+
+ # prop forces computation / notice that negative thrust is not implemented
+ T = kp * dx**2 * np.array([1, 0, 0])
+
+ # prop moments computation
+ N = np.sign(dx) * km * dx**2 * np.array([1, 0, 0])
+
+ return [T, N]
+
+ def aero(self, quat, vel, rot, T, de, w):
+ """
+ AERO aerodynamic forces and moments computation (in a wing section!)
+ this function computes the aerodynamic forces and moments in body axis
+ in view of Phi-theory in a wing section. it includes propwash effects
+ due to thrust.
+ NOTE2: elevon sign convention is positive pictch-up deflections.
+ """
+
+ # data extraction from self struct
+ PHI_fv = self.PHI_fv
+ PHI_mv = self.PHI_mv
+ PHI_mw = self.PHI_mw
+ phi_n = self.PHI_n
+ RHO = self.RHO
+ Swet = self.WET_SURFACE
+ Sdry = self.DRY_SURFACE
+ chord = self.CHORD
+ ws = self.WINGSPAN
+ Prop_R = self.PROP_RADIUS
+ Thetam = self.ELEVON_MEFFICIENCY
+ Thetaf = self.ELEVON_FEFFICIENCY
+
+ # derivative data
+ Sp = np.pi * Prop_R**2
+
+ # DCM computation
+ D = quat2dcm(quat)
+
+ # freestream velocity computation in body frame
+ vinf = D @ (vel - w)
+
+ # computation of total wing section area
+ S = Swet + Sdry
+
+ # computation of chord matrix
+ B = np.diag([ws, chord, ws])
+
+ # eta computation
+ eta = np.sqrt(np.linalg.norm(vinf)**2 + phi_n * np.linalg.norm(B@rot)**2)
+
+ # Force computation
+ # airfoil contribution
+ Fb = -1/2*RHO*S*eta * PHI_fv@vinf - 1/2*RHO*S*eta * PHI_mv@B@rot - 1/2*Swet/Sp * PHI_fv@T
+ # elevon contribution
+ Fb = Fb + 1/2*RHO*S*eta * PHI_fv@np.cross(de*Thetaf, vinf) \
+ + 1/2*RHO*S*eta * PHI_mv@B@np.cross(de*Thetaf, rot) \
+ + 1/2*Swet/Sp * PHI_fv@np.cross(de*Thetaf, T)
+
+ # Moment computation
+ # airfoil contribution
+ Mb = -1/2*RHO*S*eta * B@PHI_mv@vinf - 1/2*RHO*S*eta * B@PHI_mw@B@rot - 1/2*Swet/Sp * B@PHI_mv@T
+ # elevon contribution
+ Mb = Mb + 1/2*RHO*S*eta * B@PHI_mv@np.cross(de*Thetam, vinf) \
+ + 1/2*RHO*S*eta * B@PHI_mw@B@np.cross(de*Thetam, rot) \
+ + 1/2*Swet/Sp * B@PHI_mv@np.cross(de*Thetam, T)
+
+ return [Fb, Mb]
+
+ def dyn(self, x, u, w):
+
+ G = self.G
+ MASS = self.MASS
+ INERTIA = self.INERTIA
+
+ # position of propellers wrt center of gravity
+ P_P1_CG = self.P_P1_CG
+ P_P2_CG = self.P_P2_CG
+ # position of aerodynamic wrenches wrt center of gravity
+ P_A1_CG = self.P_A1_CG
+ P_A2_CG = self.P_A2_CG
+ # propeller blade inertia
+ INERTIA_PROP_X = self.INERTIA_PROP_X
+ INERTIA_PROP_N = self.INERTIA_PROP_N
+
+ # state demultiplexing
+ quat = x[3:7]
+ vel = x[7:10]
+ rot = x[10:13]
+
+ # input demultiplexing
+ dx1 = u[0]
+ dx2 = u[1]
+ de1 = u[2]
+ de2 = u[3]
+
+ # linear velocity derivative
+ D = quat2dcm(quat)
+
+ [T1, N1] = self.thrust(dx1)
+ [T2, N2] = self.thrust(dx2)
+ [A1, M1] = self.aero(quat, vel, rot, T1, de1, w)
+ [A2, M2] = self.aero(quat, vel, rot, T2, de2, w)
+
+ Fb = T1 + T2 + A1 + A2
+ dvdt = np.array([0, 0, G]) + 1/MASS * D.T@Fb
+
+ # angular velocity derivative
+ Ma = M1 + M2 + np.cross(P_A1_CG, A1) + np.cross(P_A2_CG, A2)
+
+ P = rot[0]
+ Q = rot[1]
+ R = rot[2]
+ Jpx = INERTIA_PROP_X
+ Jpn = INERTIA_PROP_N
+ tau1 = N1 - (P + dx1) * (Jpx - Jpn) * np.array([0, R, -Q])
+ tau2 = N2 - (P + dx2) * (Jpx - Jpn) * np.array([0, R, -Q])
+ Mg = tau1 + tau2 + np.cross(P_P1_CG, T1) + np.cross(P_P2_CG, T2)
+
+ Mb = Ma + Mg
+ drdt = np.linalg.inv(INERTIA) @ (Mb - np.cross(rot, INERTIA@rot))
+
+ # kinematic equations
+ dpdt = vel
+
+ dqdt = np.zeros(4)
+ dqdt[0] = -1/2 * np.dot(rot, quat[1:4])
+ dqdt[1:4] = 1/2 * (quat[0]*rot - np.cross(rot, quat[1:4]))
+
+ return np.concatenate([dpdt, dqdt, dvdt, drdt])
+
+ def trim(self, vh, vz):
+ x = np.zeros(13)
+ x[7] = vh
+ x[9] = vz
+
+ def func(y):
+ dx, de, theta = y
+ eul = np.array([0, theta, 0])
+ quat = eul2quat(eul)
+ x[3:7] = quat
+ u = np.array([-dx, dx, de, de])
+ dx_dt = self.dyn(x, u, np.zeros(3))
+ return dx_dt[7], dx_dt[9], dx_dt[11]
+
+ y0 = np.array([100, 0, 0])
+ y = fsolve(func, y0)
+ return y
+
+ def step(self, x, u, w, dt):
+ func = lambda t, s: self.dyn(s, u, w)
+ sol = solve_ivp(func, (0, dt), x, method='RK45')
+ return sol.y.T[-1]
+
+ def sim(self, t_span, x0, fctrl, fwind):
+ func = lambda t, s: self.dyn(s, fctrl(t, s, w), fwind(t, s))
+ sol = solve_ivp(func, t_span, x0, method='RK45', max_step=0.01)
+ return sol
+
+
+class MavionEnv(gym.Env):
+
+ def __init__(self, mavion=Mavion(), render_mode=None):
+
+ self.tau = 0.02 # seconds between state updates
+
+ # Angle at which to fail the episode
+ self.pos_threshold = np.array([2.4, 2.4, 2.4])
+ self.ang_threshold = np.array([15, 15, 15]) * deg2rad
+ self.vel_threshold = np.finfo(np.float32).max * np.ones(3)
+ self.rot_threshold = np.finfo(np.float32).max * np.ones(3)
+
+ high = np.concatenate(
+ [
+ self.pos_threshold * 2,
+ self.ang_threshold * 2,
+ self.vel_threshold,
+ self.rot_threshold
+ ],
+ dtype=np.float32,
+ )
+ self.observation_space = gym.spaces.Box(-high, high, dtype=np.float32)
+
+ self.action_space = gym.spaces.Box(0, 1, shape=(4,), dtype=np.float32)
+
+ self.target = None
+ self.obs = None
+ self.steps_beyond_terminated = None
+
+ self.mavion = mavion
+
+ def reset(self, seed=None, options=None):
+ super().reset(seed=seed)
+
+ # Note that if you use custom reset bounds, it may lead to out-of-bound
+ # state/observations.
+ low, high = utils.maybe_parse_reset_bounds(
+ options, -0.05, 0.05 # default low
+ ) # default high
+ self.target = np.zeros(12)
+ self.obs = self.np_random.uniform(low=low, high=high, size=(12,))
+ self.state = self.obs + self.target
+ self.steps_beyond_terminated = None
+
+ return self.obs, {}
+
+ def step(self, action):
+ x = s2q(self.state)
+ u = action
+ w = np.zeros(3)
+ new_state = self.mavion.step(x, u, w, self.tau)
+ self.state = q2s(new_state)
+ terminated = np.array_equal(self.state, self.target)
+ reward = 1 if terminated else 0 # Binary sparse rewards
+ self.obs = self.state - self.target
+ return self.obs, reward, terminated, False, {}
+
+
+if __name__ == "__main__":
+ mavion = Mavion()
+
+ vh = 5
+ vz = 0
+ dx, de, theta = mavion.trim(vh, vz)
+ theta = (theta + np.pi) % (2*np.pi) - np.pi
+ print(dx, de, theta*57.3)
+
+ pos = np.array([0, 0, -10])
+ eul = np.array([0, theta, 0])
+ vel = np.array([vh, 0, vz])
+ rot = np.array([0, 0, 0])
+ st = np.concatenate([pos, eul, vel, rot])
+ x = s2q(st)
+
+ u = np.array([-dx, dx, de, de])
+ w = np.zeros(3)
+
+ def ctl(t, s, w): return u
+ def wnd(t, s): return w
+
+ sim = mavion.sim((0, 20), x, ctl, wnd)
+
+ fig, axs = plt.subplots(2, 2, layout='constrained')
+ ax = axs[0][0]
+ ax.plot(sim.t, sim.y[0:3].T)
+ ax = axs[1][0]
+ ax.plot(sim.t, sim.y[3:7].T)
+ ax = axs[0][1]
+ ax.plot(sim.t, sim.y[7:10].T)
+ ax = axs[1][1]
+ ax.plot(sim.t, sim.y[10:13].T)
+ plt.show()
+
+
+ # mavion_env = MavionEnv(mavion)
+
+ # st1 = mavion_env.reset()[0]
+ # print("reset :", st1)
+
+ # mavion_env.state = st
+ # st2 = mavion_env.step(u)[0]
+ # print("new state :", st2)