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: