# -------- Settings ------------
NUM_OF_TIMESTAMP = 50
# -------- Main Code ----------
pos_setpoint = 5
vel_setpoint = 0
Q = np.array([[1, 0],
[0, 1]]) # controls how much state difference cost
R = 0.01 # controls how much control cost.
state_setpoint = np.array([[pos_setpoint], [vel_setpoint]])
initial_covariance = np.eye(2)
my_controller = LQR(env.A, env.B, Q, R)
gain = my_controller.get_K(10) # note that the minus sign is omitted here because of the way we calculate error.
print('gain: {}'.format(gain))
true_state = [env.state]
accel_cmd = [0]
for i in range(NUM_OF_TIMESTAMP):
accel = np.dot(gain, (state_setpoint - env.state))[0, 0]
meas = env.control_and_sense(accel)
print("True state: {}".format(env.state))
print("accel : {}".format(accel))
true_state.append(env.state)
accel_cmd.append(accel)
