def __init__(self, dt):
"""
dt: discretization time
"""
Y = nlmodel.full()
param = nlmodel.param()
self.dt = dt
self.Y = Y.subs(param)
self.DY = self.Y.jacobian(X)
# self.DF = self.Y.jacobian(Matrix([X[ST_V], X[ST_W]]))
self.r = ode(f).set_integrator("dopri5") # , atol=[1e-1, 1e-3], rtol=[1e-1, 1e-1])
"""muAO-MPC system module."""
from sympy import symbols
import numpy as np
from numpy import diag, eye
import nlmodel
from nlmodel import IN_NUM, ST_NUM, ST_V
# SET POINT: this has to be considered in the online implementation
x_sp = np.zeros(ST_NUM) # all setpoints are zero, unless otherwise stated
x_sp[ST_V] = 0.7 # m/s, band and car forward speed
xd = nlmodel.full()
param = nlmodel.param()
p6 = symbols('p6')
param[p6] = 0. # epsilon not necessary for linearization
Ac_sym, Bc_sym, u_sp = nlmodel.compute_linear_system_matrices(xd, param, x_sp)
Ac = np.array(Ac_sym).astype('float64')
Bc = np.array(Bc_sym).astype('float64')
u_sp = np.array(u_sp).astype('float64')
#Bc[ST_V, IN_M] =/ 2. # Take away the u0**2, the model is affine in inputs
dt = 0.005
N = 10
Q = eye(ST_NUM) * 1e1
R = eye(IN_NUM) * 1e-3
P = 'auto'
u2_b = np.deg2rad(15)
u1_lb = 0.
u1_ub = 0.2
