initial commit LTV_MPC

MPC.py

@@ -1,31 +1,32 @@
 import numpy as np
 import cvxpy as cp
-
+import osqp
+import scipy as sp
+from scipy import sparse
 
 ##################
 # MPC Controller #
 ##################
 
 class MPC:
-    def __init__(self, model, T, Q, R, Qf, StateConstraints, InputConstraints,
+    def __init__(self, model, N, Q, R, QN, StateConstraints, InputConstraints):
-                 Reference):
         """
         Constructor for the Model Predictive Controller.
         :param model: bicycle model object to be controlled
         :param T: time horizon | int
         :param Q: state cost matrix
         :param R: input cost matrix
-        :param Qf: final state cost matrix
+        :param QN: final state cost matrix
         :param StateConstraints: dictionary of state constraints
         :param InputConstraints: dictionary of input constraints
         :param Reference: reference values for state variables
         """
 
         # Parameters
-        self.T = T  # horizon
+        self.N = N  # horizon
         self.Q = Q  # weight matrix state vector
         self.R = R  # weight matrix input vector
-        self.Qf = Qf  # weight matrix terminal
+        self.QN = QN  # weight matrix terminal
 
         # Model
         self.model = model
@@ -34,11 +35,7 @@ class MPC:
         self.state_constraints = StateConstraints
         self.input_constraints = InputConstraints
 
-        # Reference
-        self.reference = Reference
-
         # Current control and prediction
-        self.current_control = None
         self.current_prediction = None
 
         # Initialize Optimization Problem
@@ -50,111 +47,71 @@ class MPC:
         time step.
         """
 
-        # Instantiate optimization variables
+        # number of input and state variables
-        self.x = cp.Variable((self.model.n_states+1, self.T + 1))
+        nx = self.model.n_states
-        self.u = cp.Variable((2, self.T))
+        nu = 1
 
-        # Instantiate optimization parameters
+        # system matrices
-        self.kappa = cp.Parameter(self.T+1)
+        self.A = cp.Parameter(shape=(nx, nx*self.N))
-        self.x_0 = cp.Parameter(self.model.n_states+1, 1)
+        self.B = cp.Parameter(shape=(nx, nu*self.N))
-        self.A = cp.Parameter(self.model.A.shape)
+        self.A.value = np.zeros(self.A.shape)
-        self.B = cp.Parameter(self.model.B.shape)
+        self.B.value = np.zeros(self.B.shape)
 
-        # Initialize cost
+        # reference values
-        cost = 0
+        xr = np.array([0., 0., -1.0])
+        self.ur = cp.Parameter((nu, self.N))
+        self.ur.value = np.zeros(self.ur.shape)
 
-        # Initialize constraints
+        # constraints
-        constraints = [self.x[:, 0] == self.x_0]
+        umin = self.input_constraints['umin']
+        umax = self.input_constraints['umax']
+        xmin = self.state_constraints['xmin']
+        xmax = self.state_constraints['xmax']
 
-        for t in range(self.T):
+        # initial state
+        self.x_init = cp.Parameter(self.model.n_states)
 
-            # set dynamic constraints
+        # Define problem
-            constraints += [self.x[:-1, t + 1] == self.A[:-1, :]
+        self.u = cp.Variable((nu, self.N))
-                            @ self.x[:, t] + self.B[:-1, :] @ self.u[:, t],
+        self.x = cp.Variable((nx, self.N + 1))
-                            self.x[-1, t + 1] == self.kappa[t+1]]
+        objective = 0
-
+        constraints = [self.x[:, 0] == self.x_init]
-            # set input constraints
+        for n in range(self.N):
-            inputs = ['D', 'delta']
+            objective += cp.quad_form(self.x[:, n] - xr, self.Q) + cp.quad_form(self.u[:, n] - self.ur[:, n], self.R)
-            for input_name, constraint in self.input_constraints.items():
+            constraints += [self.x[:, n + 1] == self.A[:, n*nx:n*nx+nx] * self.x[:, n]
-                input_id = inputs.index(input_name)
+                            + self.B[:, n*nu] * (self.u[:, n] - self.ur[:, n])]
-                if constraint[0] is not None:
+            constraints += [umin <= self.u[:, n], self.u[:, n] <= umax]
-                    constraints.append(-self.u[input_id, t] <= -constraint[0])
+        objective += cp.quad_form(self.x[:, self.N] - xr, self.QN)
-                if constraint[1] is not None:
+        constraints += [xmin <= self.x[:, self.N], self.x[:, self.N] <= xmax]
-                    constraints.append(self.u[input_id, t] <= constraint[1])
+        problem = cp.Problem(cp.Minimize(objective), constraints)
-
-            # Set state constraints
-            for state_name, constraint in self.state_constraints.items():
-                state_id = self.model.spatial_state.list_states(). \
-                    index(state_name)
-                if constraint[0] is not None:
-                    constraints.append(-self.x[state_id, t] <= -constraint[0])
-                if constraint[1] is not None:
-                    constraints.append(self.x[state_id, t] <= constraint[1])
-
-            # update cost function for states
-            for state_name, state_reference in self.reference.items():
-                state_id = self.model.spatial_state.list_states(). \
-                    index(state_name)
-                cost += cp.norm(self.x[state_id, t] - state_reference, 2) * self.Q[
-                    state_id, state_id]
-
-            # update cost function for inputs
-            cost += cp.norm(self.u[0, t], 2) * self.R[0, 0]
-            cost += cp.norm(self.u[1, t], 2) * self.R[1, 1]
-
-        # set state constraints
-        for state_name, constraint in self.state_constraints.items():
-            state_id = self.model.spatial_state.list_states(). \
-                index(state_name)
-            if constraint[0] is not None:
-                constraints.append(-self.x[state_id, self.T] <= -constraint[0])
-            if constraint[1] is not None:
-                constraints.append(self.x[state_id, self.T] <= constraint[1])
-
-        # update cost function for states
-        for state_name, state_reference in self.reference.items():
-            state_id = self.model.spatial_state.list_states(). \
-                index(state_name)
-            cost += cp.norm(self.x[state_id, self.T] - state_reference, 2) * \
-                    self.Qf[state_id, state_id]
-
-        # sums problem objectives and concatenates constraints.
-        problem = cp.Problem(cp.Minimize(cost), constraints)
 
         return problem
 
-    def get_control(self):
+    def get_control(self, v):
         """
         Get control signal given the current position of the car. Solves a
         finite time optimization problem based on the linearized car model.
         """
 
-        # get current waypoint curvature
+        nx = self.model.n_states
-        kappa_ref = [wp.kappa for wp in self.model.reference_path.waypoints
+        nu = 1
-        [self.model.wp_id:self.model.wp_id+self.T+1]]
 
-        # Instantiate optimization parameters
+        for n in range(self.N):
-        self.kappa.value = kappa_ref
+            current_waypoint = self.model.reference_path.waypoints[self.model.wp_id+n]
-        self.x_0.value = np.array(self.model.spatial_state[:] + [kappa_ref[0]])
+            next_waypoint = self.model.reference_path.waypoints[
-        self.A.value = self.model.A
+                self.model.wp_id + n + 1]
-        self.B.value = self.model.B
+            delta_s = next_waypoint - current_waypoint
+            kappa_r = current_waypoint.kappa
+            self.A.value[:, n*nx:n*nx+nx], self.B.value[:, n*nu:n*nu+nu] = self.model.linearize(v, kappa_r, delta_s)
+            self.ur.value[:, n] = kappa_r
 
-        # Solve optimization problem
+        self.x_init.value = np.array(self.model.spatial_state[:])
-        self.problem.solve(solver=cp.ECOS, warm_start=True)
+        self.problem.solve(solver=cp.OSQP, verbose=True)
 
-        # Store computed control signals and associated prediction
-        try:
-            self.current_control = self.u.value
         self.current_prediction = self.update_prediction(self.x.value)
-        except TypeError:
+        delta = np.arctan(self.u.value[0, 0] * self.model.l)
-            print('No solution found!')
-            exit(1)
 
-        # RCH - get first control signal
+        return delta
-        D_0 = self.u.value[0, 0]
-        delta_0 = self.u.value[1, 0]
-
-        return D_0, delta_0
 
     def update_prediction(self, spatial_state_prediction):
         """
@@ -168,12 +125,180 @@ class MPC:
         x_pred, y_pred = [], []
 
         # get current waypoint ID
-        wp_id_ = np.copy(self.model.wp_id)
+        print('#########################')
 
-        for t in range(self.T):
+        for n in range(self.N):
-            associated_waypoint = self.model.reference_path.waypoints[wp_id_+t]
+            associated_waypoint = self.model.reference_path.waypoints[self.model.wp_id+n]
             predicted_temporal_state = self.model.s2t(associated_waypoint,
-                                            spatial_state_prediction[:, t])
+                                            spatial_state_prediction[:, n])
+            print('delta: ', np.arctan(self.u.value[0, n] * self.model.l))
+            print('e_y: ', spatial_state_prediction[0, n])
+            print('e_psi: ', spatial_state_prediction[1, n])
+            print('t: ', spatial_state_prediction[2, n])
+            print('+++++++++++++++++++++++')
+
+            x_pred.append(predicted_temporal_state.x)
+            y_pred.append(predicted_temporal_state.y)
+
+        return x_pred, y_pred
+
+
+class MPC_OSQP:
+    def __init__(self, model, N, Q, R, QN, StateConstraints, InputConstraints):
+        """
+        Constructor for the Model Predictive Controller.
+        :param model: bicycle model object to be controlled
+        :param T: time horizon | int
+        :param Q: state cost matrix
+        :param R: input cost matrix
+        :param QN: final state cost matrix
+        :param StateConstraints: dictionary of state constraints
+        :param InputConstraints: dictionary of input constraints
+        :param Reference: reference values for state variables
+        """
+
+        # Parameters
+        self.N = N  # horizon
+        self.Q = Q  # weight matrix state vector
+        self.R = R  # weight matrix input vector
+        self.QN = QN  # weight matrix terminal
+
+        # Model
+        self.model = model
+
+        # Constraints
+        self.state_constraints = StateConstraints
+        self.input_constraints = InputConstraints
+
+        # Current control and prediction
+        self.current_prediction = None
+
+        # Initialize Optimization Problem
+        self.optimizer = osqp.OSQP()
+
+    def _init_problem(self, v):
+        """
+        Initialize optimization problem for current time step.
+        """
+
+        # Number of state and input variables
+        nx = self.model.n_states
+        nu = 1
+
+        # Constraints
+        umin = self.input_constraints['umin']
+        umax = self.input_constraints['umax']
+        xmin = self.state_constraints['xmin']
+        xmax = self.state_constraints['xmax']
+
+        # LTV System Matrices
+        A = np.zeros((nx * (self.N + 1), nx * (self.N + 1)))
+        B = np.zeros((nx * (self.N + 1), nu * (self.N)))
+        # Reference vector for state and input variables
+        ur = np.zeros(self.N)
+        xr = np.array([0.0, 0.0, -1.0])
+        # Offset for equality constraint (due to B * (u - ur))
+        uq = np.zeros(self.N * nx)
+
+        # Iterate over horizon
+        for n in range(self.N):
+
+            # Get information about current waypoint
+            current_waypoint = self.model.reference_path.waypoints[
+                self.model.wp_id + n]
+            next_waypoint = self.model.reference_path.waypoints[
+                self.model.wp_id + n + 1]
+            delta_s = next_waypoint - current_waypoint
+            kappa_r = current_waypoint.kappa
+
+            # Compute LTV matrices
+            A_lin, B_lin = self.model.linearize(v, kappa_r, delta_s)
+            A[nx + n * nx:n * nx + 2 * nx, n * nx:n * nx + nx] = A_lin
+            B[nx + n * nx:n * nx + 2 * nx, n * nu:n * nu + nu] = B_lin
+
+            # Set kappa_r to reference for input signal
+            ur[n] = kappa_r
+            # Compute equality constraint offset (B*ur)
+            uq[n * nx:n * nx + nx] = B_lin[:, 0] * kappa_r
+
+        # Get equality matrix
+        Ax = sparse.kron(sparse.eye(self.N + 1),
+                         -sparse.eye(nx)) + sparse.csc_matrix(A)
+        Bu = sparse.csc_matrix(B)
+        Aeq = sparse.hstack([Ax, Bu])
+        # Get inequality matrix
+        Aineq = sparse.eye((self.N + 1) * nx + self.N * nu)
+        # Combine constraint matrices
+        A = sparse.vstack([Aeq, Aineq], format='csc')
+
+        # Get upper and lower bound vectors for equality constraints
+        lineq = np.hstack([np.kron(np.ones(self.N + 1), xmin),
+             np.kron(np.ones(self.N), umin)])
+        uineq = np.hstack([np.kron(np.ones(self.N + 1), xmax),
+             np.kron(np.ones(self.N), umax)])
+        # Get upper and lower bound vectors for inequality constraints
+        x0 = np.array(self.model.spatial_state[:])
+        leq = np.hstack([-x0, uq])
+        ueq = leq
+        # Combine upper and lower bound vectors
+        l = np.hstack([leq, lineq])
+        u = np.hstack([ueq, uineq])
+
+        # Set cost matrices
+        P = sparse.block_diag([sparse.kron(sparse.eye(self.N), self.Q), self.QN,
+             sparse.kron(sparse.eye(self.N), self.R)], format='csc')
+        q = np.hstack(
+            [np.kron(np.ones(self.N), -self.Q.dot(xr)), -self.QN.dot(xr),
+             -self.R.A[0, 0] * ur])
+
+        # Initialize optimizer
+        self.optimizer = osqp.OSQP()
+        self.optimizer.setup(P=P, q=q, A=A, l=l, u=u, verbose=False)
+
+    def get_control(self, v):
+        """
+        Get control signal given the current position of the car. Solves a
+        finite time optimization problem based on the linearized car model.
+        """
+
+        # Number of state variables
+        nx = self.model.n_states
+
+        # Initialize optimization problem
+        self._init_problem(v)
+
+        # Solve optimization problem
+        dec = self.optimizer.solve()
+        x = np.reshape(dec.x[:(self.N+1)*nx], (self.N+1, nx))
+        u = np.arctan(dec.x[-self.N] * self.model.l)
+        self.current_prediction = self.update_prediction(u, x)
+
+        return u
+
+    def update_prediction(self, u, spatial_state_prediction):
+        """
+        Transform the predicted states to predicted x and y coordinates.
+        Mainly for visualization purposes.
+        :param spatial_state_prediction: list of predicted state variables
+        :return: lists of predicted x and y coordinates
+        """
+
+        # containers for x and y coordinates of predicted states
+        x_pred, y_pred = [], []
+
+        # get current waypoint ID
+        print('#########################')
+
+        for n in range(self.N):
+            associated_waypoint = self.model.reference_path.waypoints[self.model.wp_id+n]
+            predicted_temporal_state = self.model.s2t(associated_waypoint,
+                                            spatial_state_prediction[n, :])
+            print('delta: ', u)
+            print('e_y: ', spatial_state_prediction[n, 0])
+            print('e_psi: ', spatial_state_prediction[n, 1])
+            print('t: ', spatial_state_prediction[n, 2])
+            print('+++++++++++++++++++++++')
+
             x_pred.append(predicted_temporal_state.x)
             y_pred.append(predicted_temporal_state.y)
 

simulation.py

@@ -1,17 +1,17 @@
 from map import Map
 import numpy as np
 from reference_path import ReferencePath
-from spatial_bicycle_models import SimpleBicycleModel, ExtendedBicycleModel
+from spatial_bicycle_models import BicycleModel
 import matplotlib.pyplot as plt
-from MPC import MPC
+from MPC import MPC, MPC_OSQP
-from time import time
+from scipy import sparse
+import time
+
 
 if __name__ == '__main__':
 
     # Select Simulation Mode | 'Race' or 'Q'
     sim_mode = 'Race'
-    # Select Model Type | 'Simple' or 'Extended'
-    model_type = 'Simple'
 
     # Create Map
     if sim_mode == 'Race':
@@ -45,46 +45,23 @@ if __name__ == '__main__':
     # Initial state
     e_y_0 = 0.0
     e_psi_0 = 0.0
-    v_x_0 = 0.1
+    t_0 = 0.0
-    v_y_0 = 0
+    v = 1.0
-    omega_0 = 0
-    t_0 = 0
 
-    if model_type == 'Extended':
+    car = BicycleModel(reference_path=reference_path,
-        car = ExtendedBicycleModel(reference_path=reference_path,
+                                e_y=e_y_0, e_psi=e_psi_0, t=t_0)
-                             e_y=e_y_0, e_psi=e_psi_0, v_x=v_x_0, v_y=v_y_0,
-                               omega=omega_0, t=t_0)
-    elif model_type == 'Simple':
-        car = SimpleBicycleModel(reference_path=reference_path,
-                                e_y=e_y_0, e_psi=e_psi_0, v=v_x_0)
-    else:
-        car = None
-        print('Invalid Model Type!')
-        exit(1)
 
     ##############
     # Controller #
     ##############
 
-    if model_type == 'Extended':
+    N = 20
-        Q = np.diag([1, 0, 0, 0, 0, 0])
+    Q = sparse.diags([0.01, 0.0, 0.4])
-        Qf = Q
+    R = sparse.diags([0.01])
-        R = np.diag([0, 0])
+    QN = Q
-        Reference = {'e_y': 0, 'e_psi': 0, 'v_x': 1.0, 'v_y': 0, 'omega': 0, 't':0}
+    InputConstraints = {'umin': np.array([-np.tan(0.44)/car.l]), 'umax': np.array([np.tan(0.44)/car.l])}
-    elif model_type == 'Simple':
+    StateConstraints = {'xmin': np.array([-0.2, -np.inf, 0]), 'xmax': np.array([0.2, np.inf, np.inf])}
-        Reference = {'e_y': 0, 'e_psi': 0, 'v': 4.0}
+    mpc = MPC_OSQP(car, N, Q, R, QN, StateConstraints, InputConstraints)
-        Q = np.diag([0.0005, 0.05, 0.5])
-        Qf = Q
-        R = np.diag([0, 0])
-    else:
-        Q, Qf, R, Reference = None, None, None, None
-        print('Invalid Model Type!')
-        exit(1)
-
-    T = 5
-    StateConstraints = {'e_y': (-0.2, 0.2), 'v': (0, 4)}
-    InputConstraints = {'D': (-1, 1), 'delta': (-0.44, 0.44)}
-    mpc = MPC(car, T, Q, R, Qf, StateConstraints, InputConstraints, Reference)
 
     ##############
     # Simulation #
@@ -93,26 +70,22 @@ if __name__ == '__main__':
     # logging containers
     x_log = [car.temporal_state.x]
     y_log = [car.temporal_state.y]
-    psi_log = [car.temporal_state.psi]
-    v_log = [car.temporal_state.v_x]
-    D_log = []
-    delta_log = []
 
     # iterate over waypoints
-    for wp_id in range(len(car.reference_path.waypoints)-T-1):
+    for wp_id in range(len(car.reference_path.waypoints)-mpc.N-1):
 
         # get control signals
-        D, delta = mpc.get_control()
+        start = time.time()
+        delta = mpc.get_control(v)
+        end = time.time()
+        u = np.array([v, delta])
 
         # drive car
-        car.drive(D, delta)
+        car.drive(u)
 
-        # log current state
+        # log
         x_log.append(car.temporal_state.x)
         y_log.append(car.temporal_state.y)
-        v_log.append(car.temporal_state.v_x)
-        D_log.append(D)
-        delta_log.append(delta)
 
         ###################
         # Plot Simulation #
@@ -121,19 +94,13 @@ if __name__ == '__main__':
         car.reference_path.show()
 
         # plot car trajectory and velocity
-        plt.scatter(x_log, y_log, c='g', s=15)
+        plt.scatter(x_log[:-1], y_log[:-1], c='g', s=15)
 
-        # plot mpc prediction
+        plt.scatter(mpc.current_prediction[0], mpc.current_prediction[1], c='b', s=5)
-        if mpc.current_prediction is not None:
-            x_pred = mpc.current_prediction[0]
-            y_pred = mpc.current_prediction[1]
-            plt.scatter(x_pred, y_pred, c='b', s=10)
 
-        plt.title('MPC Simulation: Position: {:.2f} m, {:.2f} m, Velocity: '
+        plt.title('MPC Simulation: Position: {:.2f} m, {:.2f} m'.
-                  '{:.2f} m/s'.format(car.temporal_state.x,
+                  format(car.temporal_state.x, car.temporal_state.y))
-                                      car.temporal_state.y, car.temporal_state.v_x))
         plt.xticks([])
         plt.yticks([])
-        plt.pause(0.0000001)
+        plt.pause(0.00000001)
-
     plt.close()

spatial_bicycle_models.py

@@ -7,7 +7,7 @@ from abc import ABC, abstractmethod
 #########################
 
 class TemporalState:
-    def __init__(self, x, y, psi, v_x, v_y):
+    def __init__(self, x, y, psi):
         """
         Temporal State Vector containing car pose (x, y, psi) and velocity
         :param x: x position in global coordinate system | [m]
@@ -19,8 +19,6 @@ class TemporalState:
         self.x = x
         self.y = y
         self.psi = psi
-        self.v_x = v_x
-        self.v_y = v_y
 
 
 ########################
@@ -39,6 +37,9 @@ class SpatialState(ABC):
     def __getitem__(self, item):
         return list(vars(self).values())[item]
 
+    def __setitem__(self, key, value):
+        vars(self)[list(vars(self).keys())[key]] = value
+
     def __len__(self):
         return len(vars(self))
 
@@ -60,40 +61,18 @@ class SpatialState(ABC):
 
 
 class SimpleSpatialState(SpatialState):
-    def __init__(self, e_y, e_psi, v):
+    def __init__(self, e_y, e_psi, t):
         """
         Simplified Spatial State Vector containing orthogonal deviation from
         reference path (e_y), difference in orientation (e_psi) and velocity
         :param e_y: orthogonal deviation from center-line | [m]
         :param e_psi: yaw angle relative to path | [rad]
-        :param v: absolute velocity | [m/s]
+        :param t: time | [s]
         """
         super(SimpleSpatialState, self).__init__()
 
         self.e_y = e_y
         self.e_psi = e_psi
-        self.v = v
-
-
-class ExtendedSpatialState(SpatialState):
-    def __init__(self, e_y, e_psi, v_x, v_y, omega, t):
-        """
-        Extended Spatial State Vector containing separate velocities in x and
-        y direction, angular velocity and time
-        :param e_y: orthogonal deviation from center-line | [m]
-        :param e_psi: yaw angle relative to path | [rad]
-        :param v_x: velocity in x direction (car frame) | [m/s]
-        :param v_y: velocity in y direction (car frame) | [m/s]
-        :param omega: anglular velocity of the car | [rad/s]
-        :param t: simulation time| [s]
-        """
-        super(ExtendedSpatialState, self).__init__()
-
-        self.e_y = e_y
-        self.e_psi = e_psi
-        self.v_x = v_x
-        self.v_y = v_y
-        self.omega = omega
         self.t = t
 
 
@@ -161,27 +140,32 @@ class SpatialBicycleModel(ABC):
 
         return x, y, psi
 
-    def drive(self, D, delta):
+    def drive(self, input, state=None, kappa=None, delta_s=None):
         """
-        Update states of spatial bicycle model. Model drive to the next
+        Drive.
-        waypoint on the reference path.
+        :param state: state vector for which to compute derivatives
-        :param D: acceleration command | [-1, 1]
+        :param input: input vector
-        :param delta: angular velocity | [rad]
+        :param kappa: curvature of corresponding waypoint
+        :return: numpy array with spatial derivatives for all state variables
         """
 
         # Get spatial derivatives
-        spatial_derivatives = self.get_spatial_derivatives(D, delta)
+        if state is None and kappa is None and delta_s is None:
-
+            state = np.array(self.spatial_state[:])
             # Get delta_s | distance to next waypoint
-        next_waypoint = self.reference_path.waypoints[self.wp_id+1]
+            next_waypoint = self.reference_path.waypoints[self.wp_id + 1]
             delta_s = next_waypoint - self.current_waypoint
+            # Get current curvature
+            kappa = self.current_waypoint.kappa
+
+            spatial_derivatives = self.get_spatial_derivatives(state, input, kappa)
 
             # Update spatial state (Forward Euler Approximation)
             self.spatial_state += spatial_derivatives * delta_s
 
             # Assert that unique projections of car pose onto path exists
-        assert self.spatial_state.e_y < (1 / (self.current_waypoint.kappa +
+            #assert self.spatial_state.e_y < (1 / (self.current_waypoint.kappa +
-                                              self.eps))
+             #                                     self.eps))
 
             # Increase waypoint ID
             self.wp_id += 1
@@ -196,10 +180,20 @@ class SpatialBicycleModel(ABC):
             self.s += delta_s
 
             # Linearize model around new operating point
-        self.A, self.B = self.linearize()
+            # self.A, self.B = self.linearize()
+
+        else:
+
+            spatial_derivatives = self.get_spatial_derivatives(state, input,
+                                                               kappa)
+
+            # Update spatial state (Forward Euler Approximation)
+            state += spatial_derivatives * delta_s
+
+            return state
 
     @abstractmethod
-    def get_spatial_derivatives(self, D, delta):
+    def get_spatial_derivatives(self, state, input, kappa):
         pass
 
     @abstractmethod
@@ -207,12 +201,12 @@ class SpatialBicycleModel(ABC):
         pass
 
 
-########################
+#################
-# Simple Bicycle Model #
+# Bicycle Model #
-########################
+#################
 
-class SimpleBicycleModel(SpatialBicycleModel):
+class BicycleModel(SpatialBicycleModel):
-    def __init__(self, reference_path, e_y, e_psi, v):
+    def __init__(self, reference_path, e_y, e_psi, t):
         """
         Simplified Spatial Bicycle Model. Spatial Reformulation of Kinematic
         Bicycle Model. Uses Simplified Spatial State.
@@ -223,24 +217,22 @@ class SimpleBicycleModel(SpatialBicycleModel):
         """
 
         # Initialize base class
-        super(SimpleBicycleModel, self).__init__(reference_path)
+        super(BicycleModel, self).__init__(reference_path)
 
         # Constants
-        self.C1 = 0.5
+        self.l = 0.06
-        self.C2 = 17.06
-        self.Cm1 = 12.0
-        self.Cm2 = 2.17
-        self.Cr2 = 0.1
-        self.Cr0 = 0.6
 
         # Initialize spatial state
-        self.spatial_state = SimpleSpatialState(e_y, e_psi, v)
+        self.spatial_state = SimpleSpatialState(e_y, e_psi, t)
+
+        # Number of spatial state variables
+        self.n_states = len(self.spatial_state)
 
         # Initialize temporal state
         self.temporal_state = self.s2t()
 
         # Compute linear system matrices | Used for MPC
-        self.A, self.B = self.linearize()
+        # self.A, self.B = self.linearize()
 
     def s2t(self, reference_waypoint=None, reference_state=None):
         """
@@ -253,547 +245,85 @@ class SimpleBicycleModel(SpatialBicycleModel):
 
         if reference_state is None and reference_waypoint is None:
             # Get pose information from base class implementation
-            x, y, psi = super(SimpleBicycleModel, self).s2t()
+            x, y, psi = super(BicycleModel, self).s2t()
             # Compute simplified velocities
-            v_x = self.spatial_state.v
-            v_y = 0
         else:
             # Get pose information from base class implementation
-            x, y, psi = super(SimpleBicycleModel, self).s2t(reference_waypoint,
+            x, y, psi = super(BicycleModel, self).s2t(reference_waypoint,
                                                             reference_state)
-            v_x = reference_state[2]
-            v_y = 0
 
-        return TemporalState(x, y, psi, v_x, v_y)
+        return TemporalState(x, y, psi)
 
-    def get_temporal_derivatives(self, D, delta):
+    def get_temporal_derivatives(self, state, input, kappa):
         """
         Compute relevant temporal derivatives needed for state update.
-        :param D: duty-cycle of DC motor | [-1, 1]
+        :param state: state vector for which to compute derivatives
-        :param delta: steering command | [rad]
+        :param input: input vector
+        :param kappa: curvature of corresponding waypoint
         :return: temporal derivatives of distance, angle and velocity
         """
 
-        # Compute velocity components | Approximation for small delta
+        e_y, e_psi, t = state
-        v_x = self.spatial_state.v
+        v, delta = input
-        v_y = self.spatial_state.v * delta * self.C1
-
-        # Compute velocity along waypoint direction
-        v_sigma = v_x * np.cos(self.spatial_state.e_psi) - v_y * np.sin(
-            self.spatial_state.e_psi)
 
         # Compute velocity along path
-        s_dot = 1 / (1 - (self.spatial_state.e_y * self.current_waypoint.kappa)) * v_sigma
+        s_dot = 1 / (1 - (e_y * kappa)) * v * np.cos(e_psi)
 
         # Compute yaw angle rate of change
-        psi_dot = self.spatial_state.v * delta * self.C2
+        psi_dot = v / self.l * np.tan(delta)
 
-        # Compute acceleration
+        return s_dot, psi_dot
-        v_dot = (self.Cm1 - self.Cm2 * self.spatial_state.v) * D - self.Cr2 * (
-                self.spatial_state.v ** 2) - self.Cr0 - (
-                self.spatial_state.v * delta) ** 2 * self.C2 * self.C1 ** 2
 
-        return s_dot, psi_dot, v_dot
+    def get_spatial_derivatives(self, state, input, kappa):
-
-    def get_spatial_derivatives(self, D, delta):
         """
         Compute spatial derivatives of all state variables for update.
-        :param D: duty-cycle of DC motor | [-1, 1]
+        :param state: state vector for which to compute derivatives
-        :param delta: steering angle | [rad]
+        :param input: input vector
+        :param kappa: curvature of corresponding waypoint
         :return: numpy array with spatial derivatives for all state variables
         """
 
+        e_y, e_psi, t = state
+        v, delta = input
+
         # Compute temporal derivatives
-        s_dot, psi_dot, v_dot = self.get_temporal_derivatives(D, delta)
+        s_dot, psi_dot = self.get_temporal_derivatives(state, input, kappa)
 
         # Compute spatial derivatives
-        d_e_y_d_s = (self.spatial_state.v * np.sin(self.spatial_state.e_psi)
+        d_e_y_d_s = v * np.sin(e_psi) / s_dot
-                 + self.spatial_state.v * delta * self.C1 * np.cos(
+        d_e_psi_d_s = psi_dot / s_dot - kappa
-                    self.spatial_state.e_psi)) / s_dot
+        d_t_d_s = 1 / s_dot
-        d_e_psi_d_s = psi_dot / s_dot - self.current_waypoint.kappa
-        d_v_d_s = v_dot / s_dot
 
-        return np.array([d_e_y_d_s, d_e_psi_d_s, d_v_d_s])
+        return np.array([d_e_y_d_s, d_e_psi_d_s, d_t_d_s])
 
-    def linearize(self, D=0, delta=0):
+    def linearize(self, v=None, kappa_r=None, delta_s=None):
         """
         Linearize the system equations around the current state and waypoint.
-        :param delta: reference steering angle | [rad]
+        :param kappa_r: kappa of waypoint around which to linearize
-        :param D: reference duty-cycle of DC-motor | [-1, 1]
          """
 
-        # Get current state | operating point to linearize around
+        # Get linearization state
-        e_y = self.spatial_state.e_y
+        if kappa_r is None and delta_s is None:
-        e_psi = self.spatial_state.e_psi
+            # Get curvature of linearization waypoint
-        v = self.spatial_state.v
+            kappa_r = self.reference_path.waypoints[self.wp_id].kappa
-
-        # Get curvature of current waypoint
-        kappa = self.reference_path.waypoints[self.wp_id].kappa
-
             # Get delta_s
-        next_waypoint = self.reference_path.waypoints[self.wp_id+1]
+            next_waypoint = self.reference_path.waypoints[self.wp_id + 1]
             delta_s = next_waypoint - self.current_waypoint
 
-        ##############################
-        # Helper Partial Derivatives #
-        ##############################
-
-        # Compute velocity components
-        v_x = v
-        v_y = v * delta * self.C1
-
-        # Compute partial derivatives of s_dot w.r.t. each state variable,
-        # input variable and kappa
-        s_dot = 1 / (1 - e_y*kappa) * (v_x * np.cos(e_psi) - v_y * np.sin(e_psi))
-        d_s_dot_d_e_y = kappa / (1-e_y*kappa)**2 * (v_x * np.cos(e_psi) - v_y * np.sin(e_psi))
-        d_s_dot_d_e_psi = 1 / (1 - e_y*kappa) * (-v_x * np.sin(e_psi) - v_y * np.cos(e_psi))
-        d_s_dot_d_v = 1 / (1 - e_y*kappa) * (np.cos(e_psi) - delta * self.C1 * np.sin(e_psi))
-        # d_s_dot_d_D = 0
-        d_s_dot_d_delta = 1 / (1 - e_y*kappa) * (- v * self.C1 * np.sin(e_psi))
-        d_s_dot_d_kappa = e_y / (1-e_y*kappa)**2 * (v_x * np.cos(e_psi) - v_y * np.sin(e_psi))
-
-        # Compute partial derivatives of v_psi w.r.t. each state variable,
-        # input variable and kappa
-        v_psi = (v_x * np.sin(e_psi) + v_y * np.cos(e_psi))
-        # d_v_psi_d_e_y = 0
-        d_v_psi_d_e_psi = v_x * np.cos(e_psi) - v_y * np.sin(e_psi)
-        d_v_psi_d_v = np.sin(e_psi) + self.C1 * delta * np.cos(e_psi)
-        # d_v_psi_d_D = 0
-        d_v_psi_d_delta = self.C1 * v * np.cos(e_psi)
-        # d_v_psi_d_kappa = 0
-
-        # Compute partial derivatives of psi_dot w.r.t. each state variable,
-        # input variable and kappa
-        psi_dot = v * delta * self.C2
-        # d_psi_dot_d_e_y = 0
-        # d_psi_dot_d_e_psi = 0
-        d_psi_dot_d_v = delta * self.C2
-        # d_psi_dot_d_D = 0
-        d_psi_dot_d_delta = v * self.C2
-        # d_psi_dot_d_kappa = 0
-
-        # Compute partial derivatives of v_dot w.r.t. each state variable,
-        # input variable and kappa
-        v_dot = (self.Cm1 - self.Cm2 * v) * D - self.Cr2 * (v ** 2) - self.Cr0 \
-                - (v * delta) ** 2 * self.C2 * (self.C1 ** 2)
-        # d_v_dot_d_e_y = 0
-        # d_v_dot_d_e_psi = 0
-        d_v_dot_d_v = -self.Cm2 * D - 2 * self.Cr2 * v - 2 * v * (delta ** 2) \
-                      * self.C2 * (self.C1 ** 2)
-        d_v_dot_d_D = self.Cm1 - self.Cm2 * v
-        d_v_dot_d_delta = -2 * (v ** 2) * delta * self.C2 * self.C1 ** 2
-        # d_v_dot_d_kappa = 0
-
-        #############################
-        # State Partial Derivatives #
-        #############################
-
-        # Use pre-computed helper derivatives to compute spatial derivatives of
-        # all state variables using Quotient Rule
-
-        # Compute partial derivatives of e_y w.r.t. each state variable,
-        # input variable and kappa
-        # e_y = v_psi / s_dot
-        d_e_y_d_e_y = - d_s_dot_d_e_y * v_psi / (s_dot**2)
-        d_e_y_d_e_psi = (d_v_psi_d_e_psi * s_dot - d_s_dot_d_e_psi * v_psi) / (s_dot**2)
-        d_e_y_d_v = (d_v_psi_d_v * s_dot - d_s_dot_d_v * v_psi) / (s_dot**2)
-        d_e_y_d_D = 0
-        d_e_y_d_delta = (d_v_psi_d_delta * s_dot - d_s_dot_d_delta * v_psi) / (s_dot**2)
-        d_e_y_d_kappa = - d_s_dot_d_kappa * v_psi / (s_dot**2)
-
-        # Compute partial derivatives of e_psi w.r.t. each state variable,
-        # input variable and kappa
-        # e_psi = psi_dot / s_dot - kappa
-        d_e_psi_d_e_y = - d_s_dot_d_e_y * psi_dot / (s_dot**2)
-        d_e_psi_d_e_psi = - d_s_dot_d_e_psi * psi_dot / (s_dot**2)
-        d_e_psi_d_v = (d_psi_dot_d_v * s_dot - d_s_dot_d_v * psi_dot) / (s_dot**2)
-        d_e_psi_d_D = 0
-        d_e_psi_d_delta = (d_psi_dot_d_delta * s_dot - d_s_dot_d_delta * psi_dot) / (s_dot**2)
-        d_e_psi_d_kappa = - d_s_dot_d_kappa * psi_dot / (s_dot**2) - 1
-
-        # Compute partial derivatives of v w.r.t. each state variable,
-        # input variable and kappa
-        # v = v_dot / s_dot
-        d_v_d_e_y = - d_s_dot_d_e_y * v_dot / (s_dot**2)
-        d_v_d_e_psi = - d_s_dot_d_e_psi * v_dot / (s_dot**2)
-        d_v_d_v = (d_v_dot_d_v * s_dot - d_s_dot_d_v * v_dot) / (s_dot**2)
-        d_v_d_D = d_v_dot_d_D * s_dot / (s_dot**2)
-        d_v_d_delta = (d_v_dot_d_delta * s_dot - d_s_dot_d_delta * v_dot) / (s_dot**2)
-        d_v_d_kappa = - d_s_dot_d_kappa * v_dot / (s_dot**2)
-
-        #############
-        # Jacobians #
-        #############
-
-        # Construct Jacobian Matrix
-        a_1 = np.array([d_e_y_d_e_y,    d_e_y_d_e_psi,      d_e_y_d_v,      d_e_y_d_kappa])
-        a_2 = np.array([d_e_psi_d_e_y,  d_e_psi_d_e_psi,    d_e_psi_d_v,    d_e_psi_d_kappa])
-        a_3 = np.array([d_v_d_e_y,      d_v_d_e_psi,        d_v_d_v,        d_v_d_kappa])
-
-        b_1 = np.array([d_e_y_d_D, d_e_y_d_delta])
-        b_2 = np.array([d_e_psi_d_D, d_e_psi_d_delta])
-        b_3 = np.array([d_v_d_D, d_v_d_delta])
-
-        # Add extra row for kappa | Allows for updating kappa during MPC
-        # optimization
-        a_4 = np.array([0, 0, 0, 0])
-        b_4 = np.array([0, 0])
-
-        Ja = np.stack((a_1, a_2, a_3, a_4), axis=0)
-        Jb = np.stack((b_1, b_2, b_3, b_4), axis=0)
-
         ###################
         # System Matrices #
         ###################
 
-        # Construct system matrices from Jacobians. Multiply by sampling
+        # Construct Jacobian Matrix
-        # distance delta_s + add identity matrix (Forward Euler Approximation)
+        a_1 = np.array([1,                    delta_s,      0])
-        A = Ja * delta_s + np.identity(Ja.shape[1])
+        a_2 = np.array([-kappa_r**2*delta_s,  1,            0])
-        B = Jb * delta_s
+        a_3 = np.array([-kappa_r/v*delta_s,   0,            0])
+
+        b_1 = np.array([0, ])
+        b_2 = np.array([delta_s, ])
+        b_3 = np.array([0, ])
+
+        A = np.stack((a_1, a_2, a_3), axis=0)
+        B = np.stack((b_1, b_2, b_3), axis=0)
 
         return A, B
-
-
-##########################
-# Extended Bicycle Model #
-##########################
-
-class ExtendedBicycleModel(SpatialBicycleModel):
-    def __init__(self, reference_path, e_y, e_psi, v_x, v_y, omega, t):
-        """
-        Construct spatial bicycle model.
-        :param e_y: initial deviation from reference path | [m]
-        :param e_psi: initial heading offset from reference path | [rad]
-        :param v: initial velocity | [m/s]
-        :param reference_path: reference path model is supposed to follow
-        """
-        super(ExtendedBicycleModel, self).__init__(reference_path)
-
-        # Constants
-        self.m = 0.041
-        self.Iz = 27.8e-6
-        self.lf = 0.029
-        self.lr = 0.033
-
-        self.Cm1 = 0.287
-        self.Cm2 = 0.0545
-        self.Cr2 = 0.0518
-        self.Cr0 = 0.00035
-
-        self.Br = 3.3852
-        self.Cr = 1.2691
-        self.Dr = 0.1737
-        self.Bf = 2.579
-        self.Cf = 1.2
-        self.Df = 0.192
-
-        # Spatial state
-        self.spatial_state = ExtendedSpatialState(e_y, e_psi, v_x, v_y, omega, t)
-
-        # Temporal state
-        self.temporal_state = self.s2t()
-
-        # Linear System Matrices
-        self.A, self.B = self.linearize()
-
-    def s2t(self, reference_waypoint=None, predicted_state=None):
-        """
-        Convert spatial state to temporal state
-        :return temporal state equivalent to self.spatial_state
-        """
-
-        # compute velocity information
-        if predicted_state is None and reference_waypoint is None:
-            # get information from base class
-            x, y, psi = super(ExtendedBicycleModel, self).s2t()
-            v_x = self.spatial_state.v_x
-            v_y = self.spatial_state.v_y
-        else:
-            # get information from base class
-            x, y, psi = super(ExtendedBicycleModel, self).s2t(reference_waypoint,
-                                                            predicted_state)
-            v_x = predicted_state[2]
-            v_y = predicted_state[3]
-
-        return TemporalState(x, y, psi, v_x, v_y)
-
-    def get_forces(self, delta, D):
-        """
-        Compute forces required for temporal derivatives of v_x and v_y
-        :param delta:
-        :param D:
-        :return:
-        """
-
-        F_rx = (self.Cm1 - self.Cm2 * self.spatial_state.v_x) * D - self.Cr0 - self.Cr2 * self.spatial_state.v_x ** 2
-
-        alpha_f = - np.arctan2(self.spatial_state.omega*self.lf + self.spatial_state.v_y, self.spatial_state.v_x) + delta
-        F_fy = self.Df * np.sin(self.Cf*np.arctan(self.Bf*alpha_f))
-
-        alpha_r = np.arctan2(self.spatial_state.omega*self.lr - self.spatial_state.v_y, self.spatial_state.v_x)
-        F_ry = self.Dr * np.sin(self.Cr * np.arctan(self.Br*alpha_r))
-
-        return F_rx, F_fy, F_ry, alpha_f, alpha_r
-
-    def get_temporal_derivatives(self, delta, F_rx, F_fy, F_ry):
-        """
-        Compute temporal derivatives needed for state update.
-        :param delta: steering command
-        :param D: duty-cycle of DC motor
-        :return: temporal derivatives of distance, angle and velocity
-        """
-
-        # velocity along path
-        s_dot = 1 / (1 - (self.spatial_state.e_y * self.current_waypoint.kappa)) \
-                * (self.spatial_state.v_x * np.cos(self.spatial_state.e_psi)
-                   + self.spatial_state.v_y * np.sin(self.spatial_state.e_psi))
-
-        # velocity in x and y direction
-        v_x_dot = (F_rx - F_fy * np.sin(delta) + self.m * self.spatial_state.
-                   v_y * self.spatial_state.omega) / self.m
-        v_y_dot = (F_ry + F_fy * np.cos(delta) - self.m * self.spatial_state.
-                   v_x * self.spatial_state.omega) / self.m
-
-        # omega dot
-        omega_dot = (F_fy * self.lf * np.cos(delta) - F_ry * self.lr) / self.Iz
-
-        return s_dot, v_x_dot, v_y_dot, omega_dot
-
-    def get_spatial_derivatives(self, delta, D):
-        """
-        Compute spatial derivatives of all state variables for update.
-        :param delta: steering angle
-        :param psi_dot: heading rate of change
-        :param s_dot: velocity along path
-        :param v_dot: acceleration
-        :return: spatial derivatives for all state variables
-        """
-
-        # get required forces
-        F_rx, F_fy, F_ry, _, _ = self.get_forces(delta, D)
-
-        # Compute state derivatives
-        s_dot, v_x_dot, v_y_dot, omega_dot = \
-            self.get_temporal_derivatives(delta, F_rx, F_fy, F_ry)
-
-
-        d_e_y = (self.spatial_state.v_x * np.sin(self.spatial_state.e_psi)
-                 + self.spatial_state.v_y * np.cos(self.spatial_state.e_psi)) \
-                / (s_dot + self.eps)
-        d_e_psi = (self.spatial_state.omega / (s_dot + self.eps) - self.current_waypoint.kappa)
-
-        d_v_x = v_x_dot / (s_dot + self.eps)
-        d_v_y = v_y_dot / (s_dot + self.eps)
-        d_omega = omega_dot / (s_dot + self.eps)
-        d_t = 1 / (s_dot + self.eps)
-
-        return np.array([d_e_y, d_e_psi, d_v_x, d_v_y, d_omega, d_t])
-
-    def linearize(self, delta=0, D=0):
-        """
-        Linearize the system equations around the current state and waypoint.
-        :param delta: reference steering angle
-        :param D: reference dutycycle
-        """
-
-        # get current state
-        e_y = self.spatial_state.e_y
-        e_psi = self.spatial_state.e_psi
-        v_x = self.spatial_state.v_x
-        v_y = self.spatial_state.v_y
-        omega = self.spatial_state.omega
-        t = self.spatial_state.t
-
-        # get information about current waypoint
-        kappa = self.reference_path.waypoints[self.wp_id].kappa
-
-        # get delta_s
-        next_waypoint = self.reference_path.waypoints[self.wp_id + 1]
-        delta_s = next_waypoint - self.current_waypoint
-
-        # get temporal derivatives
-        F_rx, F_fy, F_ry, alpha_f, alpha_r = self.get_forces(delta, D)
-        s_dot, v_x_dot, v_y_dot, omega_dot = self.\
-            get_temporal_derivatives(delta, F_rx, F_fy, F_ry)
-
-        ##############################
-        # Forces Partial Derivatives #
-        ##############################
-
-        d_alpha_f_d_v_x = 1 / (1 + ((omega * self.lf + v_y) / v_x)**2) * (omega * self.lf + v_y) / (v_x**2)
-        d_alpha_f_d_v_y = - 1 / (1 + ((omega * self.lf + v_y) / v_x)**2) / v_x
-        d_alpha_f_d_omega = - 1 / (1 + ((omega * self.lf + v_y) / v_x)**2) * (self.lf / v_x)
-        d_alpha_f_d_delta = 1
-
-        d_alpha_r_d_v_x = - 1 / (1 + ((omega * self.lr - v_y) / v_x)**2) * (omega * self.lr - v_y) / (v_x**2)
-        d_alpha_r_d_v_y = - 1 / (1 + ((omega * self.lr - v_y) / v_x)**2) / v_x
-        d_alpha_r_d_omega = 1 / (1 + ((omega * self.lr - v_y) / v_x)**2) * (self.lr * v_x)
-
-        d_F_fy_d_v_x = self.Df * np.cos(self.Cf * np.arctan(self.Bf * alpha_f)) * self.Cf / (1 + (self.Bf * alpha_f)**2) * self.Bf * d_alpha_f_d_v_x
-        d_F_fy_d_v_y = self.Df * np.cos(self.Cf * np.arctan(self.Bf * alpha_f)) * self.Cf / (1 + (self.Bf * alpha_f)**2) * self.Bf * d_alpha_f_d_v_y
-        d_F_fy_d_omega = self.Df * np.cos(self.Cf * np.arctan(self.Bf * alpha_f)) * self.Cf / (1 + (self.Bf * alpha_f)**2) * self.Bf * d_alpha_f_d_omega
-        d_F_fy_d_delta = self.Df * np.cos(self.Cf * np.arctan(self.Bf * alpha_f)) * self.Cf / (1 + (self.Bf * alpha_f)**2) * self.Bf * d_alpha_f_d_delta
-
-        d_F_ry_d_v_x = self.Dr * np.cos(self.Cr * np.arctan(self.Br * alpha_r)) * self.Cr / (1 + (self.Br * alpha_r)**2) * self.Br * d_alpha_r_d_v_x
-        d_F_ry_d_v_y = self.Dr * np.cos(self.Cr * np.arctan(self.Br * alpha_r)) * self.Cr / (1 + (self.Br * alpha_r)**2) * self.Br * d_alpha_r_d_v_y
-        d_F_ry_d_omega = self.Dr * np.cos(self.Cr * np.arctan(self.Br * alpha_r)) * self.Cr / (1 + (self.Br * alpha_r)**2) * self.Br * d_alpha_r_d_omega
-
-        d_F_rx_d_v_x = - self.Cm2 * D - 2 * self.Cr2 * v_x
-        d_F_rx_d_D = self.Cm1 - self.Cm2 * v_x
-
-        ##############################
-        # Helper Partial Derivatives #
-        ##############################
-
-        d_s_dot_d_e_y = kappa / (1-e_y*kappa)**2 * (v_x * np.cos(e_psi) - v_y * np.sin(e_psi))
-        d_s_dot_d_e_psi = 1 / (1 - e_y*kappa) * (-v_x * np.sin(e_psi) - v_y * np.cos(e_psi))
-        d_s_dot_d_v_x = 1 / (1 - e_y*kappa) * np.cos(e_psi)
-        d_s_dot_d_v_y = -1 / (1 - e_y*kappa) * np.sin(e_psi)
-        d_s_dot_d_omega = 0
-        d_s_dot_d_t = 0
-        d_s_dot_d_delta = 0
-        d_s_dot_d_D = 0
-        d_s_dot_d_kappa = e_y / (1-e_y*kappa)**2 * (v_x * np.cos(e_psi) - v_y * np.sin(e_psi))
-        # Check
-
-        c_1 = (v_x * np.sin(e_psi) + v_y * np.cos(e_psi))
-        d_c_1_d_e_y = 0
-        d_c_1_d_e_psi = v_x * np.cos(e_psi) - v_y * np.sin(e_psi)
-        d_c_1_d_v_x = np.sin(e_psi)
-        d_c_1_d_v_y = np.cos(e_psi)
-        d_c_1_d_omega = 0
-        d_c_1_d_t = 0
-        d_c_1_d_delta = 0
-        d_c_1_d_D = 0
-        d_c_1_d_kappa = 0
-        # Check
-
-        d_v_x_dot_d_e_y = 0
-        d_v_x_dot_d_e_psi = 0
-        d_v_x_dot_d_v_x = (d_F_rx_d_v_x - d_F_fy_d_v_x * np.sin(delta)) / self.m
-        d_v_x_dot_d_v_y = - (d_F_fy_d_v_y * np.sin(delta) + self.m * omega) / self.m
-        d_v_x_dot_d_omega = - (d_F_fy_d_omega * np.sin(delta) + self.m * v_y) / self.m
-        d_v_x_dot_d_t = 0
-        d_v_x_dot_d_delta = - (F_fy * np.cos(delta) + d_F_fy_d_delta * np.sin(delta)) / self.m
-        d_v_x_dot_d_D = d_F_rx_d_D / self.m
-        d_v_x_dot_d_kappa = 0
-
-        d_v_y_dot_d_e_y = 0
-        d_v_y_dot_d_e_psi = 0
-        d_v_y_dot_d_v_x = (d_F_ry_d_v_x + d_F_fy_d_v_x * np.cos(delta) - self.m * omega) / self.m
-        d_v_y_dot_d_v_y = (d_F_ry_d_v_y + d_F_fy_d_v_y * np.cos(delta)) / self.m
-        d_v_y_dot_d_omega = (d_F_ry_d_omega + d_F_fy_d_omega * np.cos(delta) - self.m * v_x) / self.m
-        d_v_y_dot_d_t = 0
-        d_v_y_dot_d_delta = d_F_fy_d_delta * np.cos(delta) / self.m
-        d_v_y_dot_d_D = 0
-        d_v_y_dot_d_kappa = 0
-
-        d_omega_dot_d_e_y = 0
-        d_omega_dot_d_e_psi = 0
-        d_omega_dot_d_v_x = (d_F_fy_d_v_x * self.lf * np.cos(delta) - d_F_ry_d_v_x * self.lr) / self.Iz
-        d_omega_dot_d_v_y = (d_F_fy_d_v_y * self.lf * np.cos(delta) - d_F_fy_d_v_y * self.lr) / self.Iz
-        d_omega_dot_d_omega = (d_F_fy_d_omega * self.lf * np.cos(delta) - d_F_fy_d_omega * self.lr) / self.Iz
-        d_omega_dot_d_t = 0
-        d_omega_dot_d_delta = (- F_fy * np.sin(delta) + d_F_fy_d_delta * np.cos(delta)) / self.Iz
-        d_omega_dot_d_D = 0
-        d_omega_dot_d_kappa = 0
-
-        #######################
-        # Partial Derivatives #
-        #######################
-
-        # derivatives for E_Y
-        d_e_y_d_e_y = -c_1 * d_s_dot_d_e_y / (s_dot**2)
-        d_e_y_d_e_psi = (d_c_1_d_e_psi * s_dot - d_s_dot_d_e_psi * c_1) / (s_dot**2)
-        d_e_y_d_v_x = (d_c_1_d_v_x * s_dot - d_s_dot_d_v_x * c_1) / (s_dot**2)
-        d_e_y_d_v_y = (d_c_1_d_v_y * s_dot - d_s_dot_d_v_y * c_1) / (s_dot**2)
-        d_e_y_d_omega = (d_c_1_d_omega * s_dot - d_s_dot_d_omega * c_1) / (s_dot**2)
-        d_e_y_d_t = 0
-        d_e_y_d_D = 0
-        d_e_y_d_delta = (d_c_1_d_delta * s_dot - d_s_dot_d_delta * c_1) / (s_dot**2)
-        d_e_y_d_kappa = -d_s_dot_d_kappa * c_1 / (s_dot**2)
-
-        # derivatives for E_PSI
-        d_e_psi_d_e_y = - omega * d_s_dot_d_e_y / (s_dot**2)
-        d_e_psi_d_e_psi = - omega * d_s_dot_d_e_psi / (s_dot**2)
-        d_e_psi_d_v_x = (- omega * d_s_dot_d_v_x) / (s_dot**2)
-        d_e_psi_d_v_y = (- omega * d_s_dot_d_v_y) / (s_dot**2)
-        d_e_psi_d_omega = (s_dot - omega * d_s_dot_d_omega) / (s_dot**2)
-        d_e_psi_d_t = 0
-        d_e_psi_d_delta = (- omega * d_s_dot_d_delta) / (s_dot**2)
-        d_e_psi_d_D = (- omega * d_s_dot_d_D) / (s_dot**2)
-        d_e_psi_d_kappa = -d_s_dot_d_kappa * omega / (s_dot**2) - 1
-
-        # derivatives for V_X
-        d_v_x_d_e_y = - d_s_dot_d_e_y * v_x_dot / (s_dot**2)
-        d_v_x_d_e_psi = - d_s_dot_d_e_psi * v_x_dot / (s_dot**2)
-        d_v_x_d_v_x = (d_v_x_dot_d_v_x * s_dot - d_s_dot_d_v_x * v_x_dot) / (s_dot**2)
-        d_v_x_d_v_y = (d_v_x_dot_d_v_y * s_dot - d_s_dot_d_v_y * v_x_dot) / (s_dot**2)
-        d_v_x_d_omega = (d_v_x_dot_d_omega * s_dot - d_s_dot_d_omega * v_x_dot) / (s_dot**2)
-        d_v_x_d_t = 0
-        d_v_x_d_delta = (d_v_x_dot_d_delta * s_dot - d_s_dot_d_delta * v_x_dot) / (s_dot**2)
-        d_v_x_d_D = d_v_x_dot_d_D * s_dot / (s_dot**2)
-        d_v_x_d_kappa = -d_s_dot_d_kappa * v_x_dot / (s_dot**2)
-
-        # derivatives for V_Y
-        d_v_y_d_e_y = - d_s_dot_d_e_y * v_y_dot / (s_dot ** 2)
-        d_v_y_d_e_psi = - d_s_dot_d_e_psi * v_y_dot / (s_dot ** 2)
-        d_v_y_d_v_x = (d_v_y_dot_d_v_x * s_dot - d_s_dot_d_v_x * v_y_dot) / (
-                    s_dot ** 2)
-        d_v_y_d_v_y = (d_v_y_dot_d_v_y * s_dot - d_s_dot_d_v_y * v_y_dot) / (
-                    s_dot ** 2)
-        d_v_y_d_omega = (d_v_y_dot_d_omega * s_dot - d_s_dot_d_omega * v_y_dot) / (
-                                    s_dot ** 2)
-        d_v_y_d_t = 0
-        d_v_y_d_delta = (d_v_y_dot_d_delta * s_dot - d_s_dot_d_delta * v_y_dot) / (
-                                    s_dot ** 2)
-        d_v_y_d_D = d_v_y_dot_d_D * s_dot / (s_dot ** 2)
-        d_v_y_d_kappa = -d_s_dot_d_kappa * v_y_dot / (s_dot ** 2)
-
-        # derivatives for Omega
-        d_omega_d_e_y = (d_omega_dot_d_e_y * s_dot - omega_dot * d_s_dot_d_e_y) / (s_dot**2)
-        d_omega_d_e_psi = (d_omega_dot_d_e_psi * s_dot - omega_dot * d_s_dot_d_e_psi) / (s_dot**2)
-        d_omega_d_v_x = (d_omega_dot_d_v_x * s_dot - omega_dot * d_s_dot_d_v_x) / (s_dot**2)
-        d_omega_d_v_y = (d_omega_dot_d_v_y * s_dot - omega_dot * d_s_dot_d_v_y) / (s_dot**2)
-        d_omega_d_omega = (d_omega_dot_d_omega * s_dot - omega_dot * d_s_dot_d_omega) / (s_dot**2)
-        d_omega_d_t = (d_omega_dot_d_t * s_dot - omega_dot * d_s_dot_d_t) / (s_dot**2)
-        d_omega_d_delta = (d_omega_dot_d_delta * s_dot - omega_dot * d_s_dot_d_delta) / (s_dot**2)
-        d_omega_d_D = (d_omega_dot_d_D * s_dot - omega_dot * d_s_dot_d_D) / (s_dot**2)
-        d_omega_d_kappa = (d_omega_dot_d_kappa * s_dot - omega_dot * d_s_dot_d_kappa) / (s_dot**2)
-
-        # derivatives for T
-        d_t_d_e_y = - d_s_dot_d_e_y / (s_dot**2)
-        d_t_d_e_psi = - d_s_dot_d_e_psi / (s_dot ** 2)
-        d_t_d_v_x = - d_s_dot_d_v_x / (s_dot ** 2)
-        d_t_d_v_y = - d_s_dot_d_v_y / (s_dot ** 2)
-        d_t_d_omega = - d_s_dot_d_omega / (s_dot ** 2)
-        d_t_d_t = 0
-        d_t_d_delta = - d_s_dot_d_delta / (s_dot ** 2)
-        d_t_d_D = 0
-        d_t_d_kappa = - d_s_dot_d_kappa / (s_dot ** 2)
-
-        a_1 = np.array([d_e_y_d_e_y, d_e_y_d_e_psi, d_e_y_d_v_x, d_e_y_d_v_y, d_e_y_d_omega, d_e_y_d_t, d_e_y_d_kappa])
-        a_2 = np.array([d_e_psi_d_e_y, d_e_psi_d_e_psi, d_e_psi_d_v_x, d_e_psi_d_v_y, d_e_psi_d_omega, d_e_psi_d_t, d_e_psi_d_kappa])
-        a_3 = np.array([d_v_x_d_e_y, d_v_x_d_e_psi, d_v_x_d_v_x, d_v_x_d_v_y, d_v_x_d_omega, d_v_x_d_t, d_v_x_d_kappa])
-        a_4 = np.array([d_v_y_d_e_y, d_v_y_d_e_psi, d_v_y_d_v_x, d_v_y_d_v_y, d_v_y_d_omega, d_v_y_d_t, d_v_y_d_kappa])
-        a_5 = np.array([d_omega_d_e_y, d_omega_d_e_psi, d_omega_d_v_x, d_omega_d_v_y, d_omega_d_omega, d_omega_d_t, d_omega_d_kappa])
-        a_6 = np.array([d_t_d_e_y, d_t_d_e_psi, d_t_d_v_x, d_t_d_v_y, d_t_d_omega, d_t_d_t, d_t_d_kappa])
-        a_7 = np.array([0, 0, 0, 0, 0, 0, 1])
-        A = np.stack((a_1, a_2, a_3, a_4, a_5, a_6, a_7), axis=0) * delta_s
-        A[0, 0] += 1
-        A[1, 1] += 1
-        A[2, 2] += 1
-        A[3, 3] += 1
-        A[4, 4] += 1
-        A[5, 5] += 1
-        b_1 = np.array([d_e_y_d_D, d_e_y_d_delta])
-        b_2 = np.array([d_e_psi_d_D, d_e_psi_d_delta])
-        b_3 = np.array([d_v_x_d_D, d_v_x_d_delta])
-        b_4 = np.array([d_v_y_d_D, d_v_y_d_delta])
-        b_5 = np.array([d_omega_d_D, d_omega_d_delta])
-        b_6 = np.array([d_t_d_D, d_t_d_delta])
-        b_7 = np.array([0, 0])
-        B = np.stack((b_1, b_2, b_3, b_4, b_5, b_6, b_7), axis=0) * delta_s
-
-        # set system matrices
-        return A, B