Repository: nasa/prog_models (language: python)

Issue:
Integrate ESC Model
Integrate ESC Model from SWS Project
Float time
Update time to use float instead of date time. 

Also, small changes such as:
1.  Moved function to standalone instead of method
2. Removed eta_unix waypoint property
3. Removed vehicle model from use in Trajectory
4. Other cleanup


Discussion / hints:

Fix patch:
diff --git a/.github/workflows/coverage.yml b/.github/workflows/coverage.yml
index 92b3e19e5..5fd6cfc2c 100644
--- a/.github/workflows/coverage.yml
+++ b/.github/workflows/coverage.yml
@@ -47,6 +47,7 @@ jobs:
         coverage run -a -m tests.test_powertrain
         coverage run -a -m tests.test_serialization
         coverage run -a -m tests.test_sim_result
+        coverage run -a -m tests.test_uav_model
         coverage xml -i
     - name: "Upload coverage to Codecov"
       uses: codecov/codecov-action@v3
\ No newline at end of file
diff --git a/.github/workflows/python-package.yml b/.github/workflows/python-package.yml
index a9856c969..505b8d238 100644
--- a/.github/workflows/python-package.yml
+++ b/.github/workflows/python-package.yml
@@ -339,6 +339,24 @@ jobs:
         pip install testbook
     - name: Run tests
       run: python -m tests.test_tutorials
+  test_uav_model:
+    timeout-minutes: 10
+    runs-on: ubuntu-latest
+    steps:
+    - uses: actions/checkout@v3
+    - name: Set up Python
+      uses: actions/setup-python@v4
+      with:
+        python-version: '3.7'
+    - name: Install dependencies cache
+      uses: actions/cache@v2
+      with:
+        path: ~/.cache/pip
+        key: pip-cache
+    - name: Update
+      run: pip install --upgrade --upgrade-strategy eager -e .
+    - name: Run tests
+      run: python -m tests.test_uav_model
   # test_prog_algs_released:
   #   timeout-minutes: 30
   #   runs-on: ubuntu-latest
diff --git a/examples/uav_dynamics_model.py b/examples/uav_dynamics_model.py
new file mode 100644
index 000000000..437d89ddd
--- /dev/null
+++ b/examples/uav_dynamics_model.py
@@ -0,0 +1,155 @@
+# Copyright © 2021 United States Government as represented by the Administrator of the
+# National Aeronautics and Space Administration.  All Rights Reserved.
+
+"""
+Example of generating a trajectory for a small rotorcraft through a set of coarse waypoints, and simulate the rotorcraft flight using a 6-dof model.
+"""
+
+import matplotlib.pyplot as plt
+import numpy as np
+
+from prog_models.utils.traj_gen import Trajectory
+from prog_models.models.aircraft_model import SmallRotorcraft
+from prog_models.loading.controllers import LQR_I, LQR
+
+
+def run_example():
+    # Initialize vehicle
+    vehicle = SmallRotorcraft(
+        dt=0.05,
+        vehicle_model='tarot18',
+        process_noise=0,
+        measurement_noise=0
+    )
+
+    # EXAMPLE 1:
+    # Define coarse waypoints: latitudes, longitudes, and altitudes are
+    # required, ETAs are optional
+    # Latitudes and longitudes must be defined as numpy arrays of size n x 1
+    # and with unit radians
+    # Altitudes must be defined as numpy arrays of size n x 1 with unit meters
+    # ETAs (if included) must be defined as a list of datetime objects
+    # If ETAs are not included, speeds must be defined (see Example 2)
+
+    # Here, we specify waypoints in a dictionary and then pass
+    # lat/lon/alt/ETAs into the trajectory class
+    lat_deg = np.array([37.09776, 37.09776, 37.09776, 37.09798, 37.09748, 37.09665, 37.09703, 37.09719, 37.09719, 37.09719, 37.09719, 37.09748, 37.09798, 37.09776, 37.09776])
+    lon_deg = np.array([-76.38631, -76.38629, -76.38629, -76.38589, -76.3848, -76.38569, -76.38658, -76.38628, -76.38628, -76.38628, -76.38628, -76.3848, -76.38589, -76.38629, -76.38629])
+    alt_ft = np.array([-1.9682394, 164.01995, 164.01995, 164.01995, 164.01995, 164.01995, 164.01995, 164.01995, 0.0, 0.0, 164.01995, 164.01995, 164.01995, 164.01995, 0.0])
+    time_unix = [1544188336, 1544188358, 1544188360, 1544188377, 1544188394, 1544188411, 1544188428, 1544188496, 1544188539, 1544188584, 1544188601, 1544188635, 1544188652, 1544188672, 1544188692]
+
+    # Generate trajectory
+    # =====================
+    # Generate trajectory object and pass the route (waypoints, ETA) to it
+    traj = Trajectory(lat=lat_deg * np.pi/180.0,
+                      lon=lon_deg * np.pi/180.0,
+                      alt=alt_ft * 0.3048,
+                      etas=time_unix)
+
+    ref_traj = traj.generate(dt=vehicle.parameters['dt'])
+
+    # Define controller and build scheduled control. The controller acts as a
+    # future_loading function when simulating
+    # We use a linear quadratic regulator (LQR), which tries to minimize the
+    # cost function defined by:
+    # J = \int{ x^T Q x + u^T R u \mathrm{d}t }
+    # Where x is the state vector, u is the input vector, t is time, Q is the
+    # state error penalty matrix, and R is the input generation penalty matrix.
+    # The LQR uses a linearized version of the dynamic system
+    # (i.e., dxdt = A x + Bu) to find the gain matrix K that minimizes the cost J.
+    ctrl = LQR(ref_traj, vehicle)
+
+    # Simulate vehicle to fly trajectory
+    traj_results = vehicle.simulate_to_threshold(
+        ctrl,
+        dt=vehicle.parameters['dt'],
+        save_freq=vehicle.parameters['dt'])
+
+    # Visualize Results
+    vehicle.visualize_traj(pred=traj_results, ref=ref_traj)
+
+    # EXAMPLE 2:
+    # In this example, we define another trajectory through the same
+    # waypoints but with speeds defined instead of ETAs
+    
+    # Generate trajectory object and pass the route (lat/lon/alt, no ETAs)
+    # and speed information to it
+    traj_speed = Trajectory(lat=lat_deg * np.pi/180.0,
+                            lon=lon_deg * np.pi/180.0,
+                            alt=alt_ft * 0.3048,
+                            cruise_speed=8.0,
+                            ascent_speed=2.0,
+                            descent_speed=3.0,
+                            landing_speed=2.0)
+    ref_traj_speeds = traj_speed.generate(dt=vehicle.parameters['dt'])
+
+    # Define controller and build scheduled control. This time we'll use LQR_I,
+    # which is a linear quadratic regulator with integral action.
+    # The integral action has the same purpose of "I" in PI or PID controllers,
+    # which is to minimize offset errors in the variable of interest.
+    # This version of LQR_I compensates for integral errors in the position of
+    # the vehicle, i.e., x, y, z variables of the state vector.
+    ctrl_speeds = LQR_I(ref_traj_speeds, vehicle)
+    
+    # Set simulation options
+    options = {
+        'dt': vehicle.parameters['dt'],
+        'save_freq': vehicle.parameters['dt']
+    }
+
+    # Simulate vehicle to fly trajectory
+    traj_results_speeds = vehicle.simulate_to_threshold(ctrl_speeds, **options)
+
+    # Visualize results - notice these results are slightly different, since
+    # the speeds through the waypoints (and therefore the resulting trajectory)
+    # are different than Example 1
+    vehicle.visualize_traj(pred=traj_results_speeds, ref=ref_traj_speeds)
+
+    # EXAMPLE 3:
+    # In this example, we just want to simulate a specific portion of the
+    # reference trajectory
+    # We will simulate the second cruise interval in Example 1,
+    # i.e. waypoints 10 - 13 (where the first waypoint is index 0).
+    # We will use the reference trajectory (ref_traj) and controller (ctrl)
+    # already generated in Example 1
+
+    # First, we'll re-define the ETAs in the waypoints dictionary
+    # (since we deleted them from the waypoints in Example 2)
+    time_unix = np.array([1544188336, 1544188358, 1544188360, 1544188377, 1544188394, 1544188411, 1544188428, 1544188496, 1544188539, 1544188584, 1544188601, 1544188635, 1544188652, 1544188672, 1544188692])
+
+    # Extract time information for desired interval, starting at waypoint 10
+    # and ending at waypoint 13
+    start_time = time_unix[10] - time_unix[0]
+    end_time = time_unix[13] - time_unix[0]
+    sim_time = end_time - start_time
+
+    # Define initial state, x0, based on reference trajectory at start_time
+    ind = np.where(ref_traj['t'] == start_time)
+    x0 = {key: ref_traj[key][ind][0] for key in ref_traj.keys()}
+    vehicle.parameters['x0'] = x0
+
+    # Define simulation parameters - note that we must define t0 as start_time
+    # since we are not starting at the default of t0 = 0
+    options = {
+        'dt': vehicle.parameters['dt'],
+        'save_freq': vehicle.parameters['dt'],
+        't0': start_time
+    }
+
+    # Simulate starting from this initial state from start_time to end_time
+    traj_results_interval = vehicle.simulate_to(sim_time, ctrl, **options)
+
+    # Plot results with Example 1 results to show equivalence on this interval
+    z_1 = [output['z'] for output in traj_results.outputs]
+    z_4 = [output['z'] for output in traj_results_interval.outputs]
+
+    fig, ax = plt.subplots()
+    ax.plot(traj_results.times, z_1, '-b', label='Example 1')
+    ax.plot(traj_results_interval.times, z_4, '--r', label='Example 3')
+    ax.set_xlabel('time, s', fontsize=14)
+    ax.set_ylabel('altitude, m', fontsize=14)
+    ax.legend()
+
+# This allows the module to be executed directly
+if __name__ == '__main__':
+    run_example()
diff --git a/src/prog_models/loading/controllers.py b/src/prog_models/loading/controllers.py
new file mode 100644
index 000000000..a9b311bd2
--- /dev/null
+++ b/src/prog_models/loading/controllers.py
@@ -0,0 +1,297 @@
+# Copyright © 2021 United States Government as represented by the Administrator of the
+# National Aeronautics and Space Administration.  All Rights Reserved.
+
+import numpy as np
+
+# LQR general function
+# ====================
+def lqr_calc_k(A, B, Q, R):
+    """
+    Calculate control matrix K using LQR
+
+    The function estimates the gain matrix associated with a linear quadratic regulator(LQR) via Hamiltonian matrix.
+    The function assumes a linearized system:
+
+    dx / dt = Ax + Bu
+    y = Cx + Du
+
+    The closed loop uses - K * x as input dx / dt = (A - B * K) * x
+
+    :param A:       Nx x Nx state matrix, linearized w.r.t. current attitude and thrust
+    :param B:       Nu x Nx input matrix, linearized w.r.t. current attitude
+    :param Q:       Nx x Nx state cost matrix, the higher the values of the (i,i)-th element, the more aggresive the controller is w.r.t. that variable
+    :param R:       Nx x Nu input cost matrix, higher values imply higher cost of producing that input (used to limit the amount of power)
+    :return K:      Nx x Nu, control gain matrix
+    :return E:      Nx x 1, eigenvalues of the matrix A - B * K
+    """
+    n     = A.shape[0]
+    R_inv = np.linalg.inv(R)
+    B_tr  = B.T
+
+    # --------- Generate hamiltonian matrix ------------ #
+    HM1 = A
+    HM2 = - np.dot(np.dot(B, R_inv), B_tr)
+    HM3 = - Q
+    HM4 = - A.T
+
+    HM12 = np.concatenate((HM1, HM2), axis=1)
+    HM34 = np.concatenate((HM3, HM4), axis=1)
+    HM   = np.concatenate((HM12, HM34), axis=0)
+
+    # --------- Extract eigevectors whose eigenvalues have real part < 0 ------------ #
+    eig_val, eig_vec = np.linalg.eig(HM)
+    V_ = eig_vec[:, np.real(eig_val) < 0.0]
+
+    # --------- Partition matrix of resulting eigenvectors ------------ #
+    X = V_[:n, :n]  # first half (row-wise)
+    Y = V_[n:, :n]  # second half (row-wise)
+
+    # ----------- Estimate control gain ----------------- #
+    K = np.dot(np.dot(np.dot(R_inv, B_tr), Y), np.linalg.inv(X))
+    K = np.real(K)  # some spuriorus imaginary parts (1e-13) sometimes remain in the matrix. we manually remove them
+
+    # Calculate the eigenvalues of the matrix A - B*K
+    E, _ = np.linalg.eig(A - np.dot(B, K))
+    return K, E
+
+    
+# # CONTROLLERS: FULL STATE VECTOR CONTROL  (i.e., no powertrain)
+# # ==============================================================
+class LQR():
+    """
+    Linear Quadratic Regulator UAV Controller
+
+    A Controller that calculates the vehicle control inputs for a UAV model (prog_models.models.uav_model).
+
+    args:
+        x_ref (dict[str, np.ndarray]):
+            dictionary of reference trajectories for each state variable (x, y, z, phi, theta, psi, ...)
+        vehicle:
+            UAV model object
+
+    keyword args: 
+        Q (np.ndarray):
+            State error penalty matrix, size num_states x num_states (where num_states only includes dynamic states)
+            Represents how 'bad' an error is in the state vector w.r.t. the reference state vector
+        R (np.ndarray):
+            Input penalty matrix, size num_inputs x num_inputs (where num_inputs only includes inputs that inform the system dynamics)
+            Represents how 'hard' it is to produce the desired input (thrust and three moments along three axes)
+        scheduled_var (str):
+            Variable used to create the scheduled controller gains; must correspond to a state key
+        index_scheduled_var (int):
+            Index corresponding to the scheduled_var in the state vector
+
+    """
+    def __init__(self, x_ref, vehicle, **kwargs):
+
+        # Check correct arguments:
+        if not isinstance(x_ref, dict):
+            raise TypeError("Reference trajectory must be a dictionary of numpy arrays for each state throughout time.")
+        for vals in x_ref.values():
+            if not isinstance(vals, np.ndarray):
+                raise TypeError("Reference trajectory must be a dictionary of numpy arrays for each state throughout time.")
+
+        self.states    = vehicle.states         # state variables of the system to be controlled (x, y, z, phi, theta, psi)
+        self.n_states  = len(self.states) - 2   # number of states (minus two to remove time and mission_complete)
+        self.inputs    = vehicle.inputs         # input variables of the system to be controlled
+        self.n_inputs  = len(self.inputs) - 1   # number of inputs (minus one to remove mission_complete)
+        self.ref_traj  = x_ref                  # reference state to follow during simulation (x_ref, y_ref, z_ref, phi_ref, theta_ref, psi_ref, ...)
+        self.ss_input  = vehicle.parameters['steadystate_input']
+        self.vehicle_max_thrust = vehicle.dynamics['max_thrust']
+        
+        # Default control parameters
+        # --------------------------------
+        self.parameters = dict(Q=np.diag([1000, 1000, 25000, 100.0, 100.0, 100.0, 1000, 1000, 5000, 1000, 1000, 1000]),  # state error penalty matrix: how 'bad' is an error in the state vector w.r.t. the reference state vector
+                               R=np.diag([500, 4000, 4000, 4000]), # input penalty matrix: how 'hard' it is to produce the desired input (thrust and three moments along three axes)
+                               scheduled_var='psi',     # variable used to create the scheduled controller gains (only psi allowed for now)
+                               index_scheduled_var=5)   # index corresponding to the scheduled_var (psi) in the state vector x; i.e., x[5] = psi
+        self.parameters.update(kwargs)                  # update control parameters according to user
+
+        # Get scheduled variable index
+        self.parameters['index_scheduled_var'] = self.states.index(self.parameters['scheduled_var'])
+
+        # Initialize other controller-related variables
+        # ---------------------------------------------
+        self.dt = vehicle.parameters['dt']
+
+        # Initialize control gain storage matrix:
+        # ---------------------------------------
+        self.control_gains = np.zeros((self.n_inputs, self.n_states, 1))
+
+        # Build and store control matrices for scheduling
+        # --------------------------------------------------
+        self.build_scheduled_control(vehicle.linear_model, input_vector=[self.ss_input])
+
+    def __call__(self, t, x=None):
+
+        if x is None:
+            x_k = np.zeros((self.n_states, 1))
+        else:
+            x_k = np.array([x.matrix[ii][0] for ii in range(len(x.matrix)-2)])
+
+        # Identify reference state (desired state) at t
+        t_k = np.round(t + self.dt/2.0, 1)  # current time step
+        time_ind = np.argmin(np.abs(t_k - self.ref_traj['t'].tolist()))  # get index of time value in ref_traj closest to t_k
+        x_ref_k = []
+        for state in self.states:
+            if state != 'mission_complete':
+                x_ref_k.append(self.ref_traj[state][time_ind])
+        x_ref_k = np.asarray(x_ref_k[:-1])  # get rid of time index in state vector
+        x_k = x_k.reshape(x_k.shape[0],)
+
+        error         = x_k - x_ref_k    # Error between current and reference state
+        scheduled_var = x_k[self.parameters['index_scheduled_var']]    # get psi from current state vector (self.parameters = 'psi')
+        k_idx         = np.argmin(np.abs(self.scheduled_states[self.parameters['index_scheduled_var'], :] - scheduled_var))  # find the psi value stored in the controller closest to the current psi --> extract index
+        K             = self.control_gains[:, :, k_idx]     # extract gain corresponding to the current psi value
+        u             = self.compute_input(K, error)                 # compute input u given the gain matrix K and the error between current and reference state
+        u[0]         += self.ss_input
+        u[0]  = min(max([0, u[0]]), self.vehicle_max_thrust)
+        result = {'T': u[0], 'mx': u[1], 'my': u[2], 'mz': u[3], 'mission_complete': t_k/self.ref_traj['t'][-1]}
+        return {
+            'T': u[0],
+            'mx': u[1],
+            'my': u[2],
+            'mz': u[3],
+            'mission_complete': t_k/self.ref_traj['t'][-1]}
+
+    def compute_gain(self, A, B):
+        """ Compute controller gain given state of the system described by linear model A, B"""
+        self.K, self.E = lqr_calc_k(A, B, self.parameters['Q'], self.parameters['R'])
+        return self.K, self.E
+
+    def compute_input(self, gain, state_error):
+        """ Compute system input given the controller gain 'gain' and the error w.r.t. the reference state 'error' """
+        return - np.dot(gain, state_error)
+    
+    def build_scheduled_control(self, system_linear_model_fun, input_vector, state_vector_vals=None, index_scheduled_var=None):
+
+        if state_vector_vals is None:
+            # using psi (yaw angle) as scheduled variable as the LQR control cannot work with yaw=0 since it's in the inertial frame.
+            n_schedule_grid = 360*2 + 1
+            index_scheduled_var = 5
+            state_vector_vals = np.zeros((self.n_states, n_schedule_grid))
+            state_vector_vals[index_scheduled_var, :] = np.linspace(-2.0*np.pi, 2.0*np.pi, n_schedule_grid)
+
+        n, m = state_vector_vals.shape
+        assert n == self.n_states, "number of states set at initialization and size of state_vector_vals mismatch."
+        self.control_gains = np.zeros((self.control_gains.shape[0], self.control_gains.shape[1], m))
+        self.scheduled_states = state_vector_vals
+
+        for j in range(m):
+            phi, theta, psi = state_vector_vals[3:6, j]
+            p,       q,   r = state_vector_vals[-3:, j]
+            Aj,          Bj = system_linear_model_fun(phi, theta, psi, p, q, r, input_vector[0])
+            self.control_gains[:, :, j], _ = self.compute_gain(Aj, Bj)
+
+        print('Control gain matrices complete.')
+
+
+class LQR_I(LQR):
+    """
+    Linear Quadratic Regulator with Integral Effect
+    
+    args:
+        x_ref (dict[str, np.ndarray]):
+            dictionary of reference trajectories for each state variable (x, y, z, phi, theta, psi, ...)
+        vehicle:
+            UAV model object
+
+    keyword args: 
+        Q (np.ndarray):
+            State error penalty matrix, size num_states x num_states (where num_states only includes dynamic states)
+            Represents how 'bad' an error is in the state vector w.r.t. the reference state vector
+        R (np.ndarray):
+            Input penalty matrix, size num_inputs x num_inputs (where num_inputs only includes inputs that inform the system dynamics)
+            Represents how 'hard' it is to produce the desired input (thrust and three moments along three axes)
+        int_lag (int):
+            Length of the time window, in number of simulation steps, to integrate the state position error. The time window is defined 
+            as the last int_lag discrete steps up until the current discrete time step. The integral of the state position error adds to 
+            the overall state error to compute the gain matrix, and helps compensate for constant offsets between the reference (desired) 
+            position and the actual position of the vehicle.
+        scheduled_var (str):
+            Variable used to create the scheduled controller gains; must correspond to a state key
+        index_scheduled_var (int):
+            Index corresponding to the scheduled_var in the state vector
+    """
+
+    def __init__(self, x_ref, vehicle, **kwargs):
+        # Check correct arguments:
+        if not isinstance(x_ref, dict):
+            raise TypeError("Reference trajectory must be a dictionary of numpy arrays for each state throughout time.")
+        for vals in x_ref.values():
+            if not isinstance(vals, np.ndarray):
+                raise TypeError("Reference trajectory must be a dictionary of numpy arrays for each state throughout time.")
+
+        self.states    = vehicle.states         # state variables of the system to be controlled (x, y, z, phi, theta, psi)
+        self.n_states  = len(self.states) - 2   # number of states (minus two to remove time and mission_complete)
+        self.inputs    = vehicle.inputs         # input variables of the system to be controlled
+        self.n_inputs  = len(self.inputs) - 1   # number of inputs (minus one to remove mission_complete)
+        self.ref_traj  = x_ref                  # reference state to follow during simulation (x_ref, y_ref, z_ref, phi_ref, theta_ref, psi_ref, ...)
+        self.ss_input  = vehicle.parameters['steadystate_input']
+        self.vehicle_max_thrust = vehicle.dynamics['max_thrust']
+
+        self.outputs   = vehicle.outputs[:3]    # output variables of the system to be controlled (x, y, z only)
+        self.n_outputs = 3                      # number of outputs
+        self.err_hist  = []                     # error history (integral)
+
+        # Default control parameters
+        # --------------------------------
+        self.parameters = dict(Q = np.diag([1.e3, 1.e3, 1.e6,  # x, y, z
+                                            5.e5, 5.e5, 15.e3,  # phi, theta, psi,
+                                            2.e2, 2.e2, 1.e2,  # vx, vy, vz,
+                                            5.e4, 5.e4, 1.e4]),  # p, q, r
+                               R = np.diag([200, 4e3, 4e3, 4e3]), # T, Mx, My, Mz
+                               qi = np.array([100, 100, 500]),
+                               int_lag = 200,
+                               scheduled_var='psi',     # variable used to create the scheduled controller gains (only psi allowed for now)
+                               index_scheduled_var=5)   # index corresponding to the scheduled_var (psi) in the state vector x; i.e., x[5] = psi
+        self.parameters.update(kwargs)                  # update control parameters according to user
+
+        # Get scheduled variable index 
+        self.parameters['index_scheduled_var'] = self.states.index(self.parameters['scheduled_var'])
+
+        # Initialize other controller-related variables
+        # ---------------------------------------------
+        self.dt = vehicle.parameters['dt']
+        
+        # Initialize other controller-related variables
+        # ---------------------------------------------
+        self.C = np.zeros((self.n_outputs, self.n_states))
+        self.C[:self.n_outputs, :self.n_outputs] = np.eye(self.n_outputs)
+
+        # Initialize Augmented state space matrices for integral action
+        self.Ai                       = np.zeros((self.n_states+self.n_outputs, self.n_states+self.n_outputs))
+        self.Bi                       = np.zeros((self.n_states+self.n_outputs, self.n_inputs))
+        self.Ai[self.n_states:, :self.n_states] = self.C
+
+        # Generate augmented Q and R for integral term
+        self.parameters['Qi'] = np.diag(np.concatenate((np.diag(self.parameters['Q']), self.parameters['qi']), axis=0))
+        self.parameters['Ri'] = self.parameters['R']
+
+        # Control gain storage matrix:
+        # -----------------------------
+        self.control_gains = np.zeros((self.n_inputs, self.n_states + self.n_outputs, 1))
+
+        # Build and store control matrices for scheduling
+        # --------------------------------------------------
+        self.build_scheduled_control(vehicle.linear_model, input_vector=[self.ss_input])
+
+    def compute_gain(self, A, B):
+        """ Compute controller gain given state of the system described by linear model A, B"""
+        self.Ai[:self.n_states, :self.n_states] = A
+        self.Bi[:self.n_states,              :] = B
+        self.K, self.E = lqr_calc_k(self.Ai, self.Bi, self.parameters['Qi'], self.parameters['Ri'])
+        return self.K, self.E
+
+    def compute_input(self, gain, error):
+        """ Compute system input given the controller gain 'gain' and the error w.r.t. the reference state 'error' """
+        self.err_hist.append(np.dot(self.C, error))
+        err_hist     = np.asarray(self.err_hist).T
+        err_integral = np.sum(err_hist[:, max([0, len(self.err_hist) - self.parameters['int_lag']]):], axis=1) * self.dt
+        return - np.dot(gain[:, :self.n_states], error) - np.dot(gain[:, self.n_states:], err_integral)
+
+    def reset_controller(self):
+        """ Reset Error history (from integral action, if exist) """
+        if hasattr(self, 'err_hist'):
+            self.err_hist = []    
+        print("Controller reset complete")
diff --git a/src/prog_models/models/aircraft_model/__init__.py b/src/prog_models/models/aircraft_model/__init__.py
new file mode 100644
index 000000000..545d9d9a7
--- /dev/null
+++ b/src/prog_models/models/aircraft_model/__init__.py
@@ -0,0 +1,5 @@
+# Copyright © 2021 United States Government as represented by the Administrator of the
+# National Aeronautics and Space Administration.  All Rights Reserved.
+
+from .aircraft import AircraftModel
+from .small_rotorcraft import SmallRotorcraft
diff --git a/src/prog_models/models/aircraft_model/aircraft.py b/src/prog_models/models/aircraft_model/aircraft.py
new file mode 100644
index 000000000..fd46c852f
--- /dev/null
+++ b/src/prog_models/models/aircraft_model/aircraft.py
@@ -0,0 +1,11 @@
+# Copyright © 2021 United States Government as represented by the Administrator of the
+# National Aeronautics and Space Administration.  All Rights Reserved.
+
+from prog_models import PrognosticsModel
+
+
+class AircraftModel(PrognosticsModel):
+    """
+    Base class for aircraft models
+    """
+    pass
diff --git a/src/prog_models/models/aircraft_model/small_rotorcraft.py b/src/prog_models/models/aircraft_model/small_rotorcraft.py
new file mode 100644
index 000000000..d89e34fd7
--- /dev/null
+++ b/src/prog_models/models/aircraft_model/small_rotorcraft.py
@@ -0,0 +1,379 @@
+# Copyright © 2021 United States Government as represented by the Administrator of the
+# National Aeronautics and Space Administration.  All Rights Reserved.
+
+import numpy as np
+from warnings import warn
+
+from prog_models.models.aircraft_model import AircraftModel
+from prog_models.models.aircraft_model.vehicles.aero import aerodynamics as aero
+from prog_models.models.aircraft_model.vehicles import vehicles
+from prog_models.utils.traj_gen import geometry as geom
+
+
+class SmallRotorcraft(AircraftModel):
+    r"""
+    Vectorized prognostics :term:`model` to generate a predicted trajectory for a small rotorcraft using a n=6 degrees-of-freedom dynamic model
+    with feedback control loop. The model follows the form:
+    
+    .. math::
+        u     = h(x, x_{ref})
+        
+        dx/dt = f(x, \theta, u)
+    
+    where:
+      x is a 2n state vector containing position, attitude and corresponding derivatives
+      :math:`\theta` is a vector of model parameters including rotorcraft mass, inertia moment, aerodynamic coefficients, etc.
+      u is the input vector: thrust along the body vertical axis, and three moments along the UAV body axis to follow the desired trajectory.
+      :math:`x_{ref}` is the desired state vector at that specific time step, with dimension 2n
+      f(.) is growth rate function of all vehicle state
+      h(.) is the feedback-loop control function that returns the necessary thrust and moments (u vector) to cover the error between desired state :math:`x_{ref}` and current state x
+      dx/dt is the state-increment per unit time.
+
+    Model generates cartesian positions and velocities, pitch, roll, and yaw, and angular velocities throughout time to satisfy some user-define waypoints.
+
+    See [0]_ for modeling details.
+
+    :term:`Events<event>`: (1)
+        TrajectoryComplete: The final time of the reference trajectory has been reached
+    
+    :term:`Inputs/Loading<input>`: (0)
+        | T: thrust
+        | mx: moment in x
+        | my: moment in y
+        | mz: moment in z
+        | mission_complete: progression throughout time to final time point in reference trajectory, where 0 is no progress and 1 is mission completed
+
+    :term:`States<state>`: (14)
+        | x: first position in cartesian reference frame East-North-Up (ENU), i.e., East in fixed inertia frame, center is at first waypoint
+        | y: second position in cartesian reference frame East-North-Up (ENU), i.e., North in fixed inertia frame, center is at first waypoint
+        | z: third position in cartesian reference frame East-North-Up (ENU), i.e., Up in fixed inertia frame, center is at first waypoint
+        | phi: Euler's first attitude angle
+        | theta: Euler's second attitude angle
+        | psi: Euler's third attitude angle
+        | vx: velocity along x-axis, i.e., velocity along East in fixed inertia frame
+        | vy: velocity along y-axis, i.e., velocity along North in fixed inertia frame
+        | vz: velocity along z-axis, i.e., velocity Up in fixed inertia frame
+        | p: angular velocity around UAV body x-axis
+        | q: angular velocity around UAV body y-axis
+        | r: angular velocity around UAV body z-axis
+        | t: time
+        | mission_complete: progression throughout time to final time point in reference trajectory, where 0 is no progress and 1 is mission completed
+
+    :term:`Outputs<output>`: (12)
+        | x: first position in cartesian reference frame East-North-Up (ENU), i.e., East in fixed inertia frame, center is at first waypoint
+        | y: second position in cartesian reference frame East-North-Up (ENU), i.e., North in fixed inertia frame, center is at first waypoint 
+        | z: third position in cartesian reference frame East-North-Up (ENU), i.e., Up in fixed inertia frame, center is at first waypoint
+        | phi: Euler's first attitude angle
+        | theta: Euler's second attitude angle
+        | psi: Euler's third attitude angle
+        | vx: velocity along x-axis, i.e., velocity along East in fixed inertia frame
+        | vy: velocity along y-axis, i.e., velocity along North in fixed inertia frame
+        | vz: velocity along z-axis, i.e., velocity Up in fixed inertia frame
+        | p: angular velocity around UAV body x-axis
+        | q: angular velocity around UAV body y-axis
+        | r: angular velocity around UAV body z-axis
+
+    Keyword Args
+    ------------
+        process_noise : Optional, float or dict[str, float]
+          :term:`Process noise<process noise>` (applied at dx/next_state).
+          Can be number (e.g., .2) applied to every state, a dictionary of values for each
+          state (e.g., {'x1': 0.2, 'x2': 0.3}), or a function (x) -> x
+        process_noise_dist : Optional, str
+          distribution for :term:`process noise` (e.g., normal, uniform, triangular)
+        measurement_noise : Optional, float or dict[str, float]
+          :term:`Measurement noise<measurement noise>` (applied in output eqn).
+          Can be number (e.g., .2) applied to every output, a dictionary of values for each
+          output (e.g., {'z1': 0.2, 'z2': 0.3}), or a function (z) -> z
+        measurement_noise_dist : Optional, str
+          distribution for :term:`measurement noise` (e.g., normal, uniform, triangular)
+        dt : Optional, float
+          Time step in seconds for trajectory generation
+        gravity : Optional, float
+          m/s^2, gravity magnitude
+        air_density : Optional, float
+          kg/m^3, atmospheric density
+        steadystate_input : Optional, float
+          Input vector to maintain the vehicle in a stable position that is used to build the linearized model for the controller.
+        x0 : dict[str, float]
+          Initial :term:`state`
+        vehicle_model: Optional, str
+          String to specify vehicle type. 'tarot18' and 'djis1000' are supported
+        vehicle_payload: Optional, float
+          kg, payload mass
+        vehicle_max_speed : Optional, float
+          m/s, maximum vehicle speed
+        vehicle_max_roll : Optional, float
+          rad, maximum roll angle
+        vehicle_max_pitch : Optional, float
+          rad, maximum pitch angle
+
+    References
+    ----------
+    [0] M. Corbetta et al., "Real-time UAV trajectory prediction for safely monitoring in low-altitude airspace," AIAA Aviation 2019 Forum,  2019. https://arc.aiaa.org/doi/pdf/10.2514/6.2019-3514
+    """
+
+    events = ['TrajectoryComplete']
+    inputs = ['T', 'mx', 'my', 'mz', 'mission_complete']
+    states = ['x', 'y', 'z', 'phi', 'theta', 'psi', 'vx', 'vy', 'vz', 'p', 'q', 'r', 't', 'mission_complete']
+    outputs = ['x', 'y', 'z', 'phi', 'theta', 'psi', 'vx', 'vy', 'vz', 'p', 'q', 'r']
+    
+    is_vectorized = True
+
+    default_parameters = {  # Set to defaults
+        # Simulation parameters:
+        'dt': 0.1,
+        'gravity': 9.81,
+        'air_density': 1.225,
+        'steadystate_input': None,
+        
+        # State initialization:
+        'x0': {key: 0.0 for key in states},
+
+        # Vehicle parameters:
+        'vehicle_model': 'tarot18',
+        'vehicle_payload': 0.0,
+        'vehicle_max_speed': 15.0,
+        'vehicle_max_roll': 0.7853981633974483,
+        'vehicle_max_pitch': 0.7853981633974483
+    }
+
+    def __init__(self, **kwargs):
+        super().__init__(**kwargs)
+
+        # Select model
+        # ------------
+        if not isinstance(self.parameters['vehicle_model'], str):
+            raise TypeError("Vehicle model must be defined as a string.")
+        if self.parameters['vehicle_model'].lower() == 'djis1000':
+            self.mass, self.geom, self.dynamics = vehicles.DJIS1000(self.parameters['vehicle_payload'], self.parameters['gravity'])
+        elif self.parameters['vehicle_model'].lower() == 'tarot18':   
+            self.mass, self.geom, self.dynamics = vehicles.TAROT18(self.parameters['vehicle_payload'], self.parameters['gravity'])
+        else:                                                         
+            raise ValueError("Specified vehicle type is not supported. Only 'tarot18' and 'djis1000' are currently supported.")
+        
+        # Steady-state input value: hover, [weight, 0, 0, 0]
+        if self.parameters['steadystate_input'] is None:
+            self.parameters['steadystate_input'] = self.mass['total'] * self.parameters['gravity']
+        
+        # Introduction of Aerodynamic effects:
+        self.aero = dict(drag=aero.DragModel(bodyarea=self.dynamics['aero']['ad'],
+                                                      Cd=self.dynamics['aero']['cd'],
+                                                      air_density=self.parameters['air_density']),
+                        lift=None)
+
+    def dx(self, x: dict, u: dict):
+        # Extract useful values
+        # ---------------------
+        m = self.mass['total']  # vehicle mass
+        Ixx, Iyy, Izz = self.mass['Ixx'], self.mass['Iyy'], self.mass['Izz']  # vehicle inertia
+        
+        # Input vector
+        T = u['T']   # Thrust (along body z)
+        tp = u['mx']  # Moment along body x
+        tq = u['my']  # Moment along body y
+        tr = u['mz']  # Moment along body z
+
+        # Extract state variables from current state vector
+        # -------------------------------------------------
+        phi = x['phi']
+        theta = x['theta']
+        psi = x['psi']
+        vx_a = x['vx']
+        vy_a = x['vy']
+        vz_a = x['vz']
+        p = x['p']
+        q = x['q']
+        r = x['r']
+
+        # Pre-compute Trigonometric values
+        # --------------------------------
+        sin_phi = np.sin(phi)
+        cos_phi = np.cos(phi)
+        sin_theta = np.sin(theta)
+        cos_theta = np.cos(theta)
+        tan_theta = np.tan(theta)
+        sin_psi = np.sin(psi)
+        cos_psi = np.cos(psi)
+        
+        # Compute drag forces
+        # -------------------
+        v_earth = np.array([vx_a, vy_a, vz_a]).reshape((-1,))  # velocity in Earth-fixed frame
+        v_body = np.dot(geom.rot_eart2body_fast(sin_phi, cos_phi, sin_theta, cos_theta, sin_psi, cos_psi), v_earth)  # Velocity in body-axis
+        fb_drag = self.aero['drag'](v_body)   # drag force in body axis
+        fe_drag = np.dot(geom.rot_body2earth_fast(sin_phi, cos_phi, sin_theta, cos_theta, sin_psi, cos_psi), fb_drag)  # drag forces in Earth-fixed frame
+        fe_drag[-1] = np.sign(v_earth[-1]) * np.abs(fe_drag[-1])  # adjust vertical (z=Up) force according to velocity in fixed frame
+
+        # Update state vector
+        # -------------------
+        dxdt = np.zeros((len(x),))
+        
+        dxdt[0] = vx_a    # x-position increment (airspeed along x-direction)
+        dxdt[1] = vy_a    # y-position increment (airspeed along y-direction)
+        dxdt[2] = vz_a    # z-position increment (airspeed along z-direction)
+        
+        dxdt[3] = p + q * sin_phi * tan_theta + r * cos_phi * tan_theta        # Euler's angle phi increment
+        dxdt[4] = q * cos_phi - r * sin_phi                                    # Euler's angle theta increment
+        dxdt[5] = q * sin_phi / cos_theta + r * cos_phi / cos_theta            # Euler's angle psi increment
+        
+        dxdt[6] = ((sin_theta * cos_psi * cos_phi + sin_phi * sin_psi) * T - fe_drag[0]) / m   # Acceleration along x-axis
+        dxdt[7] = ((sin_theta * sin_psi * cos_phi - sin_phi * cos_psi) * T - fe_drag[1]) / m   # Acceleration along y-axis
+        dxdt[8] = - self.parameters['gravity'] + (cos_phi * cos_theta  * T - fe_drag[2]) / m   # Acceleration along z-axis
+
+        dxdt[9] = ((Iyy - Izz) * q * r + tp * self.geom['arm_length']) / Ixx     # Angular acceleration along body x-axis: roll rate
+        dxdt[10] = ((Izz - Ixx) * p * r + tq * self.geom['arm_length']) / Iyy     # Angular acceleration along body y-axis: pitch rate
+        dxdt[11] = ((Ixx - Iyy) * p * q + tr *        1               ) / Izz     # Angular acceleration along body z-axis: yaw rate
+        dxdt[12] = 1                                                              # Auxiliary time variable
+        dxdt[13] = (u['mission_complete'] - x['mission_complete'])/self.parameters['dt']    # Value to keep track of percentage of mission completed
+        
+        return self.StateContainer(np.array([np.atleast_1d(item) for item in dxdt]))
+    
+    def event_state(self, x: dict) -> dict:
+        # Based on percentage of reference trajectory completed 
+        return {
+                'TrajectoryComplete': x['mission_complete']
+        }
+ 
+    def output(self, x: dict):
+        # Output is the same as the state vector, without time and mission_complete
+        return self.OutputContainer(x.matrix[0:-2])
+
+    def threshold_met(self, x: dict) -> dict:
+        # Progress through the reference trajectory is saved in the state 'mission_complete'
+        return {
+            'TrajectoryComplete': x['mission_complete'] >= 1}
+
+    def simulate_to_threshold(self, future_loading_eqn, first_output=None, threshold_keys=None, **kwargs):
+        # Check for appropriately defined dt - must be same as vehicle model
+        if 'dt' in kwargs and kwargs['dt'] != self.parameters['dt']:
+          kwargs['dt'] = self.parameters['dt']
+          warn(f"Simulation dt must be equal to dt defined for the vehicle model. dt = {self.parameters['dt']} is used.")
+        elif 'dt' not in kwargs:
+          kwargs['dt'] = self.parameters['dt']
+
+        # Simulate to threshold
+        sim_res = super().simulate_to_threshold(future_loading_eqn, first_output, threshold_keys, **kwargs)
+
+        return sim_res
+
+    def linear_model(self, phi, theta, psi, p, q, r, T):
+        """ 
+        Linearized model of the small rotorcraft 6-dof.
+        The function returns the state-transition matrix A and the input matrix B that forms the linearized model as:
+
+        dx/dt = A x + B u
+
+        where x is the state vector in state-space form (namely x, \dot{x}), u is the input vector containing the thrust and three moments around the vehicle's main body axes.
+        
+        To generate the linearized matrices, only the attitude angles, angular velocities, and thrust are necessary,
+        since the model model ignores gyroscopic effect and wind rate of change. 
+        
+        :param phi:       rad, scalar, dobule, first Euler's angle
+        :param theta:     rad, scalar, dobule, second Euler's angle
+        :param psi:       rad, scalar, dobule, third Euler's angle
+        :param p:         rad/s, scalar, double, roll rate of change
+        :param q:         rad/s, scalar, double, pitch rate of change
+        :param r:         rad/s, scalar, double, yaw rate of change
+        :param T:         N, scalar, double, thrust
+        :return:          Linearized state transition matrix A, n_states x n_states, and linearized input matrix B, n_states x n_inputs
+        """
+        m         = self.mass['total']
+        Ixx       = self.mass['Ixx']
+        Iyy       = self.mass['Iyy']
+        Izz       = self.mass['Izz']
+        l         = self.geom['arm_length'] 
+        sin_phi   = np.sin(phi)
+        cos_phi   = np.cos(phi)
+        sin_theta = np.sin(theta)
+        cos_theta = np.cos(theta)
+        tan_theta = np.tan(theta)
+        sin_psi   = np.sin(psi)
+        cos_psi   = np.cos(psi)
+
+        A = np.array([[0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0], 
+                      [0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0], 
+                      [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0], 
+                      [0, 0, 0, q*cos_phi*tan_theta - r*sin_phi*tan_theta, q*(tan_theta**2 + 1)*sin_phi + r*(tan_theta**2 + 1)*cos_phi, 0, 0, 0, 0, 1, sin_phi*tan_theta, cos_phi*tan_theta], 
+                      [0, 0, 0, -q*sin_phi - r*cos_phi, 0, 0, 0, 0, 0, 0, cos_phi, -sin_phi], 
+                      [0, 0, 0, q*cos_phi/cos_theta - r*sin_phi/cos_theta, q*sin_phi*sin_theta/cos_theta**2 + r*sin_theta*cos_phi/cos_theta**2, 0, 0, 0, 0, 0, sin_phi/cos_theta, cos_phi/cos_theta], 
+                      [0, 0, 0, T*(-sin_phi*sin_theta*cos_psi + sin_psi*cos_phi)/m, T*cos_phi*cos_psi*cos_theta/m, T*(sin_phi*cos_psi - sin_psi*sin_theta*cos_phi)/m, 0, 0, 0, 0, 0, 0], 
+                      [0, 0, 0, T*(-sin_phi*sin_psi*sin_theta - cos_phi*cos_psi)/m, T*sin_psi*cos_phi*cos_theta/m, T*(sin_phi*sin_psi + sin_theta*cos_phi*cos_psi)/m, 0, 0, 0, 0, 0, 0], 
+                      [0, 0, 0, -T*sin_phi*cos_theta/m, -T*sin_theta*cos_phi/m, 0, 0, 0, 0, 0, 0, 0], 
+                      [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, r*(Iyy - Izz)/Ixx, q*(Iyy - Izz)/Ixx], 
+                      [0, 0, 0, 0, 0, 0, 0, 0, 0, r*(-Ixx + Izz)/Iyy, 0, p*(-Ixx + Izz)/Iyy], 
+                      [0, 0, 0, 0, 0, 0, 0, 0, 0, q*(Ixx - Iyy)/Izz, p*(Ixx - Iyy)/Izz, 0]])
+
+        B = np.array([ [0, 0, 0, 0], 
+                       [0, 0, 0, 0], 
+                       [0, 0, 0, 0], 
+                       [0, 0, 0, 0], 
+                       [0, 0, 0, 0], 
+                       [0, 0, 0, 0], 
+                       [(sin_phi*sin_psi + sin_theta*cos_phi*cos_psi)/m, 0, 0, 0], 
+                       [(-sin_phi*cos_psi + sin_psi*sin_theta*cos_phi)/m, 0, 0, 0], 
+                       [cos_phi*cos_theta/m, 0, 0, 0], 
+                       [0, l/Ixx, 0, 0], 
+                       [0, 0, l/Iyy, 0], 
+                       [0, 0, 0, 1.0/Izz]])
+
+        return A, B
+
+    def visualize_traj(self, pred, ref):
+        """
+        This method provides functionality to visualize a predicted trajectory generated, plotted with the reference trajectory. 
+
+        Calling this returns a figure with two subplots: 1) x vs y, and 2) z vs time.
+
+        Parameters
+        ----------
+        pred : Vehicle model simulation 
+               SimulationResults from simulate_to or simulate_to_threshold for a defined SmallRotorcraft class
+
+        ref  : Reference trajectory 
+                dict with keys for each state in the vehicle model and corresponding values as numpy arrays 
+
+        Returns 
+        -------
+        fig : Visualization of trajectory generation results 
+        """
+
+        import matplotlib.pyplot as plt
+
+        # Extract reference trajectory information
+        # ----------------------------------------
+        time        = ref['t'].tolist() 
+        ref_x       = ref['x'].tolist() 
+        ref_y       = ref['y'].tolist() 
+        ref_z       = ref['z'].tolist()
+
+        # Extract predicted trajectory information
+        # ----------------------------------------
+        pred_time = pred.times
+        pred_x = [pred.outputs[iter]['x'] for iter in range(len(pred_time))]
+        pred_y = [pred.outputs[iter]['y'] for iter in range(len(pred_time))]
+        pred_z = [pred.outputs[iter]['z'] for iter in range(len(pred_time))]
+
+        # Initialize Figure
+        # ----------------
+        params = dict(figsize=(13, 9), fontsize=14, linewidth=2.0, alpha_preds=0.6)
+        fig, (ax1, ax2) = plt.subplots(2)
+
+        # Plot trajectory predictions
+        # -------------------------
+        ax1.plot(ref_x, ref_y, '--', linewidth=params['linewidth'], color='tab:orange', alpha=0.5, label='reference trajectory')
+        ax1.plot(pred_x, pred_y,'-', color='tab:blue', alpha=params['alpha_preds'], linewidth=params['linewidth'], label='prediction')
+
+        ax1.set_xlabel('x', fontsize=params['fontsize'])
+        ax1.set_ylabel('y', fontsize=params['fontsize'])
+        ax1.legend(fontsize=params['fontsize'])
+
+        # Add altitude plot
+        # -------------------------------------------------------
+        ax2.plot(time, ref_z, '-', color='tab:orange', alpha=params['alpha_preds'], linewidth=params['linewidth'], label='reference trajectory')
+        ax2.plot(pred_time, pred_z,'-', color='tab:blue',alpha=params['alpha_preds'], linewidth=params['linewidth'], label='prediction')
+        
+        ax2.set_xlabel('time stamp, -', fontsize=params['fontsize'])
+        ax2.set_ylabel('z', fontsize=params['fontsize'])
+
+        return fig
+        
\ No newline at end of file
diff --git a/src/prog_models/models/aircraft_model/vehicles/aero/aerodynamics.py b/src/prog_models/models/aircraft_model/vehicles/aero/aerodynamics.py
new file mode 100644
index 000000000..82e022fdd
--- /dev/null
+++ b/src/prog_models/models/aircraft_model/vehicles/aero/aerodynamics.py
@@ -0,0 +1,43 @@
+# Copyright © 2021 United States Government as represented by the Administrator of the
+# National Aeronautics and Space Administration.  All Rights Reserved.
+
+"""
+Functions for aerodynamics effects
+"""
+
+import numpy as np
+
+# Aerodynamic forces
+# ====================
+class DragModel():
+    """
+    Bluff body drag model of the form:
+
+            F_D = 1/2 * rho * A * Cd * V^2
+
+    where rho is the air density, typically 1.225 kg/m^3, A is the apparent face of the body, depending on geometry.
+    Cd is the drag coefficient, also dependent on geometry.
+    V is the body or fluid speed, and F_D is the drag force opposing motion wrt the direction of V.
+    """
+    def __init__(self, bodyarea=None, Cd=None, air_density=None):
+        """
+        DragModel initialization function.
+
+        :param bodyarea:            m^2, scalar, apparent face of the body
+        :param Cd:                  -, scalar, drag coefficient
+        :param air_density:         kg/m^3, scalar, air density
+        """
+        self.area = bodyarea
+        self.cd = Cd
+        self.rho = air_density
+
+    def __call__(self, air_v):
+        """
+        DragModel call
+
+        :param air_v:       m/s^2, scalar or n x 1 array or 1D vector, airspeed
+        :return:            N, drag force, opposing motion.
+        """
+        vsq = air_v * np.abs(air_v)
+        return 0.5 * self.rho * self.area * self.cd * vsq
+ 
\ No newline at end of file
diff --git a/src/prog_models/models/aircraft_model/vehicles/control/allocation_functions.py b/src/prog_models/models/aircraft_model/vehicles/control/allocation_functions.py
new file mode 100644
index 000000000..35040fa29
--- /dev/null
+++ b/src/prog_models/models/aircraft_model/vehicles/control/allocation_functions.py
@@ -0,0 +1,58 @@
+# Copyright © 2021 United States Government as represented by the Administrator of the
+# National Aeronautics and Space Administration.  All Rights Reserved.
+
+# IMPORTS
+# ========
+import numpy as np
+
+# FUNCTIONS
+# ========
+def rotorcraft_cam(n, l, b, d, constrained=False):
+    """
+    Generate control allocation matrix (CAM) to transform rotor's angular velocity into thrust and torques around three main body axes.
+    
+        [T, Mx, My, Mz]^{\top} = \Gamma [\Omega_1^2, \Omega_2^2, ..., \Omega_n^2]^{\top}
+    
+    Where:
+        [T, Mx, My, Mz]^{\top} is the (4 x 1) column vector containing thrust (T) and moments along main body axis (Mx, My, Mz)
+        \Gamma is the (4 x n) CAM
+        [\Omega_1^2, \Omega_2^2, ..., \Omega_n^2]^{\top} is the (n x 1) column vector of the rotor angular speed squared.
+    
+    The control allocation matrix is built under the assumption of symmetric rotor configuration, for generality.
+    Special CAM should be built ad-hoc per UAV model.
+
+    :param n:           number of rotors
+    :param l:           rotor arm's length (from center of mass to rotor center)
+    :param b:           thrust constant, function of rotor type
+    :param d:           torque constant, function of rotor type
+    :return:            control allocation matrix of dimensions (4, n) and its pseudo-inverse of dimensions (n, 4)
+    """
+    
+    Gamma = np.empty((4, n))
+    optional = {}
+    if n == 8 and not constrained:
+        l_b        = l * b
+        l_b_sq2o2 = l_b * np.sqrt(2.0)/2.0
+        # This CAM is assuming there's no rotor pointing towards the drone forward direction (x-axis)
+        # See Dmitry Luchinsky's report for details (TO BE VERIFIED)
+        Gamma = np.array([ [   b,           b,     b,          b,     b,          b,    b,         b],
+                           [  l_b,  l_b_sq2o2,   0.0, -l_b_sq2o2,  -l_b, -l_b_sq2o2,  0.0, l_b_sq2o2], 
+                           [  0.0, -l_b_sq2o2,  -l_b, -l_b_sq2o2,   0.0,  l_b_sq2o2,  l_b, l_b_sq2o2],
+                           [   -d,          d,    -d,          d,    -d,          d,   -d,         d]])
+    if n == 8 and constrained:
+        bl = b * l
+        b2 = 2.0 * b
+        d2 = 2.0 * d
+        blsqrt2 = np.sqrt(2.0) * bl
+
+        Gamma = np.array([ [ b2,       b2,  b2,      b2],
+                           [ bl,      0.0, -bl,     0.0],
+                           [-bl, -blsqrt2,  bl, blsqrt2],
+                           [-d2,       d2, -d2,      d2] ])
+        optional['selector'] = np.array([ [1, 0, 1, 0, 0, 0, 0, 0],
+                                          [0, 1, 0, 1, 0, 0, 0, 0],
+                                          [0, 0, 0, 0, 1, 0, 1, 0],
+                                          [0, 0, 0, 0, 0, 1, 0, 1]]).T
+
+    return Gamma, np.linalg.pinv(Gamma), optional
+   
\ No newline at end of file
diff --git a/src/prog_models/models/aircraft_model/vehicles/vehicles.py b/src/prog_models/models/aircraft_model/vehicles/vehicles.py
new file mode 100644
index 000000000..456a3d523
--- /dev/null
+++ b/src/prog_models/models/aircraft_model/vehicles/vehicles.py
@@ -0,0 +1,232 @@
+# Copyright © 2021 United States Government as represented by the Administrator of the
+# National Aeronautics and Space Administration.  All Rights Reserved.
+
+"""
+Vehicle Models - originally developed by Matteo Corbetta (matteo.corbetta@nasa.gov) for the SWS project 
+"""
+
+# Import functions
+# =================
+import numpy as np
+
+from .control import allocation_functions as caf
+
+# VEHICLE MODELS
+# ==============
+def TAROT18(payload=0.0, gravity=9.81):
+    """
+    Tarot T18 Octocopter mass, geometry and dynamic properties.
+    :param payload:         float, payload for the flight.
+    :param gravity:         float, gravity value according to location
+    :return:                dictionaries containing mass, geometry and dynamic properties of the vehicle
+    """
+    mass = dict(body_empty=3.2,             # kg, total empty mass
+                max_payload=5.0,            # kg, admissible payload
+                max_tow=11.2,               # kg, maximum takeoff weight
+                arm=0.23+0.11+0.0373+0.03,  # kg, arm (comprehensive of motors). 230g motor, 110g ESC, 37.3g propeller. 
+                total=None,                 # kg, total mass (to be calculated using payload, num rotors and weight of all arms)
+                body=None,                  # kg, mass of central body
+                payload=payload,            # kg, payload mass
+                Ixx=None,                   # kg*m^2, inertia moments along local axis
+                Iyy=None,                   # kg*m^2, inertia moments along local axis
+                Izz=None)                   # kg*m^2, inertia moments along local axis
+    
+    geom = dict(num_rotors=8,              #, number of rotors
+                body_type='thickdisk',     # -, should be either sphere, flatdisk, thickdisk.
+                body_radius=((0.301+0.209)/2.0)/2.0, # m, body radius. Internal plate is kind of oval, took average of main axes
+                body_height=0.025*2.0,        # m, height of the body
+                arm_length=1.270/2.0 - 0.1275)       # m, arm length
+
+    dynamics = dict(num_states=12,          # -, number of states in state vector: [x, y, z, u, v, w, phi, theta, psi, p, q, r]
+                    num_inputs=4,           # -, number of inputs: thrust, moments along three axes
+                    num_outputs=3,          # -, number of output measures (position coordinates)
+                    C = None,               # observation matrix (constant)
+                    thrust2weight=(45.0/gravity * geom['num_rotors']) / (mass['body_empty'] + mass['arm']*geom['num_rotors']),  # -, thrust over weight ratio. Tarot 18: 50 N per motor divided by weight
+                    max_speed=15.0,         # m/s, max speed, see: https://www.nasa.gov/offices/amd/nasa_aircraft/small_unmanned_aircraft_systems/larc
+                    max_wind_speed=8.0,     # m/s, max wind speed for safe flight (from DJIS1000 forum)
+                    max_acceleration=None,  # m/s^2, max acceleration to be calculated using thrust2weight ratio
+                    max_thrust=None,        # N, max thrust the power system can deliver
+                    min_thrust=0.0,         # N, min thrust associated with rotors at minimum speed (25% throttle for KDE motors). Default is 0 but assigned after the propulsion system is assigned to AircraftModel.
+                    state_vars=['x', 'y', 'z', 'phi', 'theta', 'psi', 'vx', 'vy', 'vz'],
+                    max_roll=45/180.0*np.pi,      # rad, maximum roll during flight
+                    max_pitch=45/180.0*np.pi,     # rad, maximum pitch during flight
+                    aero = dict(cd=1.0,   # drag coefficient of airframe (including rotors), not reliable
+                                ad=0.25,  # apparent face of the rotorcraft facing air for drag force
+                                ),
+                    kt=5.25e-5,    # Approximation derived from: max omega of motor+rotor couple and max thrust for each rotor
+                    kq=5e-5,       # Rotor torque constant
+                    Gamma=None,     # Thrust allocation matrix (constant, depending on kt, kq, and the UAV rotor configuration)
+                    Gamma_inv=None,     # Inverse of thrust allocation matrix (constant, depending on kt, kq, and the UAV rotor configuration)
+                    )
+    
+    mass = rotorcraft_masses(mass, geom)   # compute total and body mass
+    mass, geom = rotorcraft_inertia(mass, geom)   # Build rotorcraft inertia properties
+    if payload > mass['max_payload']:   raise Warning("Payload for TAROT 18 exceeds its maximum recommended payload.")
+
+    dynamics      = rotorcraft_performance(dynamics, mass, gravity)
+    dynamics['C'] = observation_matrix(dynamics['num_states'], dynamics['num_outputs'])
+    
+    # Set control allocation matrix and its inverse
+    dynamics['Gamma'], \
+        dynamics['Gamma_inv'], _ = caf.rotorcraft_cam(n=geom['num_rotors'],
+                                                      l=geom['arm_length'],
+                                                      b=dynamics['kt'], 
+                                                      d=dynamics['kq'])
+    return mass, geom, dynamics
+
+
+def DJIS1000(payload=0.0, gravity=9.81):
+    """
+    DJI S 1000 / Octocopter
+    :param payload:         float, payload for the flight.
+    :param gravity:         float, gravity value according to location
+    :return:                dictionaries containing mass, geometry and dynamic properties of the vehicle
+    """
+
+    mass = dict(body_empty=1.8,         # kg, total empty mass
+                max_payload=6.6,        # kg, admissible payload    
+                max_tow=11.0,           # kg, max takeoff weight
+                arm=0.325,              # kg, arm mass (comprehensive of motors)
+                total=None,             # kg, total mass (to be calculated using payload, num rotors and weight of all arms)
+                body=None,              # kg, mass of central body
+                payload=payload,        # kg, payload mass
+                Ixx=None,               # kg*m^2, inertia moments along local axis
+                Iyy=None,               # kg*m^2, inertia moments along local axis
+                Izz=None)               # kg*m^2, inertia moments along local axis
+
+    geom = dict(num_rotors=8,           # #, number of rotors
+                body_type='thickdisk',  # -, should be either sphere, flatdisk, thickdisk.
+                body_radius=0.3775/2.0, # m, body radius
+                body_height=0.1,        # m, height of the body
+                arm_length=0.386)       # m, arm length
+    
+    dynamics = dict(num_states=12,      # -, number of states in state vector: [x, y, z, u, v, w, phi, theta, psi, p, q, r]
+                    num_inputs=4,       # -, number of inputs: thrust, moments along three axes
+                    num_outputs=3,      # -, number of output measures (position coordinates)
+                    C = None,           # observation matrix (constant)
+                    thrust2weight=5.0,  # -, thrust over weight ratio
+                    max_speed=16.0,     # m/s, max speed, from DJI website, not recommended
+                    max_wind_speed=8.0, # m/s, max wind speed for safe flight
+                    max_acceleration=None,  # m/s^2, max acceleration to be calculated using thrust2weight ratio
+                    max_thrust=None,    # N, max thrust the power system can deliver
+                    min_thrust=0.0,     # N, min thrust delivered at rotor minimum speed (25% of throttle with KDE motor)
+                    state_vars=['x', 'y', 'z', 'phi', 'theta', 'psi', 'vx', 'vy', 'vz'],
+                    max_roll=45 / 180.0 * np.pi,  # rad, maximum roll during flight
+                    max_pitch=45 / 180.0 * np.pi,  # rad, maximum pitch during flight
+                    aero=dict(cd=1.0,  # drag coefficient of airframe (including rotors), not reliable
+                              ad=0.25, # apparent face of the rotorcraft facing air for drag force
+                              ),
+                    kt = 5.41e-5,       # Approximation derived from: max omega of motor+rotor couple and max thrust for each rotor
+                    kq=5e-5,            # Rotor torque constant
+                    Gamma=None,         # Thrust allocation matrix (constant, depending on kt, kq, and the UAV rotor configuration)
+                    Gamma_inv=None,     # Inverse of thrust allocation matrix (constant, depending on kt, kq, and the UAV rotor configuration)
+                    )
+    
+    mass = rotorcraft_masses(mass, geom)
+    mass, geom = rotorcraft_inertia(mass, geom)
+    if payload > mass['max_payload']:   raise Warning("Payload for DJIS1000 exceeds its maximum recommended payload.")
+
+    dynamics = rotorcraft_performance(dynamics, mass, gravity)
+
+    # Generate observation matrix
+    dynamics['C'] = observation_matrix(dynamics['num_states'], dynamics['num_outputs'])
+    
+    # Set control allocation matrix and its inverse
+    dynamics['Gamma'], \
+        dynamics['Gamma_inv'], _ = caf.rotorcraft_cam(n=geom['num_rotors'],
+                                                      l=geom['arm_length'],
+                                                      b=dynamics['kt'], 
+                                                      d=dynamics['kq'])
+    
+    return mass, geom, dynamics
+
+
+# Vehicle-agnostic functions
+# =========================
+def rotor_angles(n):
+    # n: number of rotors
+    arm_angle = 2.0 * np.pi / n
+    nhalf     = int(n/2)    # half of number of rotors
+    angular_vector = np.zeros((nhalf,))
+    for ri in range(nhalf):
+        angular_vector[ri] = arm_angle/2.0 * (2*ri + 1)
+    return angular_vector
+
+
+def sphere_inertia(mass, radius):
+    Ix = 2.0 * mass * radius**2.0 / 5.0
+    Iz = Ix.copy()
+    return Ix, Iz
+
+
+def flatdisk_inertia(mass, radius):
+    Ix = 1.0/4.0 * mass * radius**2.0
+    Iz = Ix * 2.0
+    return Ix, Iz
+
+
+def thickdisk_inertia(mass, radius, height):
+    Ix = 1.0/4.0 * mass * radius**2.0 + 1.0/12.0 * mass * height**2.0
+    Iz = 1.0/2.0 * mass * radius**2.0
+    return Ix, Iz
+
+
+def rotorcraft_inertia(m, g):
+    n_rotors   = g['num_rotors']
+    m['body']  = m['body_empty'] + n_rotors * m['arm']
+    m['total'] = m['body'] + m['payload']
+
+    # Define rotor positions on the 360 degree circle
+    # ------------------------------------------------
+    angular_vector = rotor_angles(n_rotors)
+    
+    motor_distance_from_xaxis = g['arm_length'] * np.sin(angular_vector)
+    if g['body_type'].lower() == 'sphere':          Ix0, Iz0 = sphere_inertia(m['body'], g['body_radius'])
+    elif g['body_type'].lower() == 'flatdisk':      Ix0, Iz0 = flatdisk_inertia(m['body'], g['body_radius'])
+    elif g['body_type'].lower() == 'thickdisk':     Ix0, Iz0 = thickdisk_inertia(m['body'], g['body_radius'], g['body_height'])
+    else:                                           raise Exception("Body geometry not implemented. Please choose among: sphere, flatdisk, thickdisk.")
+    m['Ixx'] = Ix0 + 2.0 * sum(m['arm'] * motor_distance_from_xaxis**2.0)  # [kg m^2], inertia along x
+    m['Iyy'] = m['Ixx']                                                    # [kg m^2], inertia along y (symmetric uav)
+    m['Izz'] = Iz0 + g['num_rotors'] * (g['arm_length']**2.0 * m['arm'])   # [kg m^2], inertia along z
+    return m, g
+
+
+def rotorcraft_masses(mass_dict, geom_dict):
+    """
+    Compute mass of a small rotorcraft given the mass and geometry dictionary defined by the vehicle
+    :param mass_dict:           dictionary of mass properties of the vehicle
+    :param geom_dict:           dictionary of geometry properties of the vehicle
+    :return:                    updated dictionary of mass properties of the vehicle
+    """
+    mass_dict['body']  = mass_dict['body_empty'] + geom_dict['num_rotors'] * mass_dict['arm']
+    mass_dict['total'] = mass_dict['body'] + mass_dict['payload'] 
+    return mass_dict
+
+
+def rotorcraft_performance(dyn_dict, mass_dict, g):
+    """
+    Update dynamic performance dictionary of vehicle with maximum thrust and acceleration.
+
+    :param dyn_dict:            dictionary of dynamic properties of the vehicle
+    :param mass_dict:           dictionary of mass properties of the vehicle
+    :param g:                   m/s^2, double, scalar, gravity magnitude
+    :return:                    updated dictionary of dynamic properties of the vehicle
+    """
+    dyn_dict['max_thrust']       = dyn_dict['thrust2weight'] * (mass_dict['body'] * g)
+    dyn_dict['max_acceleration'] = dyn_dict['max_thrust'] / mass_dict['total']
+    return dyn_dict
+
+
+def observation_matrix(num_states, num_outputs):
+    """
+    Build observation matrix C of a dynamic system following:
+
+    dx/dt = f_{\theta}(x, u)
+    y = C x
+
+    where x is the state vector, u is the input vector, \theta is the vector of model parameters, and y is the output vector.
+    """
+    c = np.zeros((num_outputs, num_states))
+    for ii in range(num_outputs):   c[ii, ii] = 1.0
+    return c
+  
\ No newline at end of file
diff --git a/src/prog_models/utils/traj_gen/__init__.py b/src/prog_models/utils/traj_gen/__init__.py
new file mode 100644
index 000000000..8c96a5550
--- /dev/null
+++ b/src/prog_models/utils/traj_gen/__init__.py
@@ -0,0 +1,4 @@
+# Copyright © 2021 United States Government as represented by the Administrator of the
+# National Aeronautics and Space Administration.  All Rights Reserved.
+
+from prog_models.utils.traj_gen.trajectory import Trajectory
diff --git a/src/prog_models/utils/traj_gen/geometry.py b/src/prog_models/utils/traj_gen/geometry.py
new file mode 100644
index 000000000..9c961fac4
--- /dev/null
+++ b/src/prog_models/utils/traj_gen/geometry.py
@@ -0,0 +1,480 @@
+# Copyright © 2021 United States Government as represented by the Administrator of the
+# National Aeronautics and Space Administration.  All Rights Reserved.
+
+"""
+Geometric functions
+"""
+
+import numpy as np
+
+# EARTH-RELATED DISTANCE FUNCTIONS
+# ================================
+def greatcircle_distance(lat1, lat2, lon1, lon2, R=6371e3):
+    """
+    Compute distance between two points on a sphere, using a circle passing for those two points given the sphere's radius.
+    Typically used to approximate the distance between two points on Earth, ignoring Earth's ellipsoid shape.
+
+    :param lat1:        scalar, rad, latitude of first point
+    :param lat2:        scalar, rad, latitude of second point
+    :param lon1:        scalar, rad, longitude of first point
+    :param lon2:        scalar, rad, longitude of second point
+    :param R:           scalar, m radius of the sphere, default is Earth's radius: 6371e3 m
+    :return:            scalar, m, distance between the two points.
+    """
+    phi1    = lat1
+    phi2    = lat2
+    dphi    = (lat2 - lat1) 
+    dlambda = (lon2 - lon1)
+
+    a = np.sin(dphi / 2.0) * np.sin(dphi / 2.0) + \
+            np.cos(phi1) * np.cos(phi2) * \
+                np.sin(dlambda / 2.0) * np.sin(dlambda / 2.0)
+    c = 2.0 * np.arctan2(np.sqrt(a), np.sqrt(1.0-a))
+    d = R * c # distance in meters
+    return d
+
+
+def vincenty_distance(p1, p2, tol=1e-12, max_iter=200):
+    """ 
+    Compute distance between two geodetic coordinates p1=(lat1, lon1), p2=(lat2, lon2) using ellipsoid equations from Vincenty, 1975.
+    T. Vincenty, 1975. Direct and inverse solution of geodesics on the ellipsoid with application of nested equations. Survey Review 23(176), 88-93. 
+
+    This algorithm provides a more accurate estimate of the distance between two points on Earth compared to the Great Circle distance, since
+    it accounts for the ellipsoid shape. 
+
+    The algorithm does not converge for two nearly antipodal points, for example: (0.0, 0.0), (0.5, 179.5).
+
+    :param p1:          tuple, list or 1D array, latitude and longitude of first point on Earth
+    :param p2:          tuple, list or 1D array, latitude and longitude of second point on Earth
+    :param tol:         scalar, tolerance to distance precision, default = 1e-12 typically refers to <1cm error
+    :param max_iter:    int, number of maximum iterations before breaking algorithm. Default = 200
+    :return:            distance between p1 and p2, in meters.
+    """
+    # Extract latitude and longitude values
+    # ------------------------------------
+    lat1, lon1 = p1
+    lat2, lon2 = p2
+
+    # If points are identical, return distance=0
+    # ------------------------------------------
+    if lat1 == lat2 and lon1 == lon2:
+        return 0.0
+    
+    # Define ellipsoid constants
+    # -----------------------------
+    a  = 6378137.0          # semi-major Earth axis (radius at Equator), meters, according to WGS84
+    f  = 1/298.257223563    # flat parameter of the ellipsoid, according to WGS84
+    b  = (1.0 - f)*a        # semi-minor axis of the ellipsoid (radius at the poles), meters, according to WGS84 = 6356752.314245 
+    
+    # Define coordinate-dependent values
+    # ------------------------------------
+    U1 = np.arctan((1.0-f)*np.tan(lat1))    # reduced latitude (latitude on auxiliary sphere, lat1)
+    U2 = np.arctan((1.0-f)*np.tan(lat2))    # reduced latitude (latitude on auxiliary sphere, lat2)
+    L  = lon2 - lon1                        # difference over longitude of the two points
+    # Compute trigonometric values for U1, U2
+    sin_U1 = np.sin(U1)
+    cos_U1 = np.cos(U1)
+    sin_U2 = np.sin(U2)
+    cos_U2 = np.cos(U2)
+
+    lam  = L    # initialize longitude difference between p1 and p2 on auxiliary sphere. It should asymptotically converge to 0
+    iter = 0    # initialize iterator
+    while iter < max_iter:
+        # Trigonometry of lambda
+        sin_lam = np.sin(lam)
+        cos_lam = np.cos(lam)
+
+        # Trigonometry of sigma
+        sin_sigma = np.sqrt( (cos_U2 * sin_lam )**2.0 + (cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lam)**2.0 )
+        if sin_sigma == 0.0:        return 0.0  # coincident points
+        cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lam
+        sigma      = np.arctan2(sin_sigma, cos_sigma)
+
+        # Trigonometry of alpha
+        sin_alpha  = cos_U1 * cos_U2 * sin_lam / sin_sigma
+        cos_alpha2 = 1.0 - sin_alpha**2.0
+        # Compute cos(2 \sigma_m)
+        try:                            cos_2sigma_m = cos_sigma - 2.0 * sin_U1 * sin_U2 / cos_alpha2
+        except ZeroDivisionError:       cos_2sigma_m = 0.0
+        
+        # Compute new lambda
+        C       = f/16.0 * cos_alpha2 * ( 4.0 + f * (4.0 - 3.0 * cos_alpha2) )
+        lam_old = lam
+        lam     = L + (1.0 - C) * f * sin_alpha * ( sigma + C * sin_sigma * ( cos_2sigma_m + C * cos_sigma * ( -1.0 + 2.0 * cos_2sigma_m**2.0 ) ) )
+        
+        # Evaluate difference
+        d_lam = abs(lam - lam_old)
+        if d_lam < tol: break
+        iter += 1   # update iterator
+    
+    # Return value
+    # ----------
+    if d_lam > tol or iter == max_iter: # Failure to converge
+        return None 
+    else:   # After lambda converged, compute the following:
+        u2     = cos_alpha2 * (a**2.0 - b**2.0) / b**2.0
+        A      = 1.0 + u2 / 16384.0 * (4096.0 + u2 * (-786.0 + u2 * (320.0 - 175.0*u2)))
+        B      = u2/1024.0 * ( 256.0 + u2 * ( -128.0 + u2 * (74.0 - 47.0*u2) ) )
+        dsigma = B * sin_sigma * ( cos_2sigma_m + 1.0/4.0 * B * ( cos_sigma * ( -1.0 + 2.0 * cos_2sigma_m**2.0) - B / 6.0 * cos_2sigma_m * (-3.0 + 4.0 * sin_sigma**2.0) * (-3.0 + 4.0 * cos_2sigma_m**2.0) ) )
+        s      = b * A * (sigma - dsigma)
+    return np.round(s, 6)
+
+
+def geodetic_distance(lats, lons, alts, method='greatcircle', return_surf_vert=False):
+    """
+    Compute geodetic distance between two points defined by latitude, longitude, and altitude
+            dist                = geodetic_distance(lats, lons, alts, method='greatcircle', return_surf_vert=False)
+            surf_dist, ver_dist = geodetic_distance(lats, lons, alts, method='greatcircle', return_surf_vert=True)
+
+    Distance in 3D using: add altitude as cartesian coordinate
+    dist = sqrt(surface_distance**2 + (alt_1 - alt_2)**2)
+
+    :param lats:                    tuple, list or 1D array, latitudes of the two points (radians)
+    :param lons:                    tuple, list or 1D array, longitudes of the two points (radians)
+    :param alts:                    tuple, list or 1D array, altitudes of the two points (meters)
+    :param method:                  string, measurement method to compute distance. Options: 'greatcircle' and 'vincenty'
+    :param return_surf_vert:        Boolean, whether to return surface distance and vertical distance separately (default=False)
+    :return:                        distance between two points in meters. Tuple of values (surface and alt distance) if return_surf_vert==True; absolute distance otherwise 
+    """
+    if len(lats) != 2 or len(lons) != 2 or len(alts) != 2:
+        raise ValueError("Latitudes, longitudes and altitude values must be 2-element lists or arrays.")
+    if type(alts[0])==np.ndarray:   # if altitudes are vectors, compute point-wise difference (must be same length)
+        if len(alts[0]) != len(alts[1]):
+            raise ValueError("If altitudes are vectors, their length must coincide.")
+        vert_dist = alts[1]-alts[0]
+    else:   # if alts are two points, compute difference between them
+        vert_dist = np.diff(alts)   # compute difference in altitude
+    # Compute geodetic distance according to method
+    if method=='greatcircle':       surface_dist = greatcircle_distance(lats[0], lats[1], lons[0], lons[1])
+    elif method=='vincenty':        surface_dist = vincenty_distance([lats[0], lons[0]], [lats[1], lons[1]])
+    else:                           raise Exception("Geodetic distance method " + method + " not recognized.")
+    # return horizontal and vertical distance or total distance
+    if return_surf_vert:            return surface_dist, vert_dist
+    else:                           return np.sqrt(surface_dist**2.0 + vert_dist**2.0)
+    
+
+def euclidean_distance_point_vector(point, vector):
+    """
+    Return the euclidean distance between multi-dimensional point and vector, the vector should be organized by column, i.e., 
+    each row is a value in the vector, and each column corresponds to a dimension.
+    Number of dimensions must match for point and vector:
+    
+    point : n x 1 array
+    vector: m x n array
+
+    :param point:   n x 1 array
+    :param vector:  m x n array
+    """
+    point_rep = np.repeat(point.reshape((1,-1)), repeats=vector.shape[0], axis=0)
+    d         = np.sum(abs(point_rep - vector)**2.0, axis=1)
+    return np.sqrt(d)
+
+    
+# REFERENCE FRAMES
+# ================
+def rot_eart2body_fast(sphi, cphi, stheta, ctheta, spsi, cpsi):
+    """
+    Return the rotation matrix R to transform coordinates from an Earth-fixed (inertial) reference frame to a body reference frame.
+    The function is called "fast" because takes the pre-computed sine, cosine, and other trigonometric functions of Euler's angles.
+
+    :param sphi:            scalar, sine of phi
+    :param cphi:            scalar, cosine of phi
+    :param stheta:          scalar, sine of theta
+    :param ctheta:          scalar, cosine of theta
+    :param spsi:            scalar, sine of psi
+    :param cpsi:            scalar, cosine of psi
+    :return:                rotation matrix to transform coordinates from Earth-fixed frame to body frame
+    """
+    return np.array([[                        ctheta*cpsi,                       ctheta * spsi,          - stheta],
+                     [-cphi * spsi + sphi * stheta * cpsi,  cphi * cpsi + sphi * stheta * spsi,     sphi * ctheta],
+                     [ sphi * spsi + cphi * stheta * cpsi, -sphi * cpsi + cphi * stheta * spsi,     cphi * ctheta]])
+
+def rot_body2earth_fast(sphi, cphi, stheta, ctheta, spsi, cpsi):
+    """
+    Return the rotation matrix R to transform coordinates from a body (non-inertial) reference frame to an Earth-fixed (inertial) reference frame.
+    The function is called "fast" because takes the pre-computed sine, cosine, and other trigonometric functions of Euler's angles.
+
+    :param sphi:            scalar, sine of phi
+    :param cphi:            scalar, cosine of phi
+    :param stheta:          scalar, sine of theta
+    :param ctheta:          scalar, cosine of theta
+    :param spsi:            scalar, sine of psi
+    :param cpsi:            scalar, cosine of psi
+    :return:                rotation matrix to transform coordinates from Earth-fixed frame to body frame
+    """
+    return np.array([[      cpsi * ctheta,          cpsi * stheta * sphi - spsi * cphi,         cpsi * stheta * cphi + spsi * sphi],
+                     [      spsi * ctheta,          spsi * stheta * sphi + cpsi * cphi,         spsi * stheta * cphi - cpsi * sphi],
+                     [           - stheta,                               ctheta * sphi,                              ctheta * cphi]])                    
+
+def body_ang_vel_from_eulers(phi, theta, psi, phidot, thetadot, psidot):
+    """ 
+    Compute the desired body angular velocities p, q, r given the desired Euler's angular velocities with respect to the inertial (Earth) reference frame.
+    The desired body angular velocities will be used to compute the desired moments and total thrust given the rotor configuration. From the desired
+    thrust and moments, it is possible to compute the desired rotor speed given the thrust allocation matrix (UAV dependent).
+
+    :param phidot:      first Euler's angle (phi) rate of change
+    :param thetadot:    second Euler's angle (theta) rate of change
+    :param psidot:      third Euler's angle (psi) rate of change
+    :return:            body angular velocities organized in column vector [p, q, r]^{\top}
+    """
+    p = phidot - psidot * np.sin(theta)
+    q = thetadot * np.cos(phi) + psidot * np.sin(phi) * np.cos(theta)
+    r = - thetadot * np.sin(phi) + psidot * np.cos(phi) * np.cos(theta)
+    return p, q, r
+
+# COORDINATE TRANSFORMATION
+# ==========================
+def gen_heading_angle(lat, lon, alt):
+    """
+    Function to generate heading angle to follow a set of points defined by latitude (lat), longitude (lon), and altitude (alt).
+    The function returns the heading from North, positive clock-wise
+
+    :param lat:             rad, n x 1, latitude points
+    :param lon:             rad, n x 1, longitude points
+    :param alt:             ft or m, n x 1, altitude points
+    :return:                rad, n x 1, heading angle to follow the input points
+    """
+    # Compute heading using lat-lon coordinates
+    # -----------------------------------------
+    # This heading is calculated from North, while reference frame is ENU (East-North-Up), therefore, the first direction is EAST, not NORTH.
+    # Need to adjust heading for ENU reference frame after calculating it.
+    head = heading_compute_geodetic(lat, lon)
+    # Adjust first heading based on altitude (avoid issues with fictitious waypoints later on)
+    head = heading_adjust_first_nonzero(head, alt)
+
+    # Adjust heading based on minimum rotation between current and next heading.
+    # ---------------------------------------------------------------------------
+    # Select the sign of the angle based on the shortest rotation the UAV is supposed to move
+    head = heading_adjust_rotation(head)
+    return head
+
+def heading_adjust_first_nonzero(heading, altitude):
+    """
+    Adjust first non-zero heading angle based on altitude.
+    :param heading:     rad, n x 1, heading angles
+    :param altitude:    m, n x 1, altitude points
+    """
+    n = len(heading)
+    for jj in range(n):
+        if altitude[jj] > 1.0 and heading[jj] == 0 and heading[jj + 1] != 0:
+            heading[jj] = heading[jj + 1]
+            break
+    return heading
+
+def heading_adjust_rotation(heading):
+    """
+    Adjust heading w.r.t. direction of travel. The sign of the heading should be decided based on the shortest rotation the UAV is supposed to move.
+    Example:
+        heading at i -th point = 60 deg
+        heading at i+1 -th point = -270 deg
+        change heading at i+1 -th point to 90 deg, so the vehicle will rotate only 30 deg from 60 to 90, instead of 60 to -270.
+    
+    :param heading:     rad, n x 1, heading angles.
+    :return:            rad, n x 1, heading angles with the shorter rotation distance between point i and i+1
+    """
+    n = len(heading)
+    for idx in range(n-1):
+        curr_p = heading[idx]
+        next_p = heading[idx+1]
+        # If angles are identical move on
+        if curr_p == next_p:
+            continue
+        else:
+            next_p_2 = next_p - 2.0*np.pi   # angle in opposite direction
+            # Select angle based on minimum rotation necessary
+            if abs(curr_p - next_p) < abs(curr_p - next_p_2):   heading[idx+1] = next_p
+            else:                                               heading[idx+1] = next_p_2
+    return heading
+
+def heading_compute_geodetic(lat, lon):
+    """
+    Compute heading angle to follow a set of points defined by geodetic coordinates lat, lon.
+
+    :param lat:         rad, n x 1, latitude points
+    :param lon:         rad, n x 1, longitude points
+    :return:            rad, n x 1, heading angle
+    """
+    n = len(lat)    
+    heading = np.zeros((n,))
+    for jj in range(1, n):  
+        dlon_ = lon[jj] - lon[jj - 1]   # compute difference in longitude
+        dlat_ = lat[jj] - lat[jj - 1]   # compute difference in latitude
+        X = np.cos(lat[jj]) * np.sin(dlon_) # compute cartesian coordinate
+        Y = np.cos(lat[jj - 1]) * np.sin(lat[jj]) - np.sin(lat[jj - 1]) * np.cos(lat[jj]) * np.cos(dlon_)   # compute cartesian coordinate
+        head_temp = np.arctan2(X, Y)    # heading as arc-tangent of cartesian coordinates
+        if Y != 0:  # adjust for 360 degrees
+            head_temp -= np.pi / 2.0
+            head_temp *= -1.0
+        if jj < n - 1 and ((dlat_ != 0 or dlon_ != 0) and head_temp == 0):  # if calculated heading is 0, keep the old one.
+            head_temp = heading[jj - 2]
+        heading[jj - 1] = head_temp
+    heading[-1] = heading[-2]
+    return heading
+
+
+# Coordinate class
+# ===============
+class Coord():
+    """
+    Coordinate class:
+    transform coordinate values between geodetic frame and cartesian frames, and vice versa.
+    """
+    def __init__(self, lat0, lon0, alt0):
+        """
+        Initialize Coordinate frame class.
+
+        :param lat0:            latitude of origin of reference frame
+        :param lon0:            longitude of origin of reference frame
+        :param alt0:            altitude of origin of reference frame
+        """
+        self.a = 6378137.0                      # [m], equatorial radius
+        self.f = 1.0 / 298.257223563            # [-], ellipsoid flatness
+        self.b = self.a * (1.0 - self.f)        # [m], polar radius
+        self.e = np.sqrt(self.f * (2-self.f))   # [-], eccentricity
+        self.lat0 = lat0
+        self.lon0 = lon0
+        self.alt0 = alt0
+        self.N0   = self.a / np.sqrt(1 - self.e**2.0 * np.sin(self.lat0)**2.0) # [m], Radius of curvature on the Earth
+
+    def ecef2enu(self, xecef, yecef, zecef):
+        """
+        Conversion of Earth-centered, Earth-fixed (ECEF) reference frame coordinates to East-North-UP (ENU) reference frame.
+
+        :param xecef:           m, n x 1, X-coordinate in ECEF reference frame
+        :param yecef:           m, n x 1, Y-coordinate in ECEF reference frame
+        :param zecef:           m, n x 1, Z-coordinate in ECEF reference frame
+        :return x:              m, n x 1, x-coordinate (East) in ENU reference frame
+        :return y:              m, n x 1, y-coordinate (North) in ENU reference frame
+        :return z:              m, n x 1, z-coordinate (Up) in ENU reference frame
+        """
+        # Compute location of the origin of ENU reference frame in the ECEF reference frame
+        x0 = (self.alt0 + self.N0) * np.cos(self.lat0) * np.cos(self.lon0)
+        y0 = (self.alt0 + self.N0) * np.cos(self.lat0) * np.sin(self.lon0)
+        z0 = (self.alt0 + (1.0 - self.e**2.0) * self.N0) * np.sin(self.lat0)
+
+        # Compute relative distance between the data points and the origin of the ENU reference frame
+        xd = xecef - x0
+        yd = yecef - y0
+        zd = zecef - z0
+
+        # Compute coordinates in ENU reference frame
+        x  = - np.sin(self.lon0) * xd + np.cos(self.lon0) * yd
+        y  = - np.cos(self.lon0) * np.sin(self.lat0) * xd - np.sin(self.lat0) * np.sin(self.lon0) * yd + np.cos(self.lat0) * zd
+        z  =   np.cos(self.lat0) * np.cos(self.lon0) * xd + np.cos(self.lat0) * np.sin(self.lon0) * yd + np.sin(self.lat0) * zd
+
+        return x, y, z
+
+    def enu2ecef(self, xenu, yenu, zenu):
+        """
+        Conversion of East-North-UP (ENU) reference frame coordinates into Earth-centered, Earth-fixed (ECEF) reference frame.
+
+        :param xenu:             m, n x 1, x-coordinate (East) in ENU reference frame
+        :param yenu:             m, n x 1, y-coordinate (North) in ENU reference frame
+        :param zenu:             m, n x 1, z-coordinate (Up) in ENU reference frame
+        :return xecef:           m, n x 1, X-coordinate in ECEF reference frame
+        :return yecef:           m, n x 1, Y-coordinate in ECEF reference frame
+        :return zecef:           m, n x 1, Z-coordinate in ECEF reference frame
+        """
+
+        # Compute coordinates of origin of ENU in the ECEF reference frame
+        x0 = (self.alt0 + self.N0) * np.cos(self.lat0) * np.cos(self.lon0)
+        y0 = (self.alt0 + self.N0) * np.cos(self.lat0) * np.sin(self.lon0)
+        z0 = (self.alt0 + (1.0 - self.e**2.0) * self.N0) * np.sin(self.lat0)
+
+        # Compute relative coordinates in ECEF reference frame
+        xd = -np.sin(self.lon0) * xenu - np.cos(self.lon0) * np.sin(self.lat0) * yenu + np.cos(self.lat0) * np.cos(self.lon0) * zenu
+        yd =  np.cos(self.lon0) * xenu - np.sin(self.lat0) * np.sin(self.lon0) * yenu + np.cos(self.lat0) * np.sin(self.lon0) * zenu
+        zd =  np.cos(self.lat0) * yenu + np.sin(self.lat0) * zenu
+
+        # Compute global coordinates in ECEF reference frame by adding the location of the ENU to the relative ECEF coordinates
+        xecef = xd + x0
+        yecef = yd + y0
+        zecef = zd + z0
+        return xecef, yecef, zecef
+
+
+    def ecef2geodetic(self, X, Y, Z):
+        """
+        lat, lon, alt = Coord.ecef2geodetic(X, Y, Z)
+    
+        Convert ECEF coordinates into geodetic coordinates.
+    
+        The equations used in this function come from the report:
+            Geomatics Guidance Note 7, part 2 Coordinate Conversions & Transformations including Formulas
+            International Association of Oil & Gas producers, Report 373-7-2,
+            April 2018.
+            See Chapter 2, Subsection 2.2, Sub-subsection 2.2.1
+    
+        INPUT
+            X       n x 1 array, real, [m], X coordinate in ECEF reference system
+            Y       n x 1 array, real, [m], Y coordinate in ECEF reference system
+            Z       n x 1 array, real, [m], Z coordinate in ECEF reference system
+    
+        OUTPUT
+            lat     n x 1 array, real, [rad], latitude values
+            lon     n x 1 array, real, [rad], longitude values
+            alt     n x 1 array, real, [m], altitude values
+        """
+
+        # Ancillary variables
+        epsilon = self.e**2.0 / (1.0 - self.e**2.0)
+        b       = self.a * (1.0 - self.f)
+        p       = np.sqrt(X**2.0 + Y**2.0)
+        q       = np.arctan2( Z * self.a , p * b)
+
+        # Compute latitude, longitude and altitude.
+        lat = np.arctan2( (Z + epsilon * b * np.sin(q)**3.0) , (p - self.e**2.0 * self.a * np.cos(q)**3.0) ) # [rad], latitude
+        lon = np.arctan2( Y , X );                                                                           # [rad], longitude
+        N   =  self.a /  np.sqrt(1.0 - self.e**2.0 * np.sin(lat)**2.0);                                      # [m], Radius of curvature on the Earth
+        alt = (p / np.cos(lat)) - N
+        return lat, lon, alt
+
+
+    def geodetic2ecef(self, lat, lon, alt):
+        """
+        Conversion of geodetic reference frame coordinates into Earth-centered, Earth-fixed (ECEF) reference frame.
+
+        :param lat:     rad, n x 1, latitude 
+        :param lon:     rad, n x 1, longitude
+        :param alt:     m, n x 1, altitude
+        :return X:      m, n x 1, X-coordinate in ECEF reference frame
+        :return Y:      m, n x 1, Y-coordinate in ECEF reference frame
+        :return Z:      m, n x 1, Z-coordinate in ECEF reference frame
+        """
+        N = self.a / np.sqrt(1.0 - self.e**2.0 * np.sin(lat)**2.0)  # [m], Radius of curvature on the Earth, phi = latitude
+        X = (N + alt) * np.cos(lat) * np.cos(lon)
+        Y = (N + alt) * np.cos(lat) * np.sin(lon)
+        Z = (alt + (1.0 - self.e**2.0) * N) * np.sin(lat)
+        return X, Y, Z
+
+
+    def enu2geodetic(self, x, y, z):
+        """
+        Conversion of East-North-UP (ENU) reference frame coordinates into geodetic coordinates.
+        This method calls first method ENU2ECEF, and then ECEF2GEODETIC.
+
+        :param x:              m, n x 1, x-coordinate (East) in ENU reference frame
+        :param y:              m, n x 1, y-coordinate (North) in ENU reference frame
+        :param z:              m, n x 1, z-coordinate (Up) in ENU reference frame
+        :return lat:           rad, n x 1, latitude in geodetic coordinates
+        :return lon:           rad, n x 1, longitude in geodetic coordinates
+        :return alt:           m, n x 1, altitude in geodetic coordinates
+        """
+        xecef_tmp, yecef_tmp, zecef_tmp = self.enu2ecef(x, y, z)
+        return self.ecef2geodetic(xecef_tmp, yecef_tmp, zecef_tmp )
+
+
+    def geodetic2enu(self, lat, lon, alt):
+        """
+        Conversion of coordinates (lat, lon, alt) in geodetic reference frame into East-North-UP (ENU) reference frame.
+        This method calls first method GEODETIC2ECEF, and then ECEF2ENU.
+
+        :param lat:           rad, n x 1, latitude in geodetic coordinates
+        :param lon:           rad, n x 1, longitude in geodetic coordinates
+        :param alt:           m, n x 1, altitude in geodetic coordinates
+        :return x:            m, n x 1, x-coordinate (East) in ENU reference frame
+        :return y:            m, n x 1, y-coordinate (North) in ENU reference frame
+        :return z:            m, n x 1, z-coordinate (Up) in ENU reference frame
+        """
+        X, Y, Z = self.geodetic2ecef(lat, lon, alt)
+        return self.ecef2enu(X, Y, Z)
diff --git a/src/prog_models/utils/traj_gen/nurbs.py b/src/prog_models/utils/traj_gen/nurbs.py
new file mode 100644
index 000000000..0ad08338e
--- /dev/null
+++ b/src/prog_models/utils/traj_gen/nurbs.py
@@ -0,0 +1,317 @@
+# Copyright © 2021 United States Government as represented by the Administrator of the
+# National Aeronautics and Space Administration.  All Rights Reserved.
+
+"""
+NURBS curve functions
+"""
+
+import numpy as np
+import scipy.interpolate as interp
+
+
+
+# NURBS Functions
+# ================
+def normalize_t_1(t, a, b):
+    """ 
+    Normalization Function 1: normalize t in (a,b) range as:
+            
+            (t - a) / (b - a)
+    
+    Variable t can be a scalar or 1D array / vector.
+    Values a, b can be scalars or 1D arrays or vectors with same length of t.
+
+    :param t:       scalar or n x 1, value to be normalized
+    :param a:       scalar or n x 1, first normalization parameter
+    :param b:       scalar or n x 1, second normalization parameter
+    :return:        scalar or n x 1, normalized t value
+    """
+    with np.errstate(divide="ignore"):  # ignore 0/0 warnings
+        return (t-a) / (b - a)
+
+
+def normalize_t_2(t, a, b):
+    """ 
+    Normalization Function 2: normalize -t in (a,b) range as:
+            
+            (b - t) / (b - a)
+    
+    Variable t can be a scalar or 1D array / vector.
+    Values a, b can be scalars or 1D arrays or vectors with same length of t.
+
+    :param t:       scalar or n x 1, value to be normalized
+    :param a:       scalar or n x 1, first normalization parameter
+    :param b:       scalar or n x 1, second normalization parameter
+    :return:        scalar or n x 1, normalized t value
+    """
+    with np.errstate(divide="ignore"):  # ignore 0/0 warnings
+        return (b-t) / (b-a)
+
+
+class NURBS:
+
+    def __init__(self, 
+                 points : dict,
+                 weights,
+                 times,
+                 yaw = None,
+                 **kwargs) -> None:
+
+        self.points = points
+        self.dims = list(self.points.keys())
+        self.weights = weights
+        self.times = times
+        self.yaw = yaw
+        self.knotv = None
+
+        self.parameters = dict(order=4, basis_length=1000)
+        self.parameters.update(**kwargs)
+
+    def point_values(self,):
+        return np.vstack(list(self.points.values())).T
+
+    def points_new_values(self,):
+        return np.vstack(list(self.points_new.values())).T
+
+    def generate(self, timestep_size):
+        """
+        Generate NURBS curve given the timestep_size.
+        This function first add intermediate way-points to ensure the control over what way-points need to be approached closely and what
+        could be passed by far away.
+        Then the function generate the position profile as a function of such way-points, using the desired timestep_size.
+        
+        The last step consists of the generation of a smooth yaw curve.
+
+        :param timestep_size:           scalar, double, time step size to be used to generate the smooth trajectory.
+        :return:                        three m x 1 arrays corresponding to the interpolated position and yaw angle, and the corresponding time vector
+        """
+        # Generate intermediate way-points to control which way-points should/could be passed by closely or far away
+        # ------------------------------------------------------------------------------------------------------
+        points_new, self.times_new, self.yaw_new, self.weight_vector_new = self.generate_intermediate_points()
+        self.points_new = {dim: points_new[:, i] for i, dim in enumerate(self.dims)}    # Save way-points with intermediate values:
+
+        # Generate the knot vector necessary for the NURBS-based interpolation
+        # --------------------------------------------------------------------
+        self.gen_knot_vector(points_new.shape[0]-1)
+        
+        # Interpolate position profile
+        # -----------------
+        position_interp = {self.dims[ii]:None for ii in range(len(self.dims))}
+        timevec_interp = None
+        for ii, dim_i in enumerate(self.dims):
+            pos_interp, timevec = self.interpolate(pos=points_new[:, ii], 
+                                                   times=self.times_new, 
+                                                   timestepsize=timestep_size, 
+                                                   weightvector=self.weight_vector_new)
+            position_interp[dim_i] = pos_interp
+        timevec_interp = timevec
+
+        # Interpolate yaw
+        # -----------------
+        yaw_interp, timevec = self.interpolate(self.yaw_new,        # anchor points: yaw points
+                                               self.times_new,      # time instants of the anchor points
+                                               np.ones_like(self.yaw_new),      # weight vector, can be all the same for the yaw points
+                                               timestep_size,                   # time step size for the curve (same as for position)
+                                               order=3,                         # overriding order of the nurbs: higher order help the sharp yaw changes
+                                               basis_length=2000,               # basis length: 1000, could be changed but would either increase computation cost or coarse the trajectory too much
+                                               interp_order='zero')             # order of the yaw curves: the yaw should be kept constant in-between waypoints (no smooth variations, except for the proximity of turns), keep order 0 so is step-wise constant
+        return position_interp, yaw_interp, timevec_interp
+
+    def gen_knot_vector(self, n):
+        """
+        Generate knot vector of k-th order for a NURBS with n-points.
+        :param n:       number of anchor points of the curve -1  (e.g., number waypoints of trajectory - 1)
+        :return:        normalized knot vector, [0,1]
+        """
+        k = self.parameters['order']
+        t              = np.zeros((n + k + 1,))             # initialize vector
+        idx_vec        = np.arange(1, n + k + 2)            # generate indices of the vector
+        msk1           = (idx_vec >= k) * (idx_vec <= n)    # create a mask for values between k and n
+        t[msk1]        = (idx_vec[msk1] - 1) - k + 1        # assign values to the masked elements
+        t[idx_vec > n] = n - k + 2                          # assign fixed value n-k+2 for everything above index n
+        t              = (t - min(t)) / (max(t) - min(t))   # normalize vector
+        self.knotv = t
+    
+    def basisfunction(self, u):
+        """
+        Basis function used to generate a smooth NURBS given order and knot vector t.
+        Variable u is the independent variable of the basis function.
+
+        :param u:           scalar, double, independent variable u used to generate basis function N(u)
+        :param t:           knot vector defining the control points
+        :return:            basis function of order 'order', N_order(u)
+        """
+        order = self.parameters['order']
+        t = self.knotv
+        
+        n_points = len(t)
+        N        = np.zeros((n_points,))
+        N[(u > t) * np.insert((u <= t[1:]), -1, True)] = 1
+        if N.any() != 0:
+            for n in range(1, order):
+                t_pn  = np.concatenate((t[n:], np.ones((n,))))
+                d     = normalize_t_1(u, t, t_pn) * N
+                t_pn1 = np.concatenate((t[n+1:], np.ones((n+1,))))
+                t_p1  = np.concatenate((t[1:], np.ones((1,))))
+                e     = normalize_t_2(u, t_p1, t_pn1) * np.insert(N[1:], -1, 0) 
+                N     = np.nan_to_num(e) + np.nan_to_num(d)
+                
+        return N[:n_points - order]
+
+    def evaluate(self, pointvec, weightvec, order=None, basis_length=None):
+        """
+        Evaluate NURBS function given order, knot vector t, weights of the waypoints, waypoint vector, and basis length
+
+        :param order:               order of the NURBS curve
+        :param t:                   knot vector with control points
+        :param weights:             weights of the waypoints
+        :param waypoint_vector:     matrix containing waypoints
+        :param basis_length:        length of the basis function, number of points used to define independent variable u
+        :return:                    NURBS curve evaluated at the desired points (defined by knot vector t. Form t to u to C(u))
+                                    2 x q, where the first row is time, second row is quantity x. q is the number of points len(U)
+        """
+        # Check validity of waypoint vector
+        # It should be a 2 x m array where m is the number of waypoints, the first row is the ETA vector and the second row is the actual waypoint vector.
+
+        n, m = pointvec.shape
+        if n != 2:
+            pointvec = pointvec.T
+
+        if order is None:
+            order = self.parameters['order']
+            
+        if basis_length is None:
+            basis_length = self.parameters['basis_length']
+
+        # Define independent variable vector U (composed of all points of the independent variable u_0, u_1, ...)
+        U = np.linspace(self.knotv[order-1], self.knotv[-1], int(round(abs(self.knotv[-1] - self.knotv[order-1])*basis_length)))
+        q = len(U)
+
+        # Calculate basis functions
+        # -----------------------------------------
+        N = np.zeros((q, pointvec.shape[1]))
+        for ii in range(q):
+            N[ii, :] = self.basisfunction(U[ii])   # first piece of basis function
+        N[0, 0] = 1.0   # Numerical issue: the first point of the first basis is not computed but remains 0 when it should be 1. This fixes it.
+
+        S = np.dot(N, weightvec)        # Calculate denominator of rational basis function
+        R = N * np.repeat(weightvec.reshape((1, -1)), repeats=N.shape[0], axis=0)       # Calculate rational basis function
+        with np.errstate(divide="ignore"):  # ignore warning divide by zero
+            R /= np.repeat(S.reshape((-1, 1)), repeats=m, axis=1)
+        R[np.isnan(R)] = 0.
+
+        # Generate points of the planned path
+        # ----------------------------------
+        C = np.zeros((2, q))
+        C[0, :] = np.dot(R, pointvec[0, :].reshape((-1,)))
+        C[1, :] = np.dot(R, pointvec[1, :].reshape((-1,)))
+        return C
+    
+    def interpolate(self, pos, times, weightvector, timestepsize, order=None, basis_length=None, interp_order='cubic'):
+        """
+        Interpolate a set of anchor points with corresponding time instants and weight vector using NURBS bases.
+        This is the core of a NURBS interpolation.
+
+        :param pos:     n x 1 array of anchor or position points
+        :param times:   n x 1 array of time instants corresponding to the anchor points
+        :param weightvector:            n x 1 array of weights associated with each anchor point
+        :param timestepsize:            scalar, double, dt of the resulting nurbs-based curve.
+        :param oder:                    scalar, int, order of the NURBS used for the interpolation. Deafult is None, which uses the one defined at the class instantiation
+        :param basis_length:            scalar, int, length of the basis function used to generate the NURBS. Default is None, which uses the one defined at the class instantiation
+        :param interp_order:            string, final interpolation of the curve. This is a 1D interpolation to further smooth out the trajectory. Default is 'cubic'.
+        :return                         two m x 1 arrays, a position profile and time vector, interpolated with the desired timestepsize.
+        """
+        m = len(times)  # time values of each corresponding point in pos
+        
+        # Initialize array containing the time-parameterized curve
+        p_vector = np.zeros((2, m)) 
+        p_vector[0, :] = times  # first row is time
+        p_vector[1, :] = pos    # second row is anchor points (or positions)
+
+        # Evaluate NURBS at desired times with corresponding weight vector, given order and basis length
+        pos_vs_time = self.evaluate(p_vector, weightvector, order=order, basis_length=basis_length)
+
+        # Generate fine-grained time vector for interpolation
+        timevec = np.arange(times[0], times[-1]+timestepsize, timestepsize)
+
+        # Interpolation position vs time to have a smooth trajectory
+        pos_vs_time[:, 0] = np.array([0.0, pos[0]])
+        pos_interp = interp.interp1d(pos_vs_time[0, :], pos_vs_time[1, :], kind=interp_order, fill_value='extrapolate')(timevec)
+        return pos_interp, timevec
+
+
+    def generate_intermediate_points(self,):
+        """
+        This function takes the original set of curve points and generates a new set containing fictitious, intermediate points.
+        The fictitious points help in generating a NURBS trajectory that passes through the desired (true) points according
+        to the weight vector.
+
+        The fictitious points are particularly useful for rotary-wing UAV trajectories, since the latter have very small minimum-radius
+        turns and can do sharp turns. Without such fictitious points, the trajectory will look more like a fixed-wing trajectory, where the waypoints
+        are approached, but they are not reached.
+
+        :param points:             (n x 3) array, coordinate of points
+        :param yaw:            (n,) array, yaw angle in between waypoints (ini yaw=0), [rad]
+        :param eta:            (n,) array, estimated time of arrival at each waypoint, [s]
+
+        :return points:                  (m x 3) array, coordinate including intermediate points,
+        :return yaw_new:                 (m,) array, yaw values including intermediate points, [rad]
+        :return eta_new:                 (m,) array, ETA at points, including intermediate points, [s]
+        """
+        # Get from class properties
+        # -----------------------
+        points = self.point_values()
+        times = self.times
+        yaw = self.yaw
+
+        # ------- New array of waypoints ------- #
+        intermediate_points = np.zeros((points.shape[0] - 1, 3))     # initialize array of fictitious waypoints
+        for ii in range(1, points.shape[0]):
+            intermediate_points[ii - 1, :] = (points[ii, :] - points[ii - 1, :]) / 2.0 + points[ii - 1, :]  # generate a fictitious waypoint as average of true waypoints
+
+        # Generate the new waypoint matrix
+        points_new = np.zeros((points.shape[0] + intermediate_points.shape[0], points.shape[1]))
+        counter_1, counter_2 = 0, 0
+        for ii in range(points_new.shape[0]):
+            if ii == 0 or ii % 2 == 0:
+                points_new[ii, :] = points[counter_1, :]
+                counter_1 += 1
+            else:
+                points_new[ii, :] = intermediate_points[counter_2, :]
+                counter_2 += 1
+
+        # -------- New Weight Vector ------------- #
+        weight_vector_new = np.zeros((points_new.shape[0],))            # initialize weight vector
+        counter_1 = 0  # add a temporary counter for the true weight vector
+        for jj in range(len(weight_vector_new)):
+            if jj % 2 == 0:
+                weight_vector_new[jj] = self.weights[counter_1]      # Assign pre-defined weight to the waypoint
+                counter_1 += 1
+            else:
+                weight_vector_new[jj] = 1                            # Assign standard weight of 1 to the fictitious waypoint
+
+        # -------- Generate new ETA vector ------------- #
+        intermediate_times = np.zeros((len(self.weights) - 1,))      # initialize fictitious ETA vector
+        for ii in range(1, len(intermediate_times) + 1):
+            intermediate_times[ii - 1] = (times[ii] - times[ii - 1]) / 2.0 + times[ii - 1] # generate fictitious ETA as average of real ETAs
+
+        # Generate new ETA vector
+        times_new = np.zeros((weight_vector_new.shape[0],))
+        counter_1, counter_2 = 0, 0
+        for ii in range(len(times_new)):
+            if ii == 0 or ii % 2 == 0:
+                times_new[ii] = times[counter_1]                # Assign existing ETA
+                counter_1 += 1
+            else:
+                times_new[ii] = intermediate_times[counter_2]      # Assign fictitious ETA
+                counter_2 += 1
+
+        # -------- Generate new Yaw vector ---------- #
+        # The fictitious yaw is NOT the average of the true yaw, but it is equal to the yaw at the previous (true) waypoint.
+        yaw_new = np.zeros((points_new.shape[0],))            # Initialize new yaw vector
+        counter_1 = 1
+        for jj in range(len(yaw)):
+            yaw_new[counter_1:counter_1 + 2] = yaw[jj]    # Assign new yaw value
+            counter_1 += 2                                # this counter is
+        return points_new, times_new, yaw_new, weight_vector_new
+        
\ No newline at end of file
diff --git a/src/prog_models/utils/traj_gen/trajectory.py b/src/prog_models/utils/traj_gen/trajectory.py
new file mode 100644
index 000000000..a59807257
--- /dev/null
+++ b/src/prog_models/utils/traj_gen/trajectory.py
@@ -0,0 +1,622 @@
+# Copyright © 2021 United States Government as represented by the Administrator of the
+# National Aeronautics and Space Administration.  All Rights Reserved.
+
+"""
+Auxiliary functions for trajectories and aircraft routes
+"""
+
+import numpy as np
+from warnings import warn
+
+from prog_models.utils.traj_gen import geometry as geom
+from prog_models.utils.traj_gen.nurbs import NURBS
+
+
+def compute_derivatives(position_profile, timevec):
+    # Compute derivatives of position: velocity and acceleration
+    # (optional: jerk, not needed)
+    # ---------------------------------------------------------
+    dim_keys = list(position_profile.keys())
+    vel_interp = {dim_key: None for dim_key in dim_keys}
+    acc_interp = {dim_key: None for dim_key in dim_keys}
+    for key in dim_keys:
+        vel_interp[key], acc_interp[key], _ = derivate_position(
+            position_profile[key],
+            timevec[1]-timevec[0])
+    
+    return {'velocity': vel_interp, 'acceleration': acc_interp}
+
+
+def linearinterp_t(t0, x0, t1, x1, xp):
+    """
+    Linear interpolation of input xp given two points (t0, x0), (t1, x1)
+    :param t0:          independent variable of first point
+    :param x0:          dependent variable of first point
+    :param t1:          independent variable of second point
+    :param x1:          dependent variable of second point
+    :param xp:          independent variable of query point
+    """
+    dx = x1 - x0
+    dt = t1 - t0
+    der = dx / dt
+    t_land = 1.0/der * (xp + der * t0 - x0)
+    return t_land
+
+
+def reshape_route_attribute(x, dim=None, msk=None):
+    """
+    Reshape route attribute to ensure correct length
+
+    :param x:           attribute to reshape
+    :param dim:         scalar, int or None, desired length of attribute. Used only if x has no attribute "len"
+    :param msk:         list or array of int, or None, mask to reshape attribute x. The function return x by repeating x[msk] at msk points.
+    :return:            reshaped attribute
+    """
+    if not hasattr(x, "__len__"):
+        return x * np.ones((dim, ))
+    elif msk:
+        return np.insert(x, msk, x[msk])
+    
+
+def angular_vel_from_attitude(phi, theta, psi, delta_t=1):
+    """
+    Compute angular velocities from attitudes. The function computes the body angular rates p, q, r given the 
+    desired Euler's angles parameterized by time: phi(t), theta(t), psi(t), and their corresponding rates.
+
+    :param phi:         double, n x 1, first Euler's angle as a function of time.
+    :param theta:       double, n x 1, second Euler's angle as a function of time.
+    :param psi:         double, n x 1, third Euler's angle as a function of time.
+    :param delta_t:     double, scalar, time step size, (default=1).
+    :return p:          double, n x 1, body roll rate as a function of time
+    :return q:          double, n x 1, body pitch rate as a function of time
+    :return r:          double, n x 1, body yaw rate as a function of time
+    """
+    phidot = np.insert(np.diff(phi) / delta_t, 0, 0.0)
+    thetadot = np.insert(np.diff(theta) / delta_t, 0, 0.0)
+    psidot = np.insert(np.diff(psi) / delta_t, 0, 0.0)
+    p = np.zeros_like(phidot)
+    q = p.copy()
+    r = p.copy()
+    for ii in range(len(phi)):
+        des_angular_vel = geom.body_ang_vel_from_eulers(
+            phi[ii], theta[ii], psi[ii], phidot[ii], thetadot[ii], psidot[ii])
+        p[ii], q[ii], r[ii] = des_angular_vel[0], des_angular_vel[1], des_angular_vel[2]
+    return p, q, r
+
+
+def derivate_position(p, dt):
+    """
+    Given a position profile as a function of time, p = p(t), derivate it up to 3 times to obtain velocity, acceleration, and jerk
+
+    For simplicity of computation as part of a trajectory profile, the derivatives are force to start at 0, e.g.,  velocity[0] = 0.0
+
+    :param p:           n x 1 array, doubles, position profile as a function of time
+    :param dt:          scalar, double, time step size used to generate the position profile, which also serves as derivative increment
+    :return:            three n x 1 arrays of doubles in a list, corresponding to velocity, acceleration and jerk, respectively.
+    """
+    v = np.zeros_like(p)
+    a = v.copy()
+    j = v.copy()
+
+    v = np.gradient(p, dt)
+    v[0] = 0.
+    a = np.gradient(v, dt)
+    a[0] = 0.
+    j = np.gradient(j, dt)
+    j[0] = 0.
+    return v, a, j
+
+
+def gen_attitude(psi, ax, ay, az, max_phi, max_theta, gravity):
+    """Calculate angular kinematics based on acceleration and yaw"""
+    # linearized angular kinematics
+    phi = 1.0 / (gravity + az) * (ax * np.sin(psi) - ay * np.cos(psi))
+    theta = 1.0 / (gravity + az) * (ax * np.cos(psi) + ay * np.sin(psi))
+    
+    # Introduce limits on attitude angles
+    phi = np.fmax(np.fmin(phi, max_phi), -max_phi)
+    theta = np.fmax(np.fmin(theta, max_theta), -max_theta)
+    return phi, theta, psi
+   
+
+class Trajectory():
+    """
+    Trajectory class.
+
+    This class generates a trajectory object that is used to create a smooth profile including
+    position, velocity, acceleration, Euler's angles, and attitude rates.
+    The profiles are used to generate the desired state of a vehicle that needs to be followed to complete the trajectory.
+    
+    args:
+            lat (np.ndarray):
+                rad, n x 1 array, doubles, latitude coordinates of waypoints
+            lon (np.ndarray):
+                rad, n x 1 array, doubles, longitude coordinates of waypoints
+            alt (np.ndarray):
+                m, n x 1 array, doubles, altitude coordinates of waypoints
+            takeoff_time (float, optional):
+                take off time of the trajectory. Default is None (starting at current time).
+            etas (list[float], optional):
+                ETAs of each waypoints. Default is None. In that case, the ETAs are calculated based on the desired speed between waypoints.
+
+    kwargs:
+            gravity (float, optional):
+                Magnitude of force of gravity. Default 9.81 m/s^2
+            max_phi (float, optional):
+                Maximum Euler's angle (rad). Default 45/180.0*np.pi
+            max_theta (float, optional):
+                maximum Euler's angle (rad). Default 45/180.0*np.pi
+            max_iter (int, optional):
+                maximum number of iterations to adjust the trajectory according to the maximum average jerk. Default: 10
+            max_avgjerk (float, optional):
+                maximum average Jerk allowed (m/s^3), if the generated trajectory has a higher value, new, more forgiving ETAs are generated to satisfy this constraint. Default: 20
+            nurbs_order (int, optional):
+                order of the NURBS used to generate the position profile. Default: 4
+            waypoint_weight (int, optional):
+                default weight value associated to all the waypoints. Default: 20
+            weight_vector (np.ndarray, optional):
+                Waypoint weight vector. Default is None (all waypoints will have same value from 'waypoint_weight'). If passed, the user can define different weight for each waypoints, such that some waypoints will be approached more closely and some will be approached more smoothly.
+            nurbs_basis_length (int, optional):
+                default length of basis function used to generate the position profile with the NURBS.Default: 1000
+            cruise_speed (float, optional):
+                desired cruise speed between waypoints (m/s). If ETAs are provided, this value is ignored. Default = 6.0.
+            ascent_speed (float, optional):
+                desired ascent speed (m/s) between waypoints. If ETAs are provided, this value is ignored. Defualt: 3
+            descent_speed (float, optional):
+                desired descent speed (m/s) between waypoints. If ETAs are provided, this value is ignored. Default: 3
+            landing_speed (float, optional):
+                desired landing speed (m/s) between waypoints. This speed is used when the vehicle's altitude is lower than 'landing_altitude' parameter. If ETAs are provided, this value is ignored. Default: 1.5
+            landing_altitude (float, optional):
+                landing altitude (m) below which the vehicle is supposed to move at 'landing_speed'. Default: 10.5
+    """
+    def __init__(self,
+                 lat,
+                 lon,
+                 alt,
+                 takeoff_time=None,
+                 etas=None,
+                 **kwargs):
+
+        # Check waypoint types:
+        # ---------------------
+        if not isinstance(lat, np.ndarray) or not isinstance(lon, np.ndarray) or not isinstance(alt, np.ndarray):
+            raise TypeError("Latitudes, longitudes, and altitudes must be provided as n x 1 arrays.")
+        if lat.shape != lon.shape or lon.shape != alt.shape:
+            raise ValueError("Provided latitude, longitude, and altitude arrays must be the same length.")
+        if lat.shape[0] <= 1 or lon.shape[0] <= 1 or alt.shape[0] <= 1:
+            raise ValueError("Latitudes, longitudes, and altitudes must be provided as n x 1 arrays, with n > 1.")
+        if etas is not None:
+            if not isinstance(etas, list):
+                raise TypeError("ETAs must be provided as a list of floats.")
+            if len(etas) != 1 and len(etas) != lat.shape[0]:
+                raise ValueError("ETA must be either a take off time (one value), or a vector array with same length as lat, lon and alt.")
+            for eta in etas:
+                if not isinstance(eta, (float, int)):
+                    raise TypeError("ETAs must be provided as a list of floats.")
+        if takeoff_time is not None and not isinstance(takeoff_time, (float, int)):
+            raise TypeError("Takeoff time must be provided as a float.")
+
+        # Trajectory dictionary to store output
+        # -----------------------------------
+        self.trajectory = {}
+
+        # Route properties
+        # =================
+        self.waypoints = {
+            'lat': lat,
+            'lon': lon,
+            'alt': alt,
+            'takeoff_time': takeoff_time,
+            'eta': etas,
+            'x': None,
+            'y': None,
+            'z': None,
+            'heading': None}
+        
+        # Assign takeoff time
+        if self.waypoints['takeoff_time'] is None:
+            if self.waypoints['eta'] is not None:
+                self.waypoints['takeoff_time'] = self.waypoints['eta'][0]
+            else:
+                self.waypoints['takeoff_time'] = 0
+
+        # Generate Heading
+        self.waypoints['heading'] = geom.gen_heading_angle(
+            self.waypoints['lat'],
+            self.waypoints['lon'],
+            self.waypoints['alt'])
+
+        # Set up coordinate system conversion between Geodetic,
+        # Earth-Centric Earth-Fixed (ECF), and Cartesian (East-North-Up, ENU)
+        # ------------------------------------------------------
+        self.coordinate_system = geom.Coord(
+            self.waypoints['lat'][0],
+            self.waypoints['lon'][0],
+            self.waypoints['alt'][0])
+
+        # Define speed parameters - only necessary if ETAs are not defined
+        # ------------------------------------------------------
+        if etas is not None and ('cruise_speed' in kwargs or 'ascent_speed' in kwargs or 'descent_speed' in kwargs or 'landing_speed' in kwargs):
+            warn("Speed values are ignored since ETAs were specified. To define speeds (cruise, ascent, descent, landing) instead, do not specify ETAs.")
+        if etas is None and ('cruise_speed' not in kwargs or 'ascent_speed' not in kwargs or 'descent_speed' not in kwargs or 'landing_speed' not in kwargs):
+            warn("Neither ETAs nor speeds were defined. Default speeds will be used.")
+        self.speed_parameters = {
+            'cruise_speed': 6.0,
+            'ascent_speed': 3.0,
+            'descent_speed': 3.0,
+            'landing_speed': 1.5,
+            'landing_altitude': 10.5}
+        self.speed_parameters.update(**kwargs)
+
+        # Set landing waypoints dimensions
+        idx_land_pos = self.set_landing_waypoints()
+
+        # Set ETAs for waypoints
+        self.set_eta(idx_land_pos=idx_land_pos)
+
+        # Get waypoints in cartesian frame, unix time,
+        # and calculate heading angle for yaw
+        # ----------------------------------------------
+        # Covert to cartesian coordinates
+        self.waypoints['x'], \
+            self.waypoints['y'], \
+            self.waypoints['z'] = self.coordinate_system.geodetic2enu(
+                self.waypoints['lat'],
+                self.waypoints['lon'],
+                self.waypoints['alt'])
+                
+        # Interpolation properties
+        # ========================
+        self.parameters = {'gravity': 9.81,
+                           'max_phi': 45/180.0*np.pi,
+                           'max_theta': 45/180.0*np.pi,
+                           'max_iter': 10,
+                           'max_avgjerk': 20.0,
+                           'nurbs_order': 4,
+                           'waypoint_weight': 20,
+                           'weight_vector': None,
+                           'nurbs_basis_length': 1000}
+        self.parameters.update(**kwargs)
+        if self.parameters['weight_vector'] is None:
+            self.parameters['weight_vector'] = np.asarray([self.parameters['waypoint_weight'],] * len(self.waypoints['x']))
+
+    @property
+    def ref_traj(self,):
+        return {
+            'x': self.trajectory['position'][:, 0],
+            'y': self.trajectory['position'][:, 1],
+            'z': self.trajectory['position'][:, 2],
+        
+            'phi': self.trajectory['attitude'][:, 0],
+            'theta': self.trajectory['attitude'][:, 1],
+            'psi': self.trajectory['attitude'][:, 2],
+        
+            'vx': self.trajectory['velocity'][:, 0],
+            'vy': self.trajectory['velocity'][:, 1],
+            'vz': self.trajectory['velocity'][:, 2],
+        
+            'p': self.trajectory['angVel'][:, 0],
+            'q': self.trajectory['angVel'][:, 1],
+            'r': self.trajectory['angVel'][:, 2],
+            
+            't': self.trajectory['time']}
+    
+    def compute_attitude(self, heading_profile, acceleration_profile, timestep_size):
+        """
+        Compute attitude defined by Euler's angles as a function of time given heading and acceleration profiles.
+        Phi and theta are limited by max_phi, max_theta, and the angular velocities p, q, r are calculated
+        from phi, theta, psi given the time step size.
+
+        :param heading_profile:             n x 1 array, double, heading angle as a function of time
+        :param acceleration_profile:        n x 3 array, double, acceleration along the three cartesian directions as a function of time
+        :param timestep_size:               scalar, double, time step size dt
+        :param max_phi:                     rad, scalar, maximum phi angle possible
+        :param max_theta:                   rad, scalar, maximum theta angle possible
+        :return:                            dictionary containing attitude phi, theta, psi and angular velocity p, q, r, as a function of time.
+        """
+        # get names of cartesian directions
+        dim_keys = list(acceleration_profile.keys())
+
+        # Compute attitude
+        # ---------------
+        phi, theta, psi = gen_attitude(heading_profile,
+                                       acceleration_profile[dim_keys[0]],
+                                       acceleration_profile[dim_keys[1]],
+                                       acceleration_profile[dim_keys[2]],
+                                       self.parameters['max_phi'],
+                                       self.parameters['max_theta'],
+                                       self.parameters['gravity'])
+        # Compute angular velocity
+        # -------------------------
+        p, q, r = angular_vel_from_attitude(phi, theta, psi, timestep_size)
+
+        return {
+            'attitude': np.array([phi, theta, psi]).T,
+            'angVel': np.array([p, q, r]).T}
+
+    def compute_trajectory_nurbs(self, dt):
+        # Compute position and yaw profiles with NURBS
+        # --------------------------------------------
+        # Instantiate NURBS class to generate trajectory
+        points = {
+            'x': self.waypoints['x'],
+            'y': self.waypoints['y'],
+            'z': self.waypoints['z']}
+        nurbs_alg = NURBS(points=points,
+                          weights=self.parameters['weight_vector'],
+                          times=self.waypoints['eta'] - self.waypoints['eta'][0],
+                          yaw=self.waypoints['heading'],
+                          order=self.parameters['nurbs_order'],
+                          basis_length=self.parameters['nurbs_basis_length'])
+        
+        # Generate position and yaw interpolated given the timestep size
+        pos_interp, yaw_interp, time_interp = nurbs_alg.generate(timestep_size=dt)
+        
+        # Generate velocity, acceleration, and jerk (optional) profile from position profile
+        linear_profiles = compute_derivatives(pos_interp, time_interp)
+        
+        # Generate angular profiles: attitude and angular velocities from heading and acceleration
+        angular_profiles = self.compute_attitude(heading_profile=yaw_interp,
+                                                 acceleration_profile=linear_profiles['acceleration'],
+                                                 timestep_size=dt)
+        # Store in trajectory dictionary
+        # ----------------------------
+        self.trajectory = {**{'position': np.vstack(list(pos_interp.values())).T}, 
+                           **{'velocity': np.vstack(list(linear_profiles['velocity'].values())).T}, 
+                           **{'acceleration': np.vstack(list(linear_profiles['acceleration'].values())).T}, 
+                           **angular_profiles,
+                           **{'time': time_interp}}
+
+    def generate(self, dt, **kwargs):
+        """
+        Generate trajectory given the waypoints and desired velocity or ETAs.
+        The function requires the time step size dt, in seconds, used to interpolate the waypoints and generate the trajectory.
+
+        :param dt:          s, scalar, time step size used to interpolate the waypoints and generate the trajectory
+        :return:            dictionary of state variables describing the trajectory as a function of time
+        """
+        self.parameters.update(**kwargs)    # Override NURBS parameters
+        assert len(self.parameters['weight_vector']) == len(self.waypoints['x']), "Length of waypoint weight vector and number of waypoints must coincide."
+
+        self.compute_trajectory_nurbs(dt)     # GENERATE NURBS-BASED TRAJECTORY
+        self.__adjust_eta_given_max_acceleration(dt)    # Adjust profile according to max accelerations
+
+        # Convert trajectory into geodetic coordinates
+        # --------------------------------------------
+        self.trajectory['geodetic_pos'] = np.zeros_like(self.trajectory['position'])
+        self.trajectory['geodetic_pos'][:, 0], \
+            self.trajectory['geodetic_pos'][:, 1], \
+                self.trajectory['geodetic_pos'][:, 2] = self.coordinate_system.enu2geodetic(
+                    self.trajectory['position'][:, 0],
+                    self.trajectory['position'][:, 1],
+                    self.trajectory['position'][:, 2])
+        return self.ref_traj
+
+    def __adjust_eta_given_max_acceleration(self, dt):
+        """
+        Adjusting the trajectory computed by the NURBS algorithm according to whether the maximum 
+        jerk exceeds the limit allowed. This prevents the vehicle to crash because of high accelerations during the flight
+        """
+        maxiter = self.parameters['max_iter']
+        
+        keep_adjusting = True
+        counter = 0
+        positions = {'x': self.waypoints['x'], 'y': self.waypoints['y'], 'z': self.waypoints['z']}
+        etas_rel = self.waypoints['eta'] - self.waypoints['eta'][0]
+
+        while keep_adjusting:
+            m = len(self.trajectory['time'])
+            counter += 1
+            new_eta_rel = etas_rel.copy()    # store eta to be updated, (relative eta)
+            keep_adjusting = False
+            position_values = np.vstack(list(positions.values())).T
+            n = position_values.shape[0]
+            for i in range(n-1):
+                
+                dist1 = geom.euclidean_distance_point_vector(position_values[i, :], self.trajectory['position'])
+                dist2 = geom.euclidean_distance_point_vector(position_values[i+1, :], self.trajectory['position'])
+                
+                delta_etas = etas_rel[i+1] - etas_rel[i]
+                dtime1 = 100.0 * np.abs(etas_rel[i] - self.trajectory['time'])**3.0  # time is fundamental and more important than distance, so I'm using a higher power to take that into account
+                dtime2 = 100.0 * np.abs(etas_rel[i+1] - self.trajectory['time'])**3.0
+
+                accelerations_abs = np.abs(self.trajectory['acceleration'][max(np.argmin(dist1 * dtime1), 0) : min(np.argmin(dist2 * dtime2), m), :])
+
+                if accelerations_abs.size > 0:
+                    acc_max = np.amax(accelerations_abs)
+                    if acc_max/delta_etas > self.parameters['max_avgjerk']:
+                        extra_time = acc_max / self.parameters['max_avgjerk'] + 2.0
+                        new_eta_rel[i+1:] += extra_time
+                        keep_adjusting = True
+
+            if any(etas_rel != new_eta_rel):
+                # Generate new curve and new_eta becomes the reference eta
+                self.waypoints['eta'] = new_eta_rel + self.waypoints['eta'][0]
+                self.compute_trajectory_nurbs(dt)
+                etas_rel = new_eta_rel.copy()
+
+            if counter == maxiter:
+                # Exit the while loop and give up if in maxiter iterations the trajectory acceleration has not been pushed under the limit
+                print("WARNING: max number of iterations reached, the trajectory still contains accelerations beyond the limit.")
+                break
+
+    def set_landing_waypoints(self,):
+        """
+        Set waypoints at altitude defined by landing_altitude.
+        By so doing, the trajectory can split the ascending and descending phases into segments, and assign the landing_speed to all segments that
+        fall below the landing_alt mark.
+        :return:                        int, n x 1 array, indices of waypoints that define the landing.
+        """
+        
+        # get boolean flag where altitude is below the landing altitude, and get the corresponding indices
+        idx_land = np.asarray(self.waypoints['alt'] < self.speed_parameters['landing_altitude'])
+        idx_land_pos = np.where(idx_land)[0]    
+        
+        # if there are waypoints below landing altitude: append a landing point (with altitude self.speed_parameters['landing_altitude']) accordingly.
+        if idx_land_pos.size != 0:
+            n_waypoints = len(self.waypoints['lat'])
+            if hasattr(self.waypoints['eta'], '__len__'):
+                n_times = len(self.waypoints['eta'])
+            else:
+                n_times = 0
+            counter = 0
+            for item in idx_land_pos:
+                if item == 0:   # if first element is below, just add a landing waypoint
+                    self.waypoints['lat'] = np.insert(self.waypoints['lat'], item + 1, self.waypoints['lat'][item])
+                    self.waypoints['lon'] = np.insert(self.waypoints['lon'], item + 1, self.waypoints['lon'][item])
+                    if n_times > 1:
+                        t_land = linearinterp_t(self.waypoints['eta'][item], 
+                                                self.waypoints['alt'][item], 
+                                                self.waypoints['eta'][item+1], 
+                                                self.waypoints['alt'][item+1], # just added the landing altitude, so I need to take the next point
+                                                self.speed_parameters['landing_altitude'])
+                        self.waypoints['eta'] = np.insert(self.waypoints['eta'], item + 1, t_land)
+                    self.waypoints['alt'] = np.insert(self.waypoints['alt'], item + 1, self.speed_parameters['landing_altitude']*1.0)
+                    counter += 1
+
+                elif item == n_waypoints-1: # if one before the last element, add landing waypoints right before landing.
+                    if self.waypoints['alt'][item+counter-1] > self.speed_parameters['landing_altitude']:
+                        self.waypoints['lat'] = np.insert(self.waypoints['lat'], -1, self.waypoints['lat'][item+counter-1])
+                        self.waypoints['lon'] = np.insert(self.waypoints['lon'], -1, self.waypoints['lon'][item+counter-1])
+                        if n_times > 1:
+                            t_land = linearinterp_t(self.waypoints['eta'][item+counter-1], 
+                                                    self.waypoints['alt'][item+counter-1], 
+                                                    self.waypoints['eta'][-1], 
+                                                    self.waypoints['alt'][-1], 
+                                                    self.speed_parameters['landing_altitude'])
+                            self.waypoints['eta'] = np.insert(self.waypoints['eta'], -1, t_land)
+                        self.waypoints['alt'] = np.insert(self.waypoints['alt'], -1, self.speed_parameters['landing_altitude']*1.0)
+                        counter += 1
+                else:
+                    if self.waypoints['alt'][item+counter] - self.waypoints['alt'][item+counter-1] < 0:  
+                        # descending
+                        idx_delta = 0
+                    else:
+                        # ascending
+                        idx_delta = +1
+                    self.waypoints['lat'] = np.insert(self.waypoints['lat'], item+counter + idx_delta, self.waypoints['lat'][item+counter])
+                    self.waypoints['lon'] = np.insert(self.waypoints['lon'], item+counter + idx_delta, self.waypoints['lon'][item+counter])
+                    if n_times > 1:
+                        t_land = linearinterp_t(self.waypoints['eta'][item+counter+idx_delta-1], 
+                                                self.waypoints['alt'][item+counter+idx_delta-1], 
+                                                self.waypoints['eta'][item+counter+idx_delta], 
+                                                self.waypoints['alt'][item+counter+idx_delta], 
+                                                self.speed_parameters['landing_altitude'])
+                        self.waypoints['eta'] = np.insert(self.waypoints['eta'], item+counter+idx_delta, t_land)
+                    self.waypoints['alt'] = np.insert(self.waypoints['alt'], item+counter+idx_delta, self.speed_parameters['landing_altitude']*1.0)
+                    counter += 1
+                    if idx_delta == 0:
+                        # if descended, needs to go back up
+                        if self.waypoints['alt'][item+counter+1] > self.speed_parameters['landing_altitude']:
+                            self.waypoints['lat'] = np.insert(self.waypoints['lat'], item+counter+1, self.waypoints['lat'][item+counter+1])
+                            self.waypoints['lon'] = np.insert(self.waypoints['lon'], item+counter+1, self.waypoints['lon'][item + counter+1])
+                            self.waypoints['alt'] = np.insert(self.waypoints['alt'], item+counter+1, self.speed_parameters['landing_altitude']*1.0)
+                            if self.waypoints['eta'] is not None:
+                                t_land = linearinterp_t(self.waypoints['eta'][item+counter], 
+                                                        self.waypoints['alt'][item+counter], 
+                                                        self.waypoints['eta'][item+counter+1], 
+                                                        self.waypoints['alt'][item+counter+1], 
+                                                        self.speed_parameters['landing_altitude'])
+                                self.waypoints['eta'] = np.insert(self.waypoints['eta'], item+counter+1, t_land)
+                                
+                            counter += 1
+        
+        # Recalculate landing positions with new waypoints:
+        idx_land     = np.asarray(self.waypoints['alt'] < self.speed_parameters['landing_altitude'])
+        idx_land_pos = np.where(idx_land)[0]
+        
+        return idx_land_pos
+    
+    def set_eta(self, idx_land_pos, hovering=0):
+        """
+        Assign ETAs to waypoints
+        If ETAS are provided (i.e., eta is not None), assign them.
+        If they are not provided, compute them from desired speeds.
+        index of the landing waypoints is necessary to reshape speed values.
+
+        :param idx_land_pos:            -, m x 1 array. Index of the added landing waypoints.
+        :param hovering:                s, scalar or n x 1 array. Default = 0. Hovering condition to add to the waypoints.
+        """
+        # Set speed dimensions
+        n = len(self.waypoints['lat'])
+        self.speed_parameters['cruise_speed']  = reshape_route_attribute(self.speed_parameters['cruise_speed'], dim=n-1, msk=idx_land_pos)
+        self.speed_parameters['ascent_speed']  = reshape_route_attribute(self.speed_parameters['ascent_speed'], dim=n-1, msk=idx_land_pos)
+        self.speed_parameters['descent_speed'] = reshape_route_attribute(self.speed_parameters['descent_speed'], dim=n-1, msk=idx_land_pos)
+        self.speed_parameters['landing_speed'] = reshape_route_attribute(self.speed_parameters['landing_speed'], dim=n-1, msk=idx_land_pos)
+        hovering = reshape_route_attribute(hovering, dim=n-1, msk=idx_land_pos)
+
+        if self.waypoints['eta'] is None or len(self.waypoints['eta']) == 1:
+            etas = None
+        else:
+
+            etas = self.waypoints['eta']
+        
+        # Compute ETAs
+        # ============================================
+        if len(self.waypoints['alt']) <= 2:
+            raise ValueError("At least 3 waypoints are required to compute ETAS from speed. Only {} were given.".format(len(self.lat)))
+        
+        # define margin on cruise speed
+        # ----------------------------
+        # If calculated ETA produces a speed that is larger than desired speed, we can accommodate it as long as is within this margin (%)
+        cruise_speed_margin = 0.1  # %, 'extra' speed we can tolerate on cruise.
+        vert_speed_margin = 0.05   # %, 'extra' speed we can tolerate on ascent/descent
+
+        # Compute relative ETAs
+        # -------------------
+        alt_for_land = self.waypoints['alt'][1:]
+        n = len(self.waypoints['alt'])-1
+        
+        if etas is None:
+            d_eta = np.zeros((n,))
+        else:
+            d_eta = np.diff(etas)
+
+        for point in range(n):  # this for loop could be improved by calculating distances all at once.
+            dh, dv = geom.geodetic_distance([self.waypoints['lat'][point], self.waypoints['lat'][point+1]], 
+                                            [self.waypoints['lon'][point], self.waypoints['lon'][point+1]], 
+                                            [self.waypoints['alt'][point], self.waypoints['alt'][point+1]], 
+                                            method='greatcircle', return_surf_vert=True)
+            dv = dv[0]
+            
+            # Identify correct vertical speed
+            # -------------------------------
+            if   dv > 0 and alt_for_land[point] > self.speed_parameters['landing_altitude']:    
+                vert_speed = self.speed_parameters['ascent_speed'][point]
+            elif dv > 0 and alt_for_land[point] <= self.speed_parameters['landing_altitude']:   
+                vert_speed = self.speed_parameters['landing_speed'][point]
+            elif dv < 0 and alt_for_land[point] >= self.speed_parameters['landing_altitude']:   
+                vert_speed = self.speed_parameters['descent_speed'][point]
+            elif dv < 0 and alt_for_land[point] < self.speed_parameters['landing_altitude']:    
+                vert_speed = self.speed_parameters['landing_speed'][point]
+            else:                                                           
+                vert_speed = 0.  # not moving vertically.
+                
+            if etas is None:
+                # Define the correct speed:
+                if np.isclose(dh + dv, 0.0):
+                    d_eta[point] = 2.0  # if there's no vertical / horizontal speed (waypoints are identical) add a default hovering value of 2 s to avoid extreme accelerations.
+                else:
+                    if np.isclose(dh, 0.):
+                        speed_sq = vert_speed*vert_speed
+                    elif np.isclose(dv, 0.):
+                        speed_sq = self.speed_parameters['cruise_speed'][point]**2.0
+                    else:
+                        speed_sq = self.speed_parameters['cruise_speed'][point]**2.0 + vert_speed*vert_speed
+                    d_eta[point] = np.sqrt( (dh**2.0 + dv**2.0) / speed_sq ) * 1.3  # adding some %
+                    
+                    # If speed is larger than desired (possible when both dh, dv>0), increment d_eta to reduce until desired (consider margin)
+                    while dh/d_eta[point] > (self.speed_parameters['cruise_speed'][point]*(1.+cruise_speed_margin)) or dv/d_eta[point] > (vert_speed*(1.+vert_speed_margin)):
+                        d_eta[point] += 2.0
+            else:
+                # If speed is larger than maximum (possible when both dh, dv>0), increment d_eta to reduce until desired (consider margin)
+                while dh / d_eta[point] > (self.speed_parameters['cruise_speed'][point]*(1.+cruise_speed_margin)) or dv / d_eta[point] > (vert_speed*(1.+vert_speed_margin)):
+                    d_eta[point] += 2.0
+
+            # Add hovering if desired
+            if hovering[point] != 0:    
+                d_eta[point] += hovering[point]
+
+        eta_array = np.asarray(np.cumsum(np.insert(d_eta, 0, 0.0)))
+        self.waypoints['eta'] = [eta + self.waypoints['takeoff_time'] for eta in eta_array]
+        
\ No newline at end of file


Test patch:
diff --git a/tests/__main__.py b/tests/__main__.py
index e9b69fe44..143980860 100644
--- a/tests/__main__.py
+++ b/tests/__main__.py
@@ -19,6 +19,7 @@
 from tests.test_serialization import main as serialization_main
 from tests.test_estimate_params import main as estimate_params_main
 from tests.test_ensemble import main as ensemble_main
+from tests.test_uav_model import main as uav_main
 
 if __name__ == '__main__':
     was_successful = True
@@ -114,5 +115,10 @@
     except Exception:
         was_successful = False
 
+    try:
+        uav_main()
+    except Exception:
+        was_successful = False
+
     if not was_successful:
         raise Exception("Failed test")
diff --git a/tests/test_uav_model.py b/tests/test_uav_model.py
new file mode 100644
index 000000000..3c783eeef
--- /dev/null
+++ b/tests/test_uav_model.py
@@ -0,0 +1,289 @@
+# Copyright © 2021 United States Government as represented by the Administrator of the National Aeronautics and Space Administration.  All Rights Reserved.
+
+from io import StringIO
+import sys
+import unittest
+import datetime as dt
+import numpy as np
+from scipy.interpolate import interp1d
+import warnings
+
+from prog_models.utils.traj_gen import Trajectory
+from prog_models.models.aircraft_model import SmallRotorcraft 
+from prog_models.loading.controllers import LQR_I, LQR
+from prog_models.utils.traj_gen import geometry as geom
+
+class TestUAVGen(unittest.TestCase):
+    
+    def setUp(self):
+        # set stdout (so it wont print)
+        sys.stdout = StringIO()
+
+    def tearDown(self):
+        sys.stdout = sys.__stdout__
+
+    def test_reference_trajectory_generation(self):
+
+        # Set warnings to temporarily act as exceptions
+        warnings.simplefilter("error", category=UserWarning)
+
+        # Define vehicle, necessary to pass to ref_traj
+        vehicle = SmallRotorcraft()
+
+        # Define waypoints 
+        waypoints_dict = {}
+        waypoints_dict['lat_deg']   = np.array([37.09776, 37.09776, 37.09776, 37.09798, 37.09748, 37.09665, 37.09703, 37.09719, 37.09719])
+        waypoints_dict['lon_deg']   = np.array([-76.38631, -76.38629, -76.38629, -76.38589, -76.3848, -76.38569, -76.38658, -76.38628, -76.38628])
+        waypoints_dict['alt_ft']    = np.array([-1.9682394, 164.01995, 164.01995, 164.01995, 164.01995, 164.01995, 164.01995, 164.01995, 0.0])
+        waypoints_dict['time_unix'] = [1544188336, 1544188358, 1544188360, 1544188377, 1544188394, 1544188411, 1544188428, 1544188496, 1544188539]
+        
+        lat_in = waypoints_dict['lat_deg'] * np.pi/180.0
+        lon_in = waypoints_dict['lon_deg'] * np.pi/180.0
+        alt_in = waypoints_dict['alt_ft'] * 0.3048
+        etas_in = waypoints_dict['time_unix']
+        takeoff_time = etas_in[0]
+        etas_wrong = [dt.datetime.fromtimestamp(waypoints_dict['time_unix'][ii]) for ii in range(len(waypoints_dict['time_unix']))]
+
+        lat_small = np.array([lat_in[0]])
+        lon_small = np.array([lon_in[0]])
+        alt_small = np.array([alt_in[0]])
+        etas_small = [etas_in[0]]
+
+        # Incorrect number of input arguments to Trajectory
+        with self.assertRaises(TypeError):
+            # No waypoint information provided
+            ref_traj = Trajectory()
+        with self.assertRaises(TypeError):
+            # Only subset of waypoint information provided
+            ref_traj = Trajectory(lon=lon_in, alt=alt_in, takeoff_time = takeoff_time, etas=etas_in)
+        with self.assertRaises(TypeError):
+            # Only subset of waypoint information provided
+            ref_traj = Trajectory(lat=lat_in, alt=alt_in, takeoff_time = takeoff_time, etas=etas_in)
+        with self.assertRaises(TypeError):
+            # Only subset of waypoint information provided
+            ref_traj = Trajectory(lat=lat_in, lon=lon_in, takeoff_time = takeoff_time, etas=etas_in)
+
+        # Waypoints defined incorrectly 
+        # Wrong type for waypoints 
+        with self.assertRaises(TypeError):
+            # Waypoints defined incorrectly; must be numpy arrays
+            ref_traj = Trajectory(lat='a', lon=lon_in, alt=alt_in, takeoff_time = takeoff_time, etas=etas_in)
+        with self.assertRaises(TypeError):
+            # Waypoints defined incorrectly; must be numpy arrays
+            ref_traj = Trajectory(lat=lat_in, lon=[1, 2, 3], alt=alt_in, takeoff_time = takeoff_time, etas=etas_in)
+        with self.assertRaises(TypeError):
+            # Waypoints defined incorrectly; must be numpy arrays
+            ref_traj = Trajectory(lat=lat_in, lon=lon_in, alt=1, takeoff_time = takeoff_time, etas=etas_in)
+        with self.assertRaises(TypeError):
+            # Waypoints defined incorrectly; must be numpy arrays
+            ref_traj = Trajectory(lat={'lat': 1}, lon=lon_in, alt=alt_in, takeoff_time = takeoff_time, etas=etas_in)
+        with self.assertRaises(TypeError):
+            # Waypoints defined incorrectly; takeoff_time must be float/int
+            ref_traj = Trajectory(lat=lat_in, lon=lon_in, alt=alt_in, takeoff_time = np.array([1]), etas=etas_in)
+        with self.assertRaises(TypeError):
+            # Waypoints defined incorrectly; takeoff_time must be float/int
+            ref_traj = Trajectory(lat=lat_in, lon=lon_in, alt=alt_in, takeoff_time = 'abc', etas=etas_in)
+        with self.assertRaises(TypeError):
+            # Waypoints defined incorrectly; etas must be list of float/int
+            ref_traj = Trajectory(lat=lat_in, lon=lon_in, alt=alt_in, takeoff_time = takeoff_time, etas=etas_wrong)
+        with self.assertRaises(TypeError):
+            # Waypoints defined incorrectly; etas must be list of float/int
+            ref_traj = Trajectory(lat=lat_in, lon=lon_in, alt=alt_in, takeoff_time = takeoff_time, etas=np.array([1, 2, 3]))
+
+        # Wrong lengths for waypoints
+        with self.assertRaises(ValueError):
+            ref_traj = Trajectory(lat=lat_in[:5], lon=lon_in, alt=alt_in, takeoff_time = takeoff_time, etas=etas_in)      
+        with self.assertRaises(ValueError):
+            ref_traj = Trajectory(lat=lat_in, lon=lon_in[2:4], alt=alt_in, takeoff_time = takeoff_time, etas=etas_in)
+        with self.assertRaises(ValueError):
+            ref_traj = Trajectory(lat=lat_in, lon=lon_in, alt=alt_in[1:7], takeoff_time = takeoff_time, etas=etas_in)         
+        with self.assertRaises(ValueError):
+            ref_traj = Trajectory(lat=lat_in, lon=lon_in, alt=alt_in, takeoff_time = takeoff_time, etas=etas_in[:5]) 
+        with self.assertRaises(ValueError):
+            ref_traj = Trajectory(lat=lat_small, lon=lon_small, alt=alt_small, etas=etas_small, takeoff=takeoff_time)
+
+        # Checking correct combination of ETAs and speeds
+        with self.assertRaises(UserWarning):
+            # No ETAs or speeds provided, warning is thrown
+            ref_traj = Trajectory(lat=lat_in, lon=lon_in, alt=alt_in, takeoff_time = takeoff_time)
+        with self.assertRaises(UserWarning):
+            # Both ETAs and spees provided, warning is thrown
+            params = {'cruise_speed': 1, 'descent_speed': 1, 'ascent_speed': 1, 'landing_speed': 1}
+            ref_traj = Trajectory(lat=lat_in, lon=lon_in, alt=alt_in, takeoff_time = takeoff_time, etas=etas_in, **params)
+
+        # Test trajectory generation functionality is generating an accurate result
+        # Convert waypoints to Cartesian 
+        DEG2RAD = np.pi/180.0
+        FEET2MET = 0.3048
+        coord = geom.Coord(lat0=waypoints_dict['lat_deg'][0]*DEG2RAD, lon0=waypoints_dict['lon_deg'][0]*DEG2RAD, alt0=waypoints_dict['alt_ft'][0]*FEET2MET)
+        x_ref, y_ref, z_ref = coord.geodetic2enu(waypoints_dict['lat_deg']*DEG2RAD, waypoints_dict['lon_deg']*DEG2RAD, waypoints_dict['alt_ft']*FEET2MET)
+        time_ref = [waypoints_dict['time_unix'][iter] - waypoints_dict['time_unix'][0] for iter in range(len(waypoints_dict['time_unix']))]
+
+        # Generate trajectory
+        vehicle.parameters['dt'] = 1
+        traj = Trajectory(lat=lat_in, lon=lon_in, alt=alt_in, etas=etas_in, takeoff_time=takeoff_time)
+        ref_traj_test = traj.generate(dt=vehicle.parameters['dt'])
+
+        # Check that generated trajectory is close to waypoints 
+        for ind, val in enumerate(time_ref):
+            # Since dt is 1, the time values in ref_traj_test['t'] correspond to their index number
+            self.assertAlmostEqual(x_ref[ind], ref_traj_test['x'][val], delta=45)
+            self.assertAlmostEqual(y_ref[ind], ref_traj_test['y'][val], delta=45)
+            self.assertAlmostEqual(z_ref[ind], ref_traj_test['z'][val], delta=45)
+
+        # Reset warnings
+        warnings.simplefilter("default", category=UserWarning)
+
+    def test_controllers_and_vehicle(self):
+
+        # Controller and vehicle tests are combined so we only have to generate the reference trajectory once 
+
+        # Set warnings to temporarily act as exceptions
+        warnings.simplefilter("error", category=UserWarning)
+
+        # Instantiate vehicles - one for each configuration
+        vehicle = SmallRotorcraft(**{'dt': 0.1})
+        vehicle_djis = SmallRotorcraft(**{'dt': 0.1, 'vehicle_model': 'djis1000'})
+
+        # Define waypoints 
+        waypoints_dict = {}
+        waypoints_dict['lat_deg']   = np.array([37.09776, 37.09776, 37.09776, 37.09798, 37.09748, 37.09665, 37.09703, 37.09719, 37.09719])
+        waypoints_dict['lon_deg']   = np.array([-76.38631, -76.38629, -76.38629, -76.38589, -76.3848, -76.38569, -76.38658, -76.38628, -76.38628])
+        waypoints_dict['alt_ft']    = np.array([-1.9682394, 164.01995, 164.01995, 164.01995, 164.01995, 164.01995, 164.01995, 164.01995, 0.0])
+        waypoints_dict['time_unix'] = [1544188336, 1544188358, 1544188360, 1544188377, 1544188394, 1544188411, 1544188428, 1544188496, 1544188539]
+        
+        lat_in = waypoints_dict['lat_deg'] * np.pi/180.0
+        lon_in = waypoints_dict['lon_deg'] * np.pi/180.0
+        alt_in = waypoints_dict['alt_ft'] * 0.3048
+        etas_in = waypoints_dict['time_unix']
+
+        # Generate reference trajectory
+        ref_traj_temp = Trajectory(lat=lat_in, lon=lon_in, alt=alt_in, etas=etas_in)
+        ref_traj = ref_traj_temp.generate(dt=vehicle.parameters['dt'])
+
+        # Controller tests: LQR
+        # -----------------
+        # Testing incorrect arguments:
+        with self.assertRaises(TypeError):
+            # Reference trajectory not defined as dict
+            ctrl = LQR(x_ref='abc', vehicle=vehicle)
+        with self.assertRaises(TypeError):
+            # Reference trajectory not defined as dict
+            ctrl = LQR(x_ref=[1, 2, 3], vehicle=vehicle)
+        with self.assertRaises(TypeError):
+            # Incorrect type for values in reference trajectory dict
+            ctrl = LQR(x_ref={'x': [1, 2, 3]}, vehicle=vehicle)
+        with self.assertRaises(TypeError):
+            # Incorrect type for values in reference trajectory dict
+            ctrl = LQR(x_ref={'x': np.array([1, 2, 3]), 'y': [1, 2, 3]}, vehicle=vehicle)
+        with self.assertRaises(TypeError):
+            # No vehicle model given
+            ctrl = LQR(x_ref=ref_traj)
+        with self.assertRaises(TypeError):
+            # No reference trajectory given
+            ctrl = LQR(vehicle=vehicle)
+
+        # Generate Controller
+        ctrl = LQR(x_ref=ref_traj, vehicle=vehicle)
+
+        # Controller tests: LQR_I
+        # -----------------
+        # Testing incorrect arguments:
+        with self.assertRaises(TypeError):
+            # Reference trajectory not defined as dict
+            ctrl = LQR_I(x_ref='abc', vehicle=vehicle)
+        with self.assertRaises(TypeError):
+            # Reference trajectory not defined as dict
+            ctrl = LQR_I(x_ref=[1, 2, 3], vehicle=vehicle)
+        with self.assertRaises(TypeError):
+            # Incorrect type for values in reference trajectory dict
+            ctrl = LQR_I(x_ref={'x': [1, 2, 3]}, vehicle=vehicle)
+        with self.assertRaises(TypeError):
+            # Incorrect type for values in reference trajectory dict
+            ctrl = LQR_I(x_ref={'x': np.array([1, 2, 3]), 'y': [1, 2, 3]}, vehicle=vehicle)
+        with self.assertRaises(TypeError):
+            # No vehicle model given
+            ctrl = LQR_I(x_ref=ref_traj)
+        with self.assertRaises(TypeError):
+            # No reference trajectory given
+            ctrl = LQR_I(vehicle=vehicle)
+
+        # Generate Controller
+        ctrl_I = LQR_I(x_ref=ref_traj, vehicle=vehicle)
+
+        # Testing appropriate input parameters: 
+        with self.assertRaises(ValueError):
+            vehicle_wrong = SmallRotorcraft(**{'vehicle_model': 'fakemodel'})
+        with self.assertRaises(TypeError):
+            vehicle_wrong = SmallRotorcraft(**{'vehicle_model': 1})
+        with self.assertRaises(TypeError):
+            vehicle_wrong = SmallRotorcraft(**{'vehicle_model': [1, 2, 3]})
+        with self.assertRaises(TypeError):
+            vehicle_wrong = SmallRotorcraft(**{'vehicle_model': np.array([1, 2, 3])})
+
+        # Test simulation parameters:
+        with self.assertRaises(UserWarning):
+            # dt value for simulation must match dt provided to vehicle
+            sim = vehicle.simulate_to(100, ctrl, **{'dt': 2})
+
+        # Run simulation and verify accuracy of simulated trajectory:
+        # Case 1: ETAs provided + LQR controller + tarot18 vehicle 
+        # Use above ref_traj, ctrl
+        # Simulate and compare
+        sim = vehicle.simulate_to(ref_traj['t'][-1], ctrl, **{'save_freq': vehicle.parameters['dt']})
+        x_temp = [sim.outputs[iter]['x'] for iter in range(len(sim.times))]
+        y_temp = [sim.outputs[iter]['y'] for iter in range(len(sim.times))]
+        z_temp = [sim.outputs[iter]['z'] for iter in range(len(sim.times))]
+        x_sim_interp = interp1d(sim.times, x_temp)(ref_traj['t'])
+        y_sim_interp = interp1d(sim.times, y_temp)(ref_traj['t'])
+        z_sim_interp = interp1d(sim.times, z_temp)(ref_traj['t'])
+        for iter in range(min(len(sim.times), len(ref_traj['t']))):
+            self.assertAlmostEqual(ref_traj['x'][iter], sim.outputs[iter]['x'], delta=5)
+            self.assertAlmostEqual(ref_traj['y'][iter], sim.outputs[iter]['y'], delta=5)
+            self.assertAlmostEqual(ref_traj['z'][iter], sim.outputs[iter]['z'], delta=5)
+
+        # Case 2: Speeds provided, no ETAs + LQR_I controller + djis1000 vehicle
+        # Define speeds:
+        ref_params = {
+            'nurbs_order': 4,
+            'cruise_speed': 8.0,
+            'ascent_speed': 2.0,
+            'descent_speed': 3.0,
+            'landing_speed': 2,
+        }
+
+        # Generate reference trajectory
+        ref_traj_speeds_temp = Trajectory(lat=lat_in, lon=lon_in, alt=alt_in, **ref_params)
+        ref_traj_speeds = ref_traj_speeds_temp.generate(dt=vehicle_djis.parameters['dt'])
+
+        # Build controller
+        ctrl_speeds = LQR_I(x_ref=ref_traj_speeds, vehicle=vehicle_djis)
+
+        # Simulate and compare:
+        sim_speeds = vehicle_djis.simulate_to(ref_traj_speeds['t'][-1], ctrl_speeds, **{'save_freq': vehicle_djis.parameters['dt']})
+        x_speeds_temp = [sim_speeds.outputs[iter]['x'] for iter in range(len(sim_speeds.times))]
+        y_speeds_temp = [sim_speeds.outputs[iter]['y'] for iter in range(len(sim_speeds.times))]
+        z_speeds_temp = [sim_speeds.outputs[iter]['z'] for iter in range(len(sim_speeds.times))]
+        x_speeds_sim_interp = interp1d(sim_speeds.times, x_speeds_temp)(ref_traj_speeds['t'])
+        y_speeds_sim_interp = interp1d(sim_speeds.times, y_speeds_temp)(ref_traj_speeds['t'])
+        z_speeds_sim_interp = interp1d(sim_speeds.times, z_speeds_temp)(ref_traj_speeds['t'])
+        for iter in range(len(sim_speeds.times)):
+            self.assertAlmostEqual(ref_traj_speeds['x'][iter], sim_speeds.outputs[iter]['x'], delta=8)
+            self.assertAlmostEqual(ref_traj_speeds['y'][iter], sim_speeds.outputs[iter]['y'], delta=8)
+            self.assertAlmostEqual(ref_traj_speeds['z'][iter], sim_speeds.outputs[iter]['z'], delta=8)
+
+        # Reset warnings
+        warnings.simplefilter("default", category=UserWarning)
+    
+def main():
+    l = unittest.TestLoader()
+    runner = unittest.TextTestRunner()
+    print("\n\nTesting Trajectory Generation model")
+    result = runner.run(l.loadTestsFromTestCase(TestUAVGen)).wasSuccessful()
+
+    if not result:
+        raise Exception("Failed test")
+
+if __name__ == '__main__':
+    main()
+