def simulate_zonotope(sys,x_0,T,w,v):
x,y,u={},{},{}
x[0]=x_0
for t in range(T+1):
print "simulating time:",t
y[t]=np.dot(sys.C[t],x[t])+v[t]
if t==T:
return x,y,u,x
Y=np.vstack([y[tau] for tau in range(t+1)])
Ybar=np.vstack([sys.ybar[tau] for tau in range(t+1)])
if t<T+2:
zono_Y=zonotope(Ybar,np.dot(sys.Phi[t],sys.E[t-1].G))
zono_U=zonotope(sys.ubar[t],np.dot(sys.theta[t],sys.E[t-1].G))
u[t]=zonotopic_controller(Y,zono_Y,zono_U)
else:
Ubar=np.vstack([sys.ubar[tau] for tau in range(t)])
U=np.vstack([u[tau] for tau in range(t)])
YU=np.vstack((Y,U))
YU_bar=np.vstack((Ybar,Ubar))
phi_E=np.dot(sys.Phi[t],sys.E[t-1].G)
theta_E=np.dot(sys.Theta[t-1],sys.E[t-2].G)
theta_E_0=np.hstack((theta_E,np.zeros((sys.Theta[t-1].shape[0],phi_E.shape[1]-theta_E.shape[1]))))
G=np.vstack((phi_E,theta_E_0))
zono_Y=zonotope(YU_bar,G)
zono_U=zonotope(sys.ubar[t],np.dot(sys.theta[t],sys.E[t-1].G))
K=np.dot(np.dot(sys.theta[t],sys.E[t-1].G),np.linalg.pinv(G))
u[t]=sys.ubar[t]+np.dot(K,YU-YU_bar)
u[t]=zonotopic_controller(YU,zono_Y,zono_U)
x[t+1]=np.dot(sys.A[t],x[t])+np.dot(sys.B[t],u[t])+w[t]
def simulate(sys, x_0, T):
x, y, u, v, w = {}, {}, {}, {}, {}
x[0] = x_0
for t in range(T + 1):
print "simulating time:", t
zeta_w, zeta_v = 2 * (np.random.random(
(sys.n, 1)) - 0.5), 2 * (np.random.random((sys.o, 1)) - 0.5)
zeta_w = np.random.randint(-1, 1, size=(sys.n, 1))
zeta_v = np.random.randint(-1, 1, size=(sys.m, 1))
v[t] = np.dot(sys.V[t].G, zeta_v) + sys.V[t].x
w[t] = np.dot(sys.W[t].G, zeta_w) + sys.W[t].x
y[t] = np.dot(sys.C[t], x[t]) + v[t]
# print "y",y[t].shape,y
if t == 0:
Zono_Y, Zono_U = zonotope(sys.ybar[t], sys.Phi[0]), zonotope(
sys.ubar[t], sys.Theta[0])
elif t == T:
print "End of simulation"
return x, y, u, v, w
else:
print t, "continue", t == T, T
YU = np.vstack([sys.ybar[tau] for tau in range(t + 1)] +
[sys.ubar[tau] for tau in range(t)])
Zono_Y,Zono_U=zonotope(YU,np.vstack((\
sys.Phi[t],np.hstack((sys.Theta[t-1],np.zeros((sys.Theta[t-1].shape[0],sys.Z[t].G.shape[1])))) ))),\
zonotope(sys.ubar[t],sys.Theta[t][sys.m*t:,:])
H = np.vstack([y[tau]
for tau in range(t + 1)] + [u[tau] for tau in range(t)])
u[t] = zonotopic_controller(H, Zono_Y, Zono_U)
# print "control",t,u[t],u[t].shape
x[t + 1] = np.dot(sys.A[t], x[t]) + np.dot(sys.B[t], u[t]) + w[t]
return x, y, u, v, w
def simulate(sys, x_0, T):
x, y, u, v, w = {}, {}, {}, {}, {}
x[0] = x_0
for t in range(T + 1):
print "simulating time:", t
zeta_w, zeta_v = 2 * (np.random.random(
(sys.n, 1)) - 0.5), 2 * (np.random.random((sys.o, 1)) - 0.5)
zeta_w = np.ones((sys.n, 1)) * (-1)**np.random.randint(1, 3)
zeta_v = np.ones((sys.o, 1)) * (-1)**np.random.randint(1, 3)
v[t] = np.dot(sys.V[t].G, zeta_v) + sys.V[t].x
w[t] = np.dot(sys.W[t].G, zeta_w) + sys.W[t].x
y[t] = np.dot(sys.C[t], x[t]) + v[t]
# print "y",y[t].shape,y
if t == 0:
Zono_Y, Zono_U = zonotope(sys.ybar[t], sys.phi[0]), zonotope(
sys.ubar[t], sys.theta[0])
elif t == T:
print "End of simulation"
return x, y, u, v, w
else:
print t, "continue", t == T, T
YU = np.vstack([sys.ybar[tau] for tau in range(t + 1)] +
[sys.ubar[tau] for tau in range(t)])
Zono_Y,Zono_U=zonotope(YU,np.vstack((\
sys.Phi[t],np.hstack((sys.Theta[t-1],np.zeros((sys.Theta[t-1].shape[0],sys.Z[t].G.shape[1])))) ))),\
zonotope(sys.ubar[t],sys.theta[t])
H = np.vstack([y[tau]
for tau in range(t + 1)] + [u[tau] for tau in range(t)])
# try:
Zono_Y = zonotope(np.vstack([sys.ybar[tau] for tau in range(t + 1)]),
sys.Phi[t])
H = np.vstack([y[tau] for tau in range(t + 1)])
u[t] = zonotopic_controller(H, Zono_Y, Zono_U)
## except:
## if t>0:
## H=np.vstack([y[tau] for tau in range(t+1)]+[u[tau] for tau in range(t)])
## K=np.dot(sys.theta[t],np.linalg.pinv(np.vstack((sys.Phi[t],
## np.hstack((sys.Theta[t-1],np.zeros((sys.Theta[t-1].shape[0],sys.Z[t].G.shape[1]))))))))
## u[t]=sys.ubar[t]+np.dot(K,H-YU)
## elif t==0:
## K=np.dot(sys.theta[0],np.linalg.pinv(sys.phi[0]))
## u[t]=sys.ubar[t]+np.dot(K,y[0])
u[t] = u[t].reshape(sys.m, 1)
print "control", t, u[t], u[t].shape
# u[t]=np.zeros((sys.m,1))
x[t + 1] = np.dot(sys.A[t], x[t]) + np.dot(sys.B[t], u[t]) + w[t]
return x, y, u, v, w
def simulate(sys, x_0, T):
x, y, u, v, w = {}, {}, {}, {}, {}
x[0] = x_0
for t in range(T + 1):
print "simulating time:", t
zeta_w, zeta_v = 2 * (np.random.random(
(sys.n, 1)) - 0.5), 2 * (np.random.random((sys.o, 1)) - 0.5)
zeta_w = np.ones((sys.n, 1)) * (-1)**np.random.randint(1, 3)
zeta_v = np.ones((sys.o, 1)) * (-1)**np.random.randint(1, 3)
v[t] = np.dot(sys.V[t].G, zeta_v) + sys.V[t].x
w[t] = np.dot(sys.W[t].G, zeta_w) + sys.W[t].x
y[t] = np.dot(sys.C[t], x[t]) + v[t]
if t == T:
return x, y, u, v, w
Y = np.vstack([y[tau] for tau in range(t + 1)])
Ybar = np.vstack([sys.ybar[tau] for tau in range(t + 1)])
zono_Y = zonotope(Ybar, np.dot(sys.Phi[t], sys.E[t - 1].G))
zono_U = zonotope(sys.ubar[t], np.dot(sys.theta[t], sys.E[t - 1].G))
u[t] = zonotopic_controller(Y, zono_Y, zono_U)
# u[t]=np.zeros((sys.m,1))
print "control", t, u[t], u[t].shape
x[t + 1] = np.dot(sys.A[t], x[t]) + np.dot(sys.B[t], u[t]) + w[t]
return x, y, u, v, w