cost = integral x.T*Q*x + u.T*R*u
"""
#ref Bertsekas, p.151
#first, try to solve the ricatti equation
X = np.matrix(scipy.linalg.solve_continuous_are(self.A, self.B, Q, R))
# X = np.matrix(scipy.linalg.solve_discrete_are(self.A, self.B, Q, R))
#compute the LQR gain
self.K = np.matrix(scipy.linalg.inv(R) @ (self.B.T @ X))
# eigVals, eigVecs = scipy.linalg.eig(A-B@K)
if __name__ == '__main__':
drone = quad(1, .5, .5, .5)
A = drone.get_jacobian()
B = drone.get_B()
C = np.eye(12)
D = np.zeros((12, 4), dtype=np.float64)
Q = np.diag([100, 100, 100, 1, 1, 1, 1, 1, 1, 100, 100, 100])
R = np.diag([1, 1, 1, 1]) * 0.1
X = np.zeros(12)[:, None]
X[-3] = 1
X[-1] = 1
sys = System(A, B, C, D, X, Q, R)
presentation = Visualizer(sys)
presentation.animate(10)
