diff --git a/MPC.py b/MPC.py index b122594..c54cbbc 100644 --- a/MPC.py +++ b/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, - Reference): + 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 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 - self.x = cp.Variable((self.model.n_states+1, self.T + 1)) - self.u = cp.Variable((2, self.T)) + # number of input and state variables + nx = self.model.n_states + nu = 1 - # Instantiate optimization parameters - self.kappa = cp.Parameter(self.T+1) - self.x_0 = cp.Parameter(self.model.n_states+1, 1) - self.A = cp.Parameter(self.model.A.shape) - self.B = cp.Parameter(self.model.B.shape) + # system matrices + self.A = cp.Parameter(shape=(nx, nx*self.N)) + self.B = cp.Parameter(shape=(nx, nu*self.N)) + self.A.value = np.zeros(self.A.shape) + self.B.value = np.zeros(self.B.shape) - # Initialize cost - cost = 0 + # reference values + 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 = [self.x[:, 0] == self.x_0] + # constraints + 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 - constraints += [self.x[:-1, t + 1] == self.A[:-1, :] - @ self.x[:, t] + self.B[:-1, :] @ self.u[:, t], - self.x[-1, t + 1] == self.kappa[t+1]] - - # set input constraints - inputs = ['D', 'delta'] - for input_name, constraint in self.input_constraints.items(): - input_id = inputs.index(input_name) - if constraint[0] is not None: - constraints.append(-self.u[input_id, t] <= -constraint[0]) - if constraint[1] is not None: - constraints.append(self.u[input_id, t] <= constraint[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, 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) + # Define problem + self.u = cp.Variable((nu, self.N)) + self.x = cp.Variable((nx, self.N + 1)) + objective = 0 + constraints = [self.x[:, 0] == self.x_init] + for n in range(self.N): + objective += cp.quad_form(self.x[:, n] - xr, self.Q) + cp.quad_form(self.u[:, n] - self.ur[:, n], self.R) + constraints += [self.x[:, n + 1] == self.A[:, n*nx:n*nx+nx] * self.x[:, n] + + self.B[:, n*nu] * (self.u[:, n] - self.ur[:, n])] + constraints += [umin <= self.u[:, n], self.u[:, n] <= umax] + objective += cp.quad_form(self.x[:, self.N] - xr, self.QN) + constraints += [xmin <= self.x[:, self.N], self.x[:, self.N] <= xmax] + problem = cp.Problem(cp.Minimize(objective), 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 - kappa_ref = [wp.kappa for wp in self.model.reference_path.waypoints - [self.model.wp_id:self.model.wp_id+self.T+1]] + nx = self.model.n_states + nu = 1 - # Instantiate optimization parameters - self.kappa.value = kappa_ref - self.x_0.value = np.array(self.model.spatial_state[:] + [kappa_ref[0]]) - self.A.value = self.model.A - self.B.value = self.model.B + for n in range(self.N): + 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 + 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.problem.solve(solver=cp.ECOS, warm_start=True) + self.x_init.value = np.array(self.model.spatial_state[:]) + 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: - print('No solution found!') - exit(1) + self.current_prediction = self.update_prediction(self.x.value) + delta = np.arctan(self.u.value[0, 0] * self.model.l) - # RCH - get first control signal - D_0 = self.u.value[0, 0] - delta_0 = self.u.value[1, 0] - - return D_0, delta_0 + return delta 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): - associated_waypoint = self.model.reference_path.waypoints[wp_id_+t] + 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[:, 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) diff --git a/simulation.py b/simulation.py index 5cb46e5..1142a81 100644 --- a/simulation.py +++ b/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 time import time +from MPC import MPC, MPC_OSQP +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 - v_y_0 = 0 - omega_0 = 0 - t_0 = 0 + t_0 = 0.0 + v = 1.0 - if model_type == 'Extended': - car = ExtendedBicycleModel(reference_path=reference_path, - 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) + car = BicycleModel(reference_path=reference_path, + e_y=e_y_0, e_psi=e_psi_0, t=t_0) ############## # Controller # ############## - if model_type == 'Extended': - Q = np.diag([1, 0, 0, 0, 0, 0]) - Qf = Q - R = np.diag([0, 0]) - Reference = {'e_y': 0, 'e_psi': 0, 'v_x': 1.0, 'v_y': 0, 'omega': 0, 't':0} - elif model_type == 'Simple': - Reference = {'e_y': 0, 'e_psi': 0, 'v': 4.0} - 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) + N = 20 + Q = sparse.diags([0.01, 0.0, 0.4]) + R = sparse.diags([0.01]) + QN = Q + InputConstraints = {'umin': np.array([-np.tan(0.44)/car.l]), 'umax': np.array([np.tan(0.44)/car.l])} + StateConstraints = {'xmin': np.array([-0.2, -np.inf, 0]), 'xmax': np.array([0.2, np.inf, np.inf])} + mpc = MPC_OSQP(car, N, Q, R, QN, StateConstraints, InputConstraints) ############## # 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 - 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.scatter(mpc.current_prediction[0], mpc.current_prediction[1], c='b', s=5) - plt.title('MPC Simulation: Position: {:.2f} m, {:.2f} m, Velocity: ' - '{:.2f} m/s'.format(car.temporal_state.x, - car.temporal_state.y, car.temporal_state.v_x)) + plt.title('MPC Simulation: Position: {:.2f} m, {:.2f} m'. + format(car.temporal_state.x, car.temporal_state.y)) plt.xticks([]) plt.yticks([]) - plt.pause(0.0000001) - + plt.pause(0.00000001) plt.close() diff --git a/spatial_bicycle_models.py b/spatial_bicycle_models.py index 60bd642..1f1b24f 100644 --- a/spatial_bicycle_models.py +++ b/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,45 +140,60 @@ 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 - waypoint on the reference path. - :param D: acceleration command | [-1, 1] - :param delta: angular velocity | [rad] + Drive. + :param state: state vector for which to compute derivatives + :param input: input vector + :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] + delta_s = next_waypoint - self.current_waypoint + # Get current curvature + kappa = self.current_waypoint.kappa - # Get delta_s | distance to next waypoint - next_waypoint = self.reference_path.waypoints[self.wp_id+1] - delta_s = next_waypoint - self.current_waypoint + spatial_derivatives = self.get_spatial_derivatives(state, input, kappa) - # Update spatial state (Forward Euler Approximation) - self.spatial_state += spatial_derivatives * delta_s + # 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 + - self.eps)) + # Assert that unique projections of car pose onto path exists + #assert self.spatial_state.e_y < (1 / (self.current_waypoint.kappa + + # self.eps)) - # Increase waypoint ID - self.wp_id += 1 + # Increase waypoint ID + self.wp_id += 1 - # Update current waypoint - self.current_waypoint = self.reference_path.waypoints[self.wp_id] + # Update current waypoint + self.current_waypoint = self.reference_path.waypoints[self.wp_id] - # Update temporal_state to match spatial state - self.temporal_state = self.s2t() + # Update temporal_state to match spatial state + self.temporal_state = self.s2t() - # Update s | total driven distance along path - self.s += delta_s + # Update s | total driven distance along path + self.s += delta_s - # Linearize model around new operating point - self.A, self.B = self.linearize() + # Linearize model around new operating point + # 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): - def __init__(self, reference_path, e_y, e_psi, v): +class BicycleModel(SpatialBicycleModel): + 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.C2 = 17.06 - self.Cm1 = 12.0 - self.Cm2 = 2.17 - self.Cr2 = 0.1 - self.Cr0 = 0.6 + self.l = 0.06 # 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 delta: steering command | [rad] + :param state: state vector for which to compute derivatives + :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 - v_x = self.spatial_state.v - 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) + e_y, e_psi, t = state + v, delta = input # 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 - 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 - return s_dot, psi_dot, v_dot - - def get_spatial_derivatives(self, D, delta): + def get_spatial_derivatives(self, state, input, kappa): """ Compute spatial derivatives of all state variables for update. - :param D: duty-cycle of DC motor | [-1, 1] - :param delta: steering angle | [rad] + :param state: state vector for which to compute derivatives + :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) - + self.spatial_state.v * delta * self.C1 * np.cos( - self.spatial_state.e_psi)) / s_dot - d_e_psi_d_s = psi_dot / s_dot - self.current_waypoint.kappa - d_v_d_s = v_dot / s_dot + d_e_y_d_s = v * np.sin(e_psi) / s_dot + d_e_psi_d_s = psi_dot / s_dot - kappa + d_t_d_s = 1 / 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 D: reference duty-cycle of DC-motor | [-1, 1] - """ + :param kappa_r: kappa of waypoint around which to linearize + """ - # Get current state | operating point to linearize around - e_y = self.spatial_state.e_y - e_psi = self.spatial_state.e_psi - v = self.spatial_state.v - - # 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] - 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) + # Get linearization state + if kappa_r is None and delta_s is None: + # Get curvature of linearization waypoint + kappa_r = 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 ################### # System Matrices # ################### - # Construct system matrices from Jacobians. Multiply by sampling - # distance delta_s + add identity matrix (Forward Euler Approximation) - A = Ja * delta_s + np.identity(Ja.shape[1]) - B = Jb * delta_s + # Construct Jacobian Matrix + a_1 = np.array([1, delta_s, 0]) + a_2 = np.array([-kappa_r**2*delta_s, 1, 0]) + a_3 = np.array([-kappa_r/v*delta_s, 0, 0]) - return A, B + 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) -########################## -# 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 + return A, B \ No newline at end of file