print(bike.B)
print(bike.C)
print(bike.D)
# ----------------------------------------------------------------------
# LQR controller design
Q = np.eye(bike.A.shape[0]) * 1
R = np.eye(bike.D.shape[1]) * 0.01
noise = 0
# Reference state
x_ref = np.zeros((1, 4))
u_ref = 0
# Create the controll
sys_d = bike.discrete_ss(dt)
K, S, E = controller_design.dlqr(sys_d.A, sys_d.B, Q, R)
sys_c = bike.continuous_ss()
print(K)
# Simulate the response
time = time_vector
sensor_time = [0]
states = [x0]
measurements = [x0]
control_input = [-K @ measurements[-1]]
for idx, t in enumerate(time[1:]):
bike.set_velocity(velocity[idx + 1])
states_dot = bike.A @ states[-1] + np.dot(bike.B, control_input[-1])
states.append(states[-1] + states_dot * (t - time[idx]))
if (t - sensor_time[-1]) >= dt:
bike.set_velocity(velocity)
bike.update_C(C)
print(bike.A)
print(bike.B)
print(bike.C)
print(bike.D)
# ----------------------------------------------------------------------
# LQR controller design
# ----------------------------------------------------------------------
# Create controller
Q_c = np.eye(4)
# Q_c[0,0] *= 100
R_c = np.eye(1) * 0.001
sys = bike.discrete_ss(dt)
K, S, E = controller_design.dlqr(sys.A, sys.B, Q_c, R_c)
# K = np.zeros((1,4))
sys = bike.continuous_ss()
print(K)
# Design the process noise matrix
Q_k = bike.calc_process_noise(q_var, dt)
# Simulate the response
t, states, est_states, sensor_time, measurements, u = controller_design.simulate_kalman(
sys.A,
sys.B,
sys.C,
sys.D,
K,
dt,
roll_gyro_sensor = Sensor(r_var)
steer_gyro_sensor = Sensor(r_var)
sensors = [roll_angle_sensor, steer_angle_sensor, roll_gyro_sensor, steer_gyro_sensor]
# ----------------------------------------------------------------------
# LQR controller design
Q = np.eye(bike.A.shape[0]) * 1
R = np.eye(bike.D.shape[1]) * 0.01
# Design the process noise matrix
Q_k = bike.calc_process_noise(q_var, dt)
# Create the controller
sys = bike.discrete_ss(dt)
K, S, E = controller_design.dlqr(sys.A, sys.B, Q, R)
sys = bike.continuous_ss()
# Simulate the response
t, states, est_states, sensor_time, measurements, u = controller_design.simulate_kalman(sys.A, sys.B, sys.C, sys.D, K, dt, time_vector, r_var, q_var, x0=x0, sensors=sensors, Q=Q_k)
# Plot the results
titles = ['Roll Angle', 'Steer Angle', 'Roll Angular Velocity', 'Steer Angular Velocity']
ylabels = ['Angle [deg]', 'Angle [deg]', 'Angular Velocity [deg/sec]', 'Angular Velocity [deg/sec]']
plt.figure()
for i in range(4):
plt.subplot(2,2,i+1)
plt.plot(t, np.rad2deg(states[:,i]), label=('Real states'), c='k')
plt.scatter(sensor_time, np.rad2deg(measurements[:,i]), label=('Measured states'), s=2)
plt.step(sensor_time, np.rad2deg(est_states[:,i]), label=('Estimated states'), c='r')
if i==3: