def test_flat_default_output(self, vehicle_flat):
# Construct a flat system with the default outputs
flatsys = fs.FlatSystem(vehicle_flat.forward,
vehicle_flat.reverse,
vehicle_flat.updfcn,
inputs=vehicle_flat.ninputs,
outputs=vehicle_flat.ninputs,
states=vehicle_flat.nstates)
# Define the endpoints of the trajectory
x0 = [0., -2., 0.]
u0 = [10., 0.]
xf = [100., 2., 0.]
uf = [10., 0.]
Tf = 10
# Find trajectory between initial and final conditions
poly = fs.PolyFamily(6)
traj1 = fs.point_to_point(vehicle_flat, Tf, x0, u0, xf, uf, basis=poly)
traj2 = fs.point_to_point(flatsys, Tf, x0, u0, xf, uf, basis=poly)
# Verify that the trajectory computation is correct
T = np.linspace(0, Tf, 10)
x1, u1 = traj1.eval(T)
x2, u2 = traj2.eval(T)
np.testing.assert_array_almost_equal(x1, x2)
np.testing.assert_array_almost_equal(u1, u2)
# Run a simulation and verify that the outputs are correct
resp1 = ct.input_output_response(vehicle_flat, T, u1, x0)
resp2 = ct.input_output_response(flatsys, T, u1, x0)
np.testing.assert_array_almost_equal(resp1.outputs[0:2], resp2.outputs)
def test_kinematic_car(self, vehicle_flat, poly):
# Define the endpoints of the trajectory
x0 = [0., -2., 0.]
u0 = [10., 0.]
xf = [100., 2., 0.]
uf = [10., 0.]
Tf = 10
# Find trajectory between initial and final conditions
traj = fs.point_to_point(vehicle_flat, Tf, x0, u0, xf, uf, basis=poly)
# Verify that the trajectory computation is correct
x, u = traj.eval([0, Tf])
np.testing.assert_array_almost_equal(x0, x[:, 0])
np.testing.assert_array_almost_equal(u0, u[:, 0])
np.testing.assert_array_almost_equal(xf, x[:, 1])
np.testing.assert_array_almost_equal(uf, u[:, 1])
# Simulate the system and make sure we stay close to desired traj
T = np.linspace(0, Tf, 100)
xd, ud = traj.eval(T)
resp = ct.input_output_response(vehicle_flat, T, ud, x0)
np.testing.assert_array_almost_equal(resp.states, xd, decimal=2)
# For SciPy 1.0+, integrate equations and compare to desired
if StrictVersion(sp.__version__) >= "1.0":
t, y, x = ct.input_output_response(vehicle_flat,
T,
ud,
x0,
return_x=True)
np.testing.assert_allclose(x, xd, atol=0.01, rtol=0.01)
def test_discrete_lqr():
# oscillator model defined in 2D
# Source: https://www.mpt3.org/UI/RegulationProblem
A = [[0.5403, -0.8415], [0.8415, 0.5403]]
B = [[-0.4597], [0.8415]]
C = [[1, 0]]
D = [[0]]
# Linear discrete-time model with sample time 1
sys = ct.ss2io(ct.ss(A, B, C, D, 1))
# Include weights on states/inputs
Q = np.eye(2)
R = 1
K, S, E = ct.dlqr(A, B, Q, R)
# Compute the integral and terminal cost
integral_cost = opt.quadratic_cost(sys, Q, R)
terminal_cost = opt.quadratic_cost(sys, S, None)
# Solve the LQR problem
lqr_sys = ct.ss2io(ct.ss(A - B @ K, B, C, D, 1))
# Generate a simulation of the LQR controller
time = np.arange(0, 5, 1)
x0 = np.array([1, 1])
_, _, lqr_x = ct.input_output_response(lqr_sys, time, 0, x0, return_x=True)
# Use LQR input as initial guess to avoid convergence/precision issues
lqr_u = np.array(-K @ lqr_x[0:time.size]) # convert from matrix
# Formulate the optimal control problem and compute optimal trajectory
optctrl = opt.OptimalControlProblem(sys,
time,
integral_cost,
terminal_cost=terminal_cost,
initial_guess=lqr_u)
res1 = optctrl.compute_trajectory(x0, return_states=True)
# Compare to make sure results are the same
np.testing.assert_almost_equal(res1.inputs, lqr_u[0])
np.testing.assert_almost_equal(res1.states, lqr_x)
# Add state and input constraints
trajectory_constraints = [
(sp.optimize.LinearConstraint, np.eye(3), [-5, -5, -.5], [5, 5, 0.5]),
]
# Re-solve
res2 = opt.solve_ocp(sys,
time,
x0,
integral_cost,
trajectory_constraints,
terminal_cost=terminal_cost,
initial_guess=lqr_u)
# Make sure we got a different solution
assert np.any(np.abs(res1.inputs - res2.inputs) > 0.1)
def compareKalmanAndSys():
# test kalman estimator & compare non-linear system and linear system
T = np.arange(tspan[0], tspan[1], dt)
uDIST = np.random.multivariate_normal(np.zeros(4), Vd, size=T.shape[0])
uDIST = np.transpose(uDIST)
uNOISE = np.random.normal(0.0, Vn, size=(1, T.shape[0]))
u0 = np.zeros([1, T.shape[0]], dtype=np.float)
u0[:, 100:110] = 20 # impulse
u0[:, 400:420] = -20 # impulse
uAUG = np.concatenate([u0, uDIST, uNOISE, u0])
y0 = []
y0.extend(yInit)
y0.extend(yInit)
t0, yout0 = control.input_output_response(sysPartKF, T, U=uAUG, X0=y0)
t1, yout1 = control.input_output_response(sysFullNonLin, T, U=u0, X0=yInit)
t2, yout2 = control.input_output_response(sysFull,
T,
U=np.concatenate(
[u0, uDIST, uNOISE]),
X0=yInit)
plt.plot(T, yout0[0, :], "k-", label="noise observation of x")
plt.plot(T, yout0[1, :], "r-", label="kalman estimation of x")
plt.plot(T, yout0[2, :], "g-", label="kalman estimation of x_dot")
plt.plot(T, yout0[3, :], "b-", label="kalman estimation of theta")
plt.plot(T, yout0[4, :], "y-", label="kalman estimation of theta_dot")
plt.plot(T, yout1[0, :], "y--", label="non-linear sys state of x")
plt.plot(T, yout1[1, :], "r--", label="non-linear sys state of x_dot")
plt.plot(T, yout1[2, :], "g--", label="non-linear sys state of theta")
plt.plot(T, yout1[3, :], "b--", label="non-linear sys state of theta_dot")
plt.plot(T, yout2[0, :], "y:", label="linear sys state of x")
plt.plot(T, yout2[1, :], "r:", label="linear sys state of x_dot")
plt.plot(T, yout2[2, :], "g:", label="linear sys state of theta")
plt.plot(T, yout2[3, :], "b:", label="linear sys state of theta_dot")
plt.title(
'Compare non-linear system, linear system and kalman estimator on noise linear system'
)
plt.legend(bbox_to_anchor=(-0.05, -0.5, 1.05, 0.25),
loc='lower left',
ncol=3,
mode="expand",
borderaxespad=0.)
plt.subplots_adjust(bottom=0.3)
plt.show()
def test_discrete_lqr():
# oscillator model defined in 2D
# Source: https://www.mpt3.org/UI/RegulationProblem
A = [[0.5403, -0.8415], [0.8415, 0.5403]]
B = [[-0.4597], [0.8415]]
C = [[1, 0]]
D = [[0]]
# Linear discrete-time model with sample time 1
sys = ct.ss2io(ct.ss(A, B, C, D, 1))
# Include weights on states/inputs
Q = np.eye(2)
R = 1
K, S, E = ct.lqr(A, B, Q, R) # note: *continuous* time LQR
# Compute the integral and terminal cost
integral_cost = opt.quadratic_cost(sys, Q, R)
terminal_cost = opt.quadratic_cost(sys, S, None)
# Formulate finite horizon MPC problem
time = np.arange(0, 5, 1)
x0 = np.array([1, 1])
optctrl = opt.OptimalControlProblem(sys,
time,
integral_cost,
terminal_cost=terminal_cost)
res1 = optctrl.compute_trajectory(x0, return_states=True)
with pytest.xfail("discrete LQR not implemented"):
# Result should match LQR
K, S, E = ct.dlqr(A, B, Q, R)
lqr_sys = ct.ss2io(ct.ss(A - B @ K, B, C, D, 1))
_, _, lqr_x = ct.input_output_response(lqr_sys,
time,
0,
x0,
return_x=True)
np.testing.assert_almost_equal(res1.states, lqr_x)
# Add state and input constraints
trajectory_constraints = [
(sp.optimize.LinearConstraint, np.eye(3), [-10, -10, -1], [10, 10, 1]),
]
# Re-solve
res2 = opt.solve_ocp(sys,
time,
x0,
integral_cost,
constraints,
terminal_cost=terminal_cost)
# Make sure we got a different solution
assert np.any(np.abs(res1.inputs - res2.inputs) > 0.1)
def step(self, steering_angle, velocity_dif):
"""
Function responsible for the action transform into the continuous state space.
:param dt: float, time step [sec]
:param steering_angle: float, [rad]
:param velocity_dif: float, difference to previous velocity [m/s]
:return: new_state: containing the input vehicle's new position, orientation, speed, lane.
"""
self.lateral_state['steering_angle'] = steering_angle
self.lateral_state['speed'] += velocity_dif
# Simulate the system + estimator
# Resolution of the trajectory
timesteps = np.array([0, self.dt])
# Velocity array
v_curvy = np.full(timesteps.shape, self.lateral_state['speed'])
# Steering angle array
delta_curvy = np.full(timesteps.shape,
self.lateral_state['steering_angle'])
# Input array
u_curvy = [v_curvy, delta_curvy]
# Initial condition (x, y, heading [rad])
x_0 = [
self.lateral_state['x_position'], self.lateral_state['y_position'],
self.lateral_state['heading']
]
_, _, x_curvy = ct.input_output_response(
self.vehicle,
timesteps,
u_curvy,
x_0,
params=self.vehicle_params,
return_x=True,
)
new_x, new_y, heading = x_curvy[:, -1]
self.pos_log['x_position'].append(new_x)
self.pos_log['y_position'].append(new_y)
self.pos_log['heading'].append(heading)
dif_x = new_x - self.lateral_state['x_position']
dif_y = new_y - self.lateral_state['y_position']
self.update_in_lane_position(dif_x, dif_y, road_curve=None)
self.update_absolute_position(new_x, new_y)
self.lateral_state['heading'] = heading
return self.lateral_state
def test_mpc_iosystem():
# model of an aircraft discretized with 0.2s sampling time
# Source: https://www.mpt3.org/UI/RegulationProblem
A = [[0.99, 0.01, 0.18, -0.09, 0],
[ 0, 0.94, 0, 0.29, 0],
[ 0, 0.14, 0.81, -0.9, 0],
[ 0, -0.2, 0, 0.95, 0],
[ 0, 0.09, 0, 0, 0.9]]
B = [[ 0.01, -0.02],
[-0.14, 0],
[ 0.05, -0.2],
[ 0.02, 0],
[-0.01, 0]]
C = [[0, 1, 0, 0, -1],
[0, 0, 1, 0, 0],
[0, 0, 0, 1, 0],
[1, 0, 0, 0, 0]]
model = ct.ss2io(ct.ss(A, B, C, 0, 0.2))
# For the simulation we need the full state output
sys = ct.ss2io(ct.ss(A, B, np.eye(5), 0, 0.2))
# compute the steady state values for a particular value of the input
ud = np.array([0.8, -0.3])
xd = np.linalg.inv(np.eye(5) - A) @ B @ ud
yd = C @ xd
# provide constraints on the system signals
constraints = [opt.input_range_constraint(sys, [-5, -6], [5, 6])]
# provide penalties on the system signals
Q = model.C.transpose() @ np.diag([10, 10, 10, 10]) @ model.C
R = np.diag([3, 2])
cost = opt.quadratic_cost(model, Q, R, x0=xd, u0=ud)
# online MPC controller object is constructed with a horizon 6
ctrl = opt.create_mpc_iosystem(
model, np.arange(0, 6) * 0.2, cost, constraints)
# Define an I/O system implementing model predictive control
loop = ct.feedback(sys, ctrl, 1)
# Choose a nearby initial condition to speed up computation
X0 = np.hstack([xd, np.kron(ud, np.ones(6))]) * 0.99
Nsim = 12
tout, xout = ct.input_output_response(
loop, np.arange(0, Nsim) * 0.2, 0, X0)
# Make sure the system converged to the desired state
np.testing.assert_allclose(
xout[0:sys.nstates, -1], xd, atol=0.1, rtol=0.01)
def main():
# Initialize gain scheduled controller
# Using two simple P-controllers, see also fcat.longitudinal_controller for more complicated examples
# Note that this is just an example on how to build and simulate an aircraft system
# The controllers are simple not tuned to get desired reference tracking
controllers = [
SaturatedStateSpaceMatricesGS(
A = -1*np.eye(1),
B = np.zeros((1,1)),
C = np.eye(1),
D = np.zeros((1,1)),
upper = 1,
lower = -1,
switch_signal = 0.3
),
SaturatedStateSpaceMatricesGS(
A = -1*np.eye(1),
B = np.zeros((1,1)),
C = np.eye(1),
D = np.zeros((1,1)),
upper = 1,
lower = -1,
switch_signal = 0.6
)
]
# Using similar controllers for simplicity
lat_gs_controller = init_gs_controller(controllers, Direction.LATERAL, 'icing')
lon_gs_controller = init_gs_controller(controllers, Direction.LONGITUDINAL, 'icing')
airspeed_pi_controller = init_airspeed_controller()
control_input = ControlInput()
control_input.throttle = 0.5
state = State()
state.vx = 20
prop = IcedSkywalkerX8Properties(control_input)
aircraft_model = build_nonlin_sys(prop, no_wind(), outputs=State.names+['icing', 'airspeed'])
motor_time_constant = 0.001
elevon_time_constant = 0.001
actuator_model = build_flying_wing_actuator_system(elevon_time_constant, motor_time_constant)
closed_loop = build_closed_loop(actuator_model, aircraft_model, lon_gs_controller, lat_gs_controller, airspeed_pi_controller)
X0 = get_init_vector(closed_loop, control_input, state)
constant_input = np.array([20, 0.04, -0.00033310605950459315])
sim_time = 15
t = np.linspace(0, sim_time, sim_time*5, endpoint=True)
u = np.array([constant_input, ]*(len(t))).transpose()
T, yout_non_lin, _ = input_output_response(
closed_loop, U=u, T=t, X0=X0, return_x=True, method='BDF')
def evalClosedFeedback():
T = np.arange(tspan[0], tspan[1], dt)
uDIST = np.random.multivariate_normal(np.zeros(4), Vd, size=T.shape[0])
uDIST = np.transpose(uDIST)
uNOISE = np.random.normal(0.0, Vn, size=(1, T.shape[0]))
y0 = []
y0.extend(yInit)
y0.extend(yInit)
y0.extend(yInit)
t0, yout0 = control.input_output_response(io_closed,
T,
U=np.concatenate([
Vd @ Vd @ uDIST, Vn * uNOISE,
Vd @ Vd @ uDIST, Vn * uNOISE
]),
X0=y0)
# plt.plot(T, yout0[0, :], "k-", label="noise observation of x")
# plt.plot(T, yout0[1, :], "r-", label="kalman estimation of x")
# plt.plot(T, yout0[2, :], "g-", label="kalman estimation of x_dot")
# plt.plot(T, yout0[3, :], "b-", label="kalman estimation of theta")
# plt.plot(T, yout0[4, :], "y-", label="kalman estimation of theta_dot")
# plt.plot(T, yout0[5, :], "y--", label="non-linear sys state of x")
# plt.plot(T, yout0[6, :], "r--", label="non-linear sys state of x_dot")
# plt.plot(T, yout0[7, :], "g--", label="non-linear sys state of theta")
# plt.plot(T, yout0[8, :], "b--", label="non-linear sys state of theta_dot")
# plt.plot(T, yout0[9, :], "k:", label="LQR control u")
# plt.title('Close loop evaluation')
# plt.legend(bbox_to_anchor=(-0.05, -0.4, 1.05, 0.25), loc='lower left',
# ncol=3, mode="expand", borderaxespad=0.)
# plt.subplots_adjust(bottom=0.3)
# plt.show()
##############################################
# GUI simulated animation
yout = yout0[:, :]
gui = ti.GUI('Sim cartpend', res=(512, 512), background_color=0xdddddd)
t_curr = 0
while gui.running:
drawcartpend(gui, yout[:, t_curr], m, M, L)
gui.show()
t_curr += 1
if (t_curr > yout.shape[1] - 1):
print(yout[:, t_curr - 1])
t_curr = 0
def compute_nonlinear_dynamical_states(initial_state,
T,
Ts,
U,
l_r=0.5,
L=1.0):
"""Compute non-linear dynamical states from bicycle kinematic model.
Parameters
==========
initial_state : ndarray
Initial state [x_0, y_0, psi_0, v_0].
T : int
Timesteps.
Ts : float
Step size in seconds.
U : ndarray
Control inputs of shape (T, 2) [u_1, u_2]
l_r : float
Length of hind wheel to bicycle center of gravity.
L : float
Length of bicycle longitudinal dimension from hind to front wheel.
Returns
=======
ndarray
Trajectory from control of shape (T + 1, 4) with rows [x, y, psi, v].
"""
timestamps = np.linspace(0, Ts * T, T + 1)
mock_inputs = np.concatenate((U, np.array([0, 0])[None]), axis=0).T
io_bicycle_kinematics = control.NonlinearIOSystem(
bicycle_kinematics,
None,
inputs=('u_1', 'u_2'),
outputs=('x', 'y', 'psi', 'v'),
states=('x', 'y', 'psi', 'v'),
params={
'l_r': l_r,
'L': L
},
name='bicycle_kinematics')
_, states = control.input_output_response(io_bicycle_kinematics,
timestamps, mock_inputs,
initial_state)
return states.T
def test_trdata_labels():
# Create an I/O system with labels
sys = ct.rss(4, 3, 2)
iosys = ct.LinearIOSystem(sys)
T = np.linspace(1, 10, 10)
U = [np.sin(T), np.cos(T)]
# Create a response
response = ct.input_output_response(iosys, T, U)
# Make sure the labels got created
np.testing.assert_equal(
response.output_labels, ["y[%d]" % i for i in range(sys.noutputs)])
np.testing.assert_equal(
response.state_labels, ["x[%d]" % i for i in range(sys.nstates)])
np.testing.assert_equal(
response.input_labels, ["u[%d]" % i for i in range(sys.ninputs)])
def test_actuator_model():
control_input = ControlInput()
control_input.elevon_right = 2
control_input.elevon_left = 3
control_input.throttle = 0.5
control_input.rudder = 0
initial_value_elevator = 3
initial_value_aileron = 2
initial_value_throttle = 2
initial_values = np.array([
initial_value_elevator, initial_value_aileron, 0,
initial_value_throttle
])
initial_values_flying_wing = np.linalg.inv(
flying_wing2ctrl_input_matrix()).dot(initial_values)
initial_value_elevon_right = initial_values_flying_wing[0]
initial_value_elevon_left = initial_values_flying_wing[1]
initial_value_throttle = initial_values_flying_wing[3]
elevon_time_constant = 0.3
motor_time_constant = 0.2
lin_model = build_flying_wing_actuator_system(elevon_time_constant,
motor_time_constant)
t = np.linspace(0.0, 10, 500, endpoint=True)
u = np.array([
control_input.control_input,
] * len(t)).transpose()
T, yout = input_output_response(lin_model, t, U=u, X0=initial_values)
yout = np.linalg.inv(flying_wing2ctrl_input_matrix()).dot(yout)
expect_elevon_r = control_input.elevon_right + \
(initial_value_elevon_right - control_input.elevon_right)*np.exp(-t/elevon_time_constant)
expect_elevon_l = control_input.elevon_left + \
(initial_value_elevon_left - control_input.elevon_left)*np.exp(-t/elevon_time_constant)
expect_motor = control_input.throttle + \
(initial_value_throttle - control_input.throttle)*np.exp(-t/motor_time_constant)
assert np.allclose(expect_elevon_r, yout[0, :], atol=5e-3)
assert np.allclose(expect_elevon_l, yout[1, :], atol=5e-3)
assert np.allclose(expect_motor, yout[3, :], atol=5e-3)
def compute_nonlinear_dynamical_states(initial_state,
T,
Ts,
U,
l_r=0.5,
L=1.0):
"""
Parameters
==========
initial_state : ndarray
Initial state [x_0, y_0, psi_0, v_0, delta_0].
T : int
Timesteps.
Ts : float
Step size in seconds.
U : ndarray
Control inputs of shape (T, 2) [u_1, u_2]
Returns
=======
ndarray
Trajectory from control of shape (T + 1, 5) with rows [x, y, psi, v, delta].
"""
timestamps = np.linspace(0, Ts * T, T + 1)
mock_inputs = np.concatenate((U, np.array([0, 0])[None]), axis=0).T
io_bicycle_kinematics = control.NonlinearIOSystem(
bicycle_kinematics,
None,
inputs=('u_1', 'u_2'),
outputs=('x', 'y', 'psi', 'v', 'delta'),
states=('x', 'y', 'psi', 'v', 'delta'),
params={
'l_r': l_r,
'L': L
},
name='bicycle_kinematics')
_, states = control.input_output_response(io_bicycle_kinematics,
timestamps, mock_inputs,
initial_state)
return states.T
def states_from_control(self, x_init, U):
"""Compute non-linear states from vehicle model
given initial states and control variables over time.
Parameters
==========
x_init : ndarray
Initial state [x_0, y_0, psi_0, v_0].
U : ndarray
Control variables u[i] for i = 0..T - 1 with rows [u_1, u_2]
Returns
=======
ndarray
State trajectory from control including origin
of shape (T + 1, 4) with rows [x, y, psi, v].
"""
U_pad = np.concatenate((U, U[-1][None]))
_, states = control.input_output_response(self.io_dynamical_system,
self.timestamps, U_pad.T,
x_init)
return states.T
def test_kinematic_car(self):
"""Differential flatness for a kinematic car"""
def vehicle_flat_forward(x, u, params={}):
b = params.get('wheelbase', 3.) # get parameter values
zflag = [np.zeros(3), np.zeros(3)] # list for flag arrays
zflag[0][0] = x[0] # flat outputs
zflag[1][0] = x[1]
zflag[0][1] = u[0] * np.cos(x[2]) # first derivatives
zflag[1][1] = u[0] * np.sin(x[2])
thdot = (u[0] / b) * np.tan(u[1]) # dtheta/dt
zflag[0][2] = -u[0] * thdot * np.sin(x[2]) # second derivatives
zflag[1][2] = u[0] * thdot * np.cos(x[2])
return zflag
def vehicle_flat_reverse(zflag, params={}):
b = params.get('wheelbase', 3.) # get parameter values
x = np.zeros(3)
u = np.zeros(2) # vectors to store x, u
x[0] = zflag[0][0] # x position
x[1] = zflag[1][0] # y position
x[2] = np.arctan2(zflag[1][1], zflag[0][1]) # angle
u[0] = zflag[0][1] * np.cos(x[2]) + zflag[1][1] * np.sin(x[2])
thdot_v = zflag[1][2] * np.cos(x[2]) - zflag[0][2] * np.sin(x[2])
u[1] = np.arctan2(thdot_v, u[0]**2 / b)
return x, u
def vehicle_update(t, x, u, params):
b = params.get('wheelbase', 3.) # get parameter values
dx = np.array([
np.cos(x[2]) * u[0],
np.sin(x[2]) * u[0], (u[0] / b) * np.tan(u[1])
])
return dx
def vehicle_output(t, x, u, params):
return x
# Create differentially flat input/output system
vehicle_flat = fs.FlatSystem(vehicle_flat_forward,
vehicle_flat_reverse,
vehicle_update,
vehicle_output,
inputs=('v', 'delta'),
outputs=('x', 'y', 'theta'),
states=('x', 'y', 'theta'))
# Define the endpoints of the trajectory
x0 = [0., -2., 0.]
u0 = [10., 0.]
xf = [100., 2., 0.]
uf = [10., 0.]
Tf = 10
# Define a set of basis functions to use for the trajectories
poly = fs.PolyFamily(6)
# Find trajectory between initial and final conditions
traj = fs.point_to_point(vehicle_flat, x0, u0, xf, uf, Tf, basis=poly)
# Verify that the trajectory computation is correct
x, u = traj.eval([0, Tf])
np.testing.assert_array_almost_equal(x0, x[:, 0])
np.testing.assert_array_almost_equal(u0, u[:, 0])
np.testing.assert_array_almost_equal(xf, x[:, 1])
np.testing.assert_array_almost_equal(uf, u[:, 1])
# Simulate the system and make sure we stay close to desired traj
T = np.linspace(0, Tf, 500)
xd, ud = traj.eval(T)
# For SciPy 1.0+, integrate equations and compare to desired
if StrictVersion(sp.__version__) >= "1.0":
t, y, x = ct.input_output_response(vehicle_flat,
T,
ud,
x0,
return_x=True)
np.testing.assert_allclose(x, xd, atol=0.01, rtol=0.01)
# System inputs
inplist=['target.v_ref'],
inputs=['v_ref'],
# System outputs
outlist=['vehicle.x', 'vehicle.y' , 'vehicle.yaw','controller.delta','target.yaw_r'],
outputs=['x', 'y', 'psi', 'delta', 'psi_ref']
)
###############################################################################
# Input Output Response
###############################################################################
# time of response
T = np.linspace(0,simulation_time,simulation_sample)
# the response
tout, yout = ct.input_output_response(LatRearWheelFeedbackControl, T, [v_ref*np.ones(len(T))],X0=[init_x,init_y,init_yaw])
err_curvature = []
for i in range(len(tout)):
ex = [yout[0][i] - icx for icx in target_curvature_sets.x]
ey = [yout[1][i] - icy for icy in target_curvature_sets.y]
d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(ex, ey)]
mind = min(d)
index = d.index(mind)
err_curvature.append(np.sqrt(mind))
plt.figure()
plt.title('Tracking')
def test_squeeze(self, fcn, nstate, nout, ninp, squeeze, shape1, shape2):
# Figure out if we have SciPy 1+
scipy0 = StrictVersion(sp.__version__) < '1.0'
# Define the system
if fcn == ct.tf and (nout > 1 or ninp > 1) and not slycot_check():
pytest.skip("Conversion of MIMO systems to transfer functions "
"requires slycot.")
else:
sys = fcn(ct.rss(nstate, nout, ninp, strictly_proper=True))
# Generate the time and input vectors
tvec = np.linspace(0, 1, 8)
uvec = np.ones((sys.ninputs, 1)) @ np.reshape(np.sin(tvec), (1, 8))
#
# Pass squeeze argument and make sure the shape is correct
#
# For responses that are indexed by the input, check against shape1
# For responses that have no/fixed input, check against shape2
#
# Impulse response
if isinstance(sys, StateSpace):
# Check the states as well
_, yvec, xvec = ct.impulse_response(
sys, tvec, squeeze=squeeze, return_x=True)
if sys.issiso():
assert xvec.shape == (sys.nstates, 8)
else:
assert xvec.shape == (sys.nstates, sys.ninputs, 8)
else:
_, yvec = ct.impulse_response(sys, tvec, squeeze=squeeze)
assert yvec.shape == shape1
# Step response
if isinstance(sys, StateSpace):
# Check the states as well
_, yvec, xvec = ct.step_response(
sys, tvec, squeeze=squeeze, return_x=True)
if sys.issiso():
assert xvec.shape == (sys.nstates, 8)
else:
assert xvec.shape == (sys.nstates, sys.ninputs, 8)
else:
_, yvec = ct.step_response(sys, tvec, squeeze=squeeze)
assert yvec.shape == shape1
# Initial response (only indexed by output)
if isinstance(sys, StateSpace):
# Check the states as well
_, yvec, xvec = ct.initial_response(
sys, tvec, 1, squeeze=squeeze, return_x=True)
assert xvec.shape == (sys.nstates, 8)
else:
_, yvec = ct.initial_response(sys, tvec, 1, squeeze=squeeze)
assert yvec.shape == shape2
# Forced response (only indexed by output)
if isinstance(sys, StateSpace):
# Check the states as well
_, yvec, xvec = ct.forced_response(
sys, tvec, uvec, 0, return_x=True, squeeze=squeeze)
assert xvec.shape == (sys.nstates, 8)
else:
# Just check the input/output response
_, yvec = ct.forced_response(sys, tvec, uvec, 0, squeeze=squeeze)
assert yvec.shape == shape2
# Test cases where we choose a subset of inputs and outputs
_, yvec = ct.step_response(
sys, tvec, input=ninp-1, output=nout-1, squeeze=squeeze)
if squeeze is False:
# Shape should be unsqueezed
assert yvec.shape == (1, 1, 8)
else:
# Shape should be squeezed
assert yvec.shape == (8, )
# For InputOutputSystems, also test input/output response
if isinstance(sys, ct.InputOutputSystem) and not scipy0:
_, yvec = ct.input_output_response(sys, tvec, uvec, squeeze=squeeze)
assert yvec.shape == shape2
#
# Changing config.default to False should return 3D frequency response
#
ct.config.set_defaults('control', squeeze_time_response=False)
_, yvec = ct.impulse_response(sys, tvec)
if squeeze is not True or sys.ninputs > 1 or sys.noutputs > 1:
assert yvec.shape == (sys.noutputs, sys.ninputs, 8)
_, yvec = ct.step_response(sys, tvec)
if squeeze is not True or sys.ninputs > 1 or sys.noutputs > 1:
assert yvec.shape == (sys.noutputs, sys.ninputs, 8)
_, yvec = ct.initial_response(sys, tvec, 1)
if squeeze is not True or sys.noutputs > 1:
assert yvec.shape == (sys.noutputs, 8)
if isinstance(sys, ct.StateSpace):
_, yvec, xvec = ct.forced_response(
sys, tvec, uvec, 0, return_x=True)
assert xvec.shape == (sys.nstates, 8)
else:
_, yvec = ct.forced_response(sys, tvec, uvec, 0)
if squeeze is not True or sys.noutputs > 1:
assert yvec.shape == (sys.noutputs, 8)
# For InputOutputSystems, also test input_output_response
if isinstance(sys, ct.InputOutputSystem) and not scipy0:
_, yvec = ct.input_output_response(sys, tvec, uvec)
if squeeze is not True or sys.noutputs > 1:
assert yvec.shape == (sys.noutputs, 8)
# System parameters
wheelbase = vehicle_params['wheelbase'] # coming from part1
v0 = vehicle_params['velocity'] # Dictionary parameters membership key
# Control inputs
T_curvy = np.linspace(0, 7, 500)
v_curvy = v0 * np.ones(T_curvy.shape)
delta_curvy = 0.1 * np.sin(T_curvy) * np.cos(4 * T_curvy) + 0.0025 * np.sin(
T_curvy * np.pi / 7)
u_curvy = [v_curvy, delta_curvy]
X0_curvy = [0, 0.8, 0]
# Simulate the system + estimator
t_curvy, y_curvy, x_curvy = ct.input_output_response(vehicle,
T_curvy,
u_curvy,
X0_curvy,
params=vehicle_params,
return_x=True)
# Configure matplotlib plots to be a bit bigger and optimize layout
plt.figure(figsize=[9, 4.5])
# Plot the resulting trajectory (and some road boundaries)
plt.subplot(
1, 4, 2) # Add an Axes to the current figure or retrieve an existing Axes
plt.plot(y_curvy[1], y_curvy[0]) # Plot y versus x as lines and/or markers.
plt.plot(y_curvy[1] - 9 / np.cos(x_curvy[2]), y_curvy[0], 'k-', linewidth=1)
plt.plot(y_curvy[1] - 3 / np.cos(x_curvy[2]), y_curvy[0], 'k--', linewidth=1)
plt.plot(y_curvy[1] + 3 / np.cos(x_curvy[2]), y_curvy[0], 'k-', linewidth=1)
plt.xlabel('y [m]') # Set the label for the x-axis.
if False:
dt = 1 # min
Tmod = np.arange(0, 24 * 60, dt) # 24 hours by minutes
Rmod = np.zeros(len(Tmod))
Cmod = Rmod
for i, t in enumerate(Tmod):
if t <= 300:
Rmod[i] = 188 - tamb # denature phase
Cmod[i] = 188
else:
Rmod[i] = 94 - tamb # fermentation
Cmod[i] = 94
# closed loop non-linear response (model)
t4, y4 = ctl.input_output_response(clsysSat, Tmod, [Rmod])
# closed loop non-linear control effort (model)
clsysEFF = ctl.feedback(
ctl.series(ctl_PID_IO, heater_IO), plant_IO,
sign=-1) # negative fb closed loop system w/ Saturation
t5, yCEF = ctl.input_output_response(clsysEFF, Tmod, [Rmod])
th = np.zeros(np.shape(t4))
for i in range(np.shape(t4)[0]):
#y4[i] = tamb + y4[i] # add back in ambient
th[i] = t4[i] / 60.00 # time to hours
ax, fig = plt.subplots()
#plt.plot(th,y4,th,yCEF,th,Cmod,th,Rmod) #
plt.plot(th, y4, th, yCEF, th, Cmod) #,th,Rmod) #
def main():
# Script showing how to simulate icedskywalkerX8 from config_file using controllers saved in file
config_file = "examples/skywalkerX8_linearize.yml"
lat_controller_file = "examples/inner_loop_controllers/single_robust_roll_ctrl.json"
lon_controller_file = "examples/inner_loop_controllers/single_robust_pitch_ctrl.json"
K_lat = get_state_space_from_file(lat_controller_file)
K_lon = get_state_space_from_file(lon_controller_file)
K_lat = SaturatedStateSpaceController(A=np.array(K_lat.A),
B=np.array(K_lat.B),
C=np.array(K_lat.C),
D=np.array(K_lat.D),
lower=-0.4,
upper=0.4)
K_lon = SaturatedStateSpaceController(A=np.array(K_lon.A),
B=np.array(K_lon.B),
C=np.array(K_lon.C),
D=np.array(K_lon.D),
lower=-0.4,
upper=0.4)
# Using similar controllers for simplicity
lat_controller = init_robust_controller(K_lat, Direction.LATERAL)
lon_controller = init_robust_controller(K_lon, Direction.LONGITUDINAL)
airspeed_pi_controller = init_airspeed_controller()
with open(config_file, 'r') as infile:
data = load(infile)
aircraft = aircraft_property_from_dct(data['aircraft'])
ctrl = ControlInput.from_dict(data['init_control'])
state = State.from_dict(data['init_state'])
updates = {
'icing': [PropUpdate(0.5, 0.5),
PropUpdate(5.0, 1.0)],
}
updater = PropertyUpdater(updates)
aircraft_model = build_nonlin_sys(aircraft,
no_wind(),
outputs=State.names +
['icing', 'airspeed'],
prop_updater=updater)
actuator = actuator_from_dct(data['actuator'])
closed_loop = build_closed_loop(actuator, aircraft_model, lon_controller,
lat_controller, airspeed_pi_controller)
X0 = get_init_vector(closed_loop, ctrl, state)
constant_input = np.array([20, 0.2, -0.2])
sim_time = 15
t = np.linspace(0, sim_time, sim_time * 5, endpoint=True)
u = np.array([
constant_input,
] * (len(t))).transpose()
T, yout_non_lin, _ = input_output_response(closed_loop,
U=u,
T=t,
X0=X0,
return_x=True,
method='BDF')
plot_respons(T, yout_non_lin)
plt.show()
def fil_y(pi_a, time, y_val, fil_y_val):
fil_y_val.put(control.input_output_response(pi_a, time, y_val))
return
#n = 3
#m = 1
# Pi CONTROLLER
n = 4
m = 1
result = Queue()
phi = np.zeros((n + m + 1, len(u)))
phi_hat = np.zeros((n + m + 1, len(u)))
A = np.array([1, 10, 100, 100, 10000]) # INITIAL ESTIMATE
for i in range(n):
p_i = np.zeros(n)
p_i[i] = 1
F = control.tf(p_i, A) # P^n/A(p)
F = control.tf2io(F)
Yf = control.input_output_response(F, t, y)
phi[i] = -Yf[1].T
for i in range(m + 1):
p_i = np.zeros(m + 1)
p_i[i] = 1
Uf = control.tf(p_i, A) # P^m/A(p)
Uf = control.tf2io(Uf)
Uf = control.input_output_response(Uf, t, u)
phi[n + i] = Uf[1].T
Pn = np.zeros(n + 1)
Pn[0] = 1
yfn = control.tf(Pn, A)
yfn = control.tf2io(yfn)
yfn = control.input_output_response(yfn, t, y)
front = np.matmul(phi, phi.T)
# Set initial conditions
X0 = np.zeros((4, 1))
X0[0] = 1
X0[1] = J_HAT_0
X0[2] = B_HAT_0
# Define simulation time span and control input
T = np.linspace(0, T_F, N_POINTS)
U = np.zeros((2, N_POINTS))
XD_IN = np.sin(0.2 * np.pi * T)
XD_DOT_IN = np.cos(0.2 * np.pi * T)
U[0, :] = XD_IN
U[1, :] = XD_DOT_IN
# Simulate the system
T_OUT, Y_OUT = control.input_output_response(IO_CLOSED, T, U, X0)
# Plot the response
plt.rc('text', usetex=True)
plt.rc('font', family='sans')
FIG = plt.figure(1, figsize=(6, 6), dpi=300, facecolor='w', edgecolor='k')
RED = '#f62d73'
BLUE = '#1269d3'
WHITE = '#ffffff'
AX_1 = FIG.add_subplot(3, 1, 1)
AX_1.plot(T, XD_IN, label=r'$x_{d}$', color=RED)
AX_1.plot(T_OUT, Y_OUT[0], label=r'$x$', color=BLUE)
AX_1.set_ylabel('Motor Velocity')
yref = 1
T = np.linspace(0, 5, 100)
# Set up a figure for plotting the results
mpl.figure()
# Plot the reference trajectory for the y position
mpl.plot([0, 5], [yref, yref], 'k--')
# Find the signals we want to plot
y_index = steering.find_output('y')
v_index = steering.find_output('v')
# Do an iteration through different speeds
for vref in [8, 10, 12]:
# Simulate the closed loop controller response
tout, yout = ct.input_output_response(
steering, T, [vref * np.ones(len(T)), yref * np.ones(len(T))])
# Plot the reference speed
mpl.plot([0, 5], [vref, vref], 'k--')
# Plot the system output
y_line, = mpl.plot(tout, yout[y_index, :], 'r') # lateral position
v_line, = mpl.plot(tout, yout[v_index, :], 'b') # vehicle velocity
# Add axis labels
mpl.xlabel('Time (s)')
mpl.ylabel('x vel (m/s), y pos (m)')
mpl.legend((v_line, y_line), ('v', 'y'), loc='center right', frameon=False)
def test_kinematics():
control_input = ControlInput()
prop = FrictionlessBall(control_input)
outputs = [
"x", "y", "z", "roll", "pitch", "yaw", "vx", "vy", "vz", "ang_rate_x",
"ang_rate_y", "ang_rate_z"
]
system = build_nonlin_sys(prop, no_wind(), outputs)
t = np.linspace(0.0, 10, 500, endpoint=True)
state = State()
state.vx = 20.0
state.vy = 1
for i in range(3):
state.roll = 0
state.pitch = 0
state.yaw = 0
if i == 0:
state.ang_rate_x = 0.157079633
state.ang_rate_y = 0
state.ang_rate_z = 0
elif i == 1:
state.ang_rate_x = 0
state.ang_rate_y = 0.157079633
state.ang_rate_z = 0
elif i == 2:
state.ang_rate_x = 0
state.ang_rate_y = 0
state.ang_rate_z = 0.157079633
T, yout = input_output_response(system, t, U=0, X0=state.state)
pos = np.array(yout[:3])
eul_ang = np.array(yout[3:6])
vel_inertial = np.array(
[np.zeros(len(t)),
np.zeros(len(t)),
np.zeros(len(t))])
for j in range(len(vel_inertial[0])):
state.roll = yout[3, j]
state.pitch = yout[4, j]
state.yaw = yout[5, j]
vel = np.array([yout[6, j], yout[7, j], yout[8, j]])
vel_inertial_elem = body2inertial(vel, state)
vel_inertial[0, j] = vel_inertial_elem[0]
vel_inertial[1, j] = vel_inertial_elem[1]
vel_inertial[2, j] = vel_inertial_elem[2]
vx_inertial_expect = 20 * np.ones(len(t))
vy_inertial_expect = 1 * np.ones(len(t))
vz_inertial_expect = GRAVITY_CONST * t
x_expect = 20 * t
y_expect = 1 * t
z_expect = 0.5 * GRAVITY_CONST * t**2
roll_expect = state.ang_rate_x * t
pitch_expect = state.ang_rate_y * t
yaw_expect = state.ang_rate_z * t
assert np.allclose(vx_inertial_expect, vel_inertial[0], atol=7e-3)
assert np.allclose(vy_inertial_expect, vel_inertial[1], atol=5e-3)
assert np.allclose(vz_inertial_expect, vel_inertial[2], atol=8e-3)
assert np.allclose(x_expect, pos[0], atol=8e-2)
assert np.allclose(y_expect, pos[1], atol=5e-2)
assert np.allclose(z_expect, pos[2], atol=7e-2)
assert np.allclose(roll_expect, eul_ang[0], atol=1e-3)
assert np.allclose(pitch_expect, eul_ang[1], atol=1e-3)
assert np.allclose(yaw_expect, eul_ang[2], atol=1e-3)
from matplotlib import pyplot as plt
print("Reading Data")
y = np.loadtxt("KHU_KongBot2/SRIVC/RefinedY.txt", delimiter=",") # linux
u = np.loadtxt("KHU_KongBot2/SRIVC/RefinedU.txt", delimiter=",") # linux
t = u[0:15000, 0]
y = y[0:15000, 1]
u = u[0:15000, 1]
theta=np.loadtxt("KHU_KongBot2/SRIVC/PD_control_theta.txt")
"""
PD_Control
452.2 s + 5781
--------------------------------
s^3 + 38.85 s^2 + 851.1 s + 5774
"""
n=3
m=1
A = theta[0:n]
A = np.insert(A, 0, 1)
B = theta[n:]
sys=control.tf(B,A)
print(sys)
sys=control.tf2io(sys)
y_m=control.input_output_response(sys,t,u)
fig = plt.figure()
plt.plot(t, u, color="b")
plt.plot(t, y, color='r')
plt.plot(y_m[0], y_m[1], color="g")
plt.show()
# System inputs
inplist=['target.v_ref'],
inputs=['vref'],
# System outputs
outlist=['vehicle.x', 'vehicle.y', 'vehicle.psi', 'vehicle.delta'],
outputs=['x', 'y', 'psi', 'delta'])
###############################################################################
# Input Output Response
###############################################################################
# time of response
T = np.linspace(0, 10, 1000)
# the response
tout, yout = ct.input_output_response(LatSlidingModeControl,
T, [vref * np.ones(len(T))],
X0=[0.1, 0.1, 0.1, 0.0, 5.0, 0.31, 0.0])
#tout, yout = ct.input_output_response(vehicle, T, [vref*np.ones(len(T)),target_delta*np.ones(len(T)),delta_rate*np.ones(len(T))],X0=[0,5.0,0,0])
#plt.figure()
#plt.title('track')
#plt.xlabel('x[m]')
#plt.ylabel('y[m]')
#plt.plot(yout[0],yout[1])
#
#
#plt.figure()
#plt.title('delta')
#plt.xlabel('t[s]')
#plt.ylabel('delta[deg]')
def test_optimal_doc():
"""Test optimal control problem from documentation"""
def vehicle_update(t, x, u, params):
# Get the parameters for the model
l = params.get('wheelbase', 3.) # vehicle wheelbase
phimax = params.get('maxsteer', 0.5) # max steering angle (rad)
# Saturate the steering input
phi = np.clip(u[1], -phimax, phimax)
# Return the derivative of the state
return np.array([
np.cos(x[2]) * u[0], # xdot = cos(theta) v
np.sin(x[2]) * u[0], # ydot = sin(theta) v
(u[0] / l) * np.tan(phi) # thdot = v/l tan(phi)
])
def vehicle_output(t, x, u, params):
return x # return x, y, theta (full state)
# Define the vehicle steering dynamics as an input/output system
vehicle = ct.NonlinearIOSystem(vehicle_update,
vehicle_output,
states=3,
name='vehicle',
inputs=('v', 'phi'),
outputs=('x', 'y', 'theta'))
# Define the initial and final points and time interval
x0 = [0., -2., 0.]
u0 = [10., 0.]
xf = [100., 2., 0.]
uf = [10., 0.]
Tf = 10
# Define the cost functions
Q = np.diag([0, 0, 0.1]) # don't turn too sharply
R = np.diag([1, 1]) # keep inputs small
P = np.diag([1000, 1000, 1000]) # get close to final point
traj_cost = opt.quadratic_cost(vehicle, Q, R, x0=xf, u0=uf)
term_cost = opt.quadratic_cost(vehicle, P, 0, x0=xf)
# Define the constraints
constraints = [opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1])]
# Solve the optimal control problem
horizon = np.linspace(0, Tf, 3, endpoint=True)
result = opt.solve_ocp(vehicle,
horizon,
x0,
traj_cost,
constraints,
terminal_cost=term_cost,
initial_guess=u0)
# Make sure the resulting trajectory generate a good solution
resp = ct.input_output_response(vehicle,
horizon,
result.inputs,
x0,
t_eval=np.linspace(0, Tf, 10))
t, y = resp
assert (y[0, -1] - xf[0]) / xf[0] < 0.01
assert (y[1, -1] - xf[1]) / xf[1] < 0.01
assert y[2, -1] < 0.1
xf = [40., 2., 0.]
uf = [10., 0.]
Tf = 4
# Define a set of basis functions to use for the trajectories
poly = fs.PolyFamily(6)
# Find a trajectory between the initial condition and the final condition
traj = fs.point_to_point(vehicle_flat, x0, u0, xf, uf, Tf, basis=poly)
# Create the desired trajectory between the initial and final condition
T = np.linspace(0, Tf, 500)
xd, ud = traj.eval(T)
# Simulation the open system dynamics with the full input
t, y, x = ct.input_output_response(vehicle_flat, T, ud, x0, return_x=True)
# Plot the open loop system dynamics
plt.figure()
plt.suptitle("Open loop trajectory for kinematic car lane change")
# Plot the trajectory in xy coordinates
plt.subplot(4, 1, 2)
plt.plot(x[0], x[1])
plt.xlabel('x [m]')
plt.ylabel('y [m]')
plt.axis([x0[0], xf[0], x0[1] - 1, xf[1] + 1])
# Time traces of the state and input
plt.subplot(2, 4, 5)
plt.plot(t, x[1])
# Open-loop: 5 * (dy^2/dt^2)(t) + 4 * (dy/dt)(t) + 3 * y(t) = 2 * u(t)
# Avec y(t) la sortie au temps t et u(t) la commande au temps t
# On modelise ce systeme en temps discret (dt = 50ms, comme pour le ball and beam)
dt = 0.05
# On utilise la fonction de transfert (facile a calculer), puis on transforme en representation
# d'etat, car c'est necessaire pour ct.iosys.LinearIOSystem.
syst_open_tf = ct.tf(np.array([2]), np.array([5, 4, 3]), dt)
syst_open_ss = ct.tf2ss(syst_open_tf)
# Creation d'un objet input/output pour modeliser la boucle ouverte.
syst_open_io = ct.iosys.LinearIOSystem(syst_open_ss)
# Analyse de la reponse du systeme a un signal en entree pendant 10s.
t_in = np.arange(0, 10, dt)
u_in = np.sin(t_in)
t_out, y_open = ct.input_output_response(syst_open_io,
T=t_in,
U=u_in,
squeeze=True)
plt.plot(t_in, u_in, label="Command u(t)")
plt.plot(t_out, y_open, label="Output y(t)")
plt.title("Response of the open-loop system to a sinewave")
plt.grid()
plt.xlabel("Time t[s]")
plt.ylabel("e.g. Voltage u[V]")
plt.legend()
plt.show()