Commit 0b030461 authored by Emmanuel PIGNAT's avatar Emmanuel PIGNAT
Browse files

update lqr

(cherry picked from commit 56196082)
parent e3c16bfb
......@@ -19,6 +19,28 @@ class LQR(object):
self._seq_xi, self._seq_u = None, None
self._S, self._v, self._K, self._Kv, self._ds, self._Q = \
None, None, None, None, None, None
@property
def K(self):
assert self._K is not None, "Solve Ricatti before"
return self._K
@property
def Q(self):
assert self._Q is not None, "Solve Ricatti before"
return self._Q
@property
def ds(self):
if self._ds is None:
self._ds = self.get_target()
return self._ds
@property
def horizon(self):
return self._horizon
......@@ -150,7 +172,7 @@ class LQR(object):
_v = [None for i in range(self._horizon)]
_K = [None for i in range(self._horizon-1)]
_Kv = [None for i in range(self._horizon-1)]
_Q = [None for i in range(self._horizon-1)]
# _S = np.empty((self._horizon, self.xi_dim, self.xi_dim))
# _v = np.empty((self._horizon, self.xi_dim))
# _K = np.empty((self._horizon-1, self.u_dim, self.xi_dim))
......@@ -163,7 +185,8 @@ class LQR(object):
Q, z = self.get_Q_z(t)
R = self.get_R(t)
_Kv[t] = np.linalg.inv(R + self.B.T.dot(_S[t+1]).dot(self.B)).dot(self.B.T)
_Q[t] = np.linalg.inv(R + self.B.T.dot(_S[t+1]).dot(self.B))
_Kv[t] = _Q[t].dot(self.B.T)
_K[t] = _Kv[t].dot(_S[t+1]).dot(self.A)
AmBK = self.A - self.B.dot(_K[t])
......@@ -175,6 +198,17 @@ class LQR(object):
self._v = _v
self._K = _K
self._Kv = _Kv
self._Q = _Q
self._ds = None
def get_target(self):
ds = []
for t in range(0, self.horizon-1):
ds += [np.linalg.inv(self._S[t].dot(self.A)).dot(self._v[t])]
return np.array(ds)
def get_target(self):
ds = []
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment