-
Notifications
You must be signed in to change notification settings - Fork 0
/
lqr_01.py
executable file
·75 lines (58 loc) · 2.16 KB
/
lqr_01.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Fri Sep 20 19:20:41 2019
@author: emil
"""
import numpy as np
import scipy.signal as signal
import scipy
from matplotlib import pyplot as plt
class system:
def __init__(self):
self.A = np.array([[0,1],[-1,0]], dtype = np.float64)
self.B = np.array([[0],[1]], dtype = np.float64)
self.C = np.array([[1,0],[0,1]], dtype = np.float64)
self.D = np.array([[0],[0]], dtype = np.float64)
self.x = np.array([[0],[0]], dtype = np.float64)
self.delta = 1e-2
self.discretize(self.delta)
self.lqr(np.diag(np.array([1.3,-1])),np.diag(np.array([1])))
def discretize(self, delta):
self.delta = delta
self.Ad = scipy.linalg.expm(self.A*delta)
shape = self.A.shape
self.Bd = np.linalg.inv(self.A)@(scipy.linalg.expm(self.A*delta)-np.eye(shape[0],shape[1]))@self.B
# (scipy.linalg.expm(self.A*delta)-np.eye(shape[0],shape[1]))@self.B
self.Cd = self.C
self.Dd = self.D
def iterate(self, u):
self.x = [email protected] + self.Bd@u
y = [email protected] + self.Dd@u
def simluate(self, init, time):
out = np.empty((0,2))
self.x = init
u = np.array([[0]])
for i in np.arange(0,time,self.delta):
u = -self.K@(self.x - np.array([[-1],[0]]))
self.y = [email protected] + self.Dd@u
out = np.vstack((out, np.squeeze(self.y)))
self.iterate(u)
return out
def lqr(self, Q,R):
"""Solve the continuous time lqr controller.
dx/dt = A x + B u
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__':
print('run')
sys = system()
out = sys.simluate(np.array([[1],[0]]), 10)
plt.plot(out[:,0])