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_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 outputfeedback_synthesis_zonotope_solution(sys,u_tilde,theta):
T=max(theta.keys())+1
phi,Phi,Theta={},{},{}
y_tilde={}
Y_tilde,U_tilde={},{}
z_bar,u_bar={},{}
Z,U={},{} # Z matrices
# Initial
y_tilde[0]=np.zeros((sys.o,1))
phi[0]=np.eye(sys.o)
# Yashasin
Phi[0],Theta[0]=phi[0],theta[0]
theta[T]=np.zeros((sys.m,sys.o*(T+1)))
u_tilde[T]=np.zeros((sys.m,1))
for t in range(T):
Y_tilde[t]=np.vstack([y_tilde[tau] for tau in range(t+1)])
U_tilde[t]=np.vstack([u_tilde[tau] for tau in range(t+1)])
y_tilde[t+1]=np.dot(sys.M[t],Y_tilde[t])+np.dot(sys.N[t],U_tilde[t])
phi[t+1]=np.hstack(( np.dot(sys.M[t],Phi[t])+np.dot(sys.N[t],Theta[t]),np.eye(sys.o) ))
Phi[t+1]=triangular_stack(Phi[t],phi[t+1])
Theta[t+1]=triangular_stack(Theta[t],theta[t+1])
Y_tilde[T]=np.vstack([y_tilde[tau] for tau in range(T+1)])
U_tilde[T]=np.vstack([u_tilde[tau] for tau in range(T+1)])
# Performance variables
for t in range(T+1):
# Z zonotope
print("time synthesis",t)
z_bar=np.linalg.multi_dot([sys.R[t],Y_tilde[t]])+\
np.linalg.multi_dot([sys.R[t],Phi[t],sys.Xi[t].x])+\
np.linalg.multi_dot([sys.S[t],U_tilde[t]])+\
np.linalg.multi_dot([sys.S[t],Theta[t],sys.Xi[t].x])+\
sys.F[t].x
Gz1=np.linalg.multi_dot([sys.R[t],Phi[t],sys.Xi["x",t]])+\
np.linalg.multi_dot([sys.S[t],Theta[t],sys.Xi["x",t]])+\
np.dot(sys.D[t],sys.P["x",t])-np.dot(sys.R[t],sys.Q["x",t])
Gz2=np.linalg.multi_dot([sys.R[t],Phi[t],sys.Xi["w",t]])+\
np.linalg.multi_dot([sys.S[t],Theta[t],sys.Xi["w",t]])+\
np.hstack(( np.dot(sys.D[t],sys.P["w",t]),np.zeros((sys.z,sys.n)) ))-\
np.dot(sys.R[t],sys.Q["w",t])
Gz3=np.linalg.multi_dot([sys.R[t],Phi[t],sys.Xi["v",t]])+\
np.linalg.multi_dot([sys.S[t],Theta[t],sys.Xi["v",t]])+\
-np.dot(sys.R[t],sys.Q["v",t])
Gw=spa.block_diag(*[sys.W[t].G for tau in range(0,t+1)])
Gv=spa.block_diag(*[sys.V[t].G for tau in range(0,t+1)])
Gz=np.hstack(( np.dot(Gz1,sys.X0.G), np.dot(Gz2,Gw), np.dot(Gz3,Gv) ))
Z[t]=zonotope(z_bar,Gz)
for t in range(T):
# U zonotope
u_bar=u_tilde[t]+np.dot(theta[t],sys.Xi[t].x)
Gu=np.dot(theta[t],sys.Xi[t].G)
U[t]=zonotope(u_bar,Gu)
return Z,U
def reduced_order(sys, T):
M, N, Z = {}, {}, {}
for t in range(T):
# 1: intiial state
K_1 = np.dot(sys.F["x", t], sys.X0.G)
e_1 = np.dot(sys.C[t + 1], sys.E["x", t])
L_1 = np.dot(e_1, sys.X0.G)
# 2: process noice
K_2 = np.dot(sys.F["w", t],
spa.block_diag(*[sys.W[t].G for tau in range(0, t + 1)]))
e_2 = np.dot(sys.C[t + 1], sys.E["w", t])
L_2 = np.dot(e_2,
spa.block_diag(*[sys.W[t].G for tau in range(0, t + 1)]))
# 3: observation noise
K_3 = np.dot(sys.F["v", t],
spa.block_diag(*[sys.V[t].G for tau in range(0, t + 1)]))
L_3 = np.zeros((sys.o, K_3.shape[1]))
# Build the L and K matrices: L=M*K
L = np.hstack((L_1, L_2, L_3))
K = np.hstack((K_1, K_2, K_3))
M[t] = np.dot(L, np.linalg.pinv(K))
N[t] = np.dot(sys.C[t + 1], sys.E["u", t]) - np.dot(
M[t], sys.F["u", t])
Z[t] = zonotope(None, None)
Z[t].G = np.hstack((L - np.dot(M[t], K), sys.V[t + 1].G))
Z[t].x=np.dot(e_1-np.dot(M[t],sys.F["x",t]),sys.X0.x)\
+np.dot(e_2-np.dot(M[t],sys.F["w",t]),np.vstack([sys.W[t].x for tau in range(0,t+1)]))\
+np.dot(np.dot(M[t],sys.F["v",t]),np.vstack([sys.V[t].x for tau in range(0,t+1)]))\
+sys.V[t+1].x
Z["X0", t] = L_1 - np.dot(M[t], K_1)
Z["W", t] = L_2 - np.dot(M[t], K_2)
Z["V", t] = np.hstack((L_3 - np.dot(M[t], K_3), sys.V[t + 1].G))
return M, N, Z
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 test_LQG_LTV():
S=LTV()
print S.R
n=7
m=1
o=1
T=55
np.random.seed(212)
S.X0=zonotope(np.ones((n,1))*0,np.eye(n)*1)
for t in range(T):
S.A[t]=0.95*np.eye(n)+np.random.normal(size=(n,n))*0.01
S.B[t]=np.random.randint(0,2,size=(n,m))
S.C[t]=np.zeros((o,n))
S.C[t][0,0]=1
S.W[t]=zonotope(np.zeros((n,1)),np.eye(n)*0.01)
S.V[t]=zonotope(np.zeros((o,1)),np.eye(o)*0.01)
S.Q[t]=np.eye(n)*1
S.R[t]=np.eye(m)*1
S.F=np.eye(n)*1
L,K=LQG_LTV(S,T=50)
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
def error_construction_old(sys, T, q0):
sys.ebar = {}
for t in range(-1, T + 1):
sys.E[t] = zonotope(None, None)
sys.E[-1].x = np.zeros((q0, 1))
for t in range(T + 1):
sys.ebar[t] = sys.Z[t].x
# t=0
sys.E[-1].G = np.eye(q0)
sys.ebar[-1] = sys.E[-1].x
# Other t
for t in range(T + 1):
sys.E[t].x = np.vstack([sys.ebar[tau] for tau in range(-1, t + 1)])
sys.E[t].G = triangular_stack(sys.E[t - 1].G, sys.Z[t].G)
def error_construction(sys, T, q0):
sys.ebar = {}
for t in range(-1, T + 1):
sys.E[t] = zonotope(None, None)
sys.E[-1].x = np.zeros((q0, 1))
for t in range(T + 1):
sys.ebar[t] = sys.Z[t].x
# t=0
sys.E[-1].G = np.eye(q0)
sys.ebar[-1] = sys.E[-1].x
for source in ["X0", "V", "W"]:
sys.E[source, -1, "G"] = np.zeros((0, 0))
# Other t
for t in range(T + 1):
sys.E[t].x = np.vstack([sys.ebar[tau] for tau in range(-1, t + 1)])
for source in ["X0", "V", "W"]:
sys.E[source, t, "G"] = triangular_stack(sys.E[source, t - 1, "G"],
sys.Z[source, t])
A = np.hstack([sys.E[source, t, "G"] for source in ["X0", "V", "W"]])
sys.E[t].G = triangular_stack(sys.E[-1].G, A)
print("All sources complete")
from pypolytrajectory.reduction import reduced_order, order_reduction_error, error_construction, error_construction_old
from pypolycontain.lib.objects import zonotope
from pypolycontain.lib.zonotope_order_reduction.methods import G_cut, Girard
from pypolytrajectory.synthesis import output_feedback_synthesis,outputfeedback_synthesis_zonotope_solution,\
triangular_stack_list,output_feedback_synthesis_lightweight_many_variables
from pypolytrajectory.system import LQG_LTV, LTV, LQG
import scipy.linalg as spa
np.random.seed(0)
S = LTV()
n = 1000
m = 2
o = 1
z = 1
T = 32
S.X0 = zonotope(np.ones((n, 1)) * 0, np.eye(n) * 1)
B = np.random.randint(0, 2, size=(n, m))
B[0, 0] = 0
#B[1,0]=0
A = 0.995 * np.eye(n) + np.random.randint(-100, 100,
size=(n, n)) * 0.01 * 0.005
C = np.zeros((o, n))
C[0, 0] = 1
#C[1,1]=1
D = np.eye(n)[0:z, :]
#A=np.array([[0.28,0.25,-0.19,-0.22,0.03,-0.50],
# [0.25,-0.47,0.30,0.17,-0.11,-0.11],
# [-0.19,0.30,0.46,0.09,-0.02,-0.08],
# [-0.22,0.17,0.09,0.60,-0.06,0.14],
# [0.03,-0.11,-0.02,-0.06,0.46,-0.13],
# [-0.50,-0.11,-0.08,0.14,-0.13,-0.23]]).reshape(6,6)
def invariance_time(sys,T,H_X=None,H_U=None,reset_map=[]):
"""
The idea is to find invariance
"""
prog=MP.MathematicalProgram()
Theta={}
theta={}
u_tilde,U_tilde={},{}
# Initial
# Main variables
for t in range(T):
theta[t]=prog.NewContinuousVariables(sys.m,sys.o*(t+1),"theta%d"%t)
u_tilde[t]=prog.NewContinuousVariables(sys.m,1,"u_tilde%d"%t)
Theta[0]=theta[0]
# Aggragates
for t in range(T):
U_tilde[t]=np.vstack([u_tilde[tau] for tau in range(t+1)])
for t in range(T-1):
Theta[t+1]=triangular_stack(Theta[t],theta[t+1])
X,U={},{}
for t in range(1,T+1):
Ptheta=np.dot(sys.P["u",t],Theta[t-1])
a_x=sys.P["x",t]+np.dot(Ptheta,sys.Xi["x",t-1])
a_w=sys.P["w",t] + np.dot(Ptheta,sys.Xi["w",t-1])
a_v=np.dot(Ptheta,sys.Xi["v",t-1])
Gw=spa.block_diag(*[sys.W[tau].G for tau in range(0,t)])
Gv=spa.block_diag(*[sys.V[tau].G for tau in range(0,t)])
Wbar=np.vstack([sys.W[tau].x for tau in range(0,t)])
Vbar=np.vstack([sys.V[tau].x for tau in range(0,t)])
x_bar=np.dot(sys.P["u",t],U_tilde[t-1]) + np.dot(a_x,sys.X0.x) \
+ np.dot(a_w,Wbar) + np.dot(a_v,Vbar)
G=np.hstack(( np.dot(a_x,sys.X0.G), np.dot(a_w,Gw), np.dot(a_v,Gv) ))
X[t]=zonotope(x_bar,G)
if H_X!=None:
subset(prog,X[t],H_X)
if H_U!=None:
for t in range(T):
u_bar=u_tilde[t]+np.dot(theta[t],sys.Xi[t].x)
Gu=np.dot(theta[t],sys.Xi[t].G)
U[t]=zonotope(u_bar,Gu)
subset(prog,U[t],H_U)
print("subset for U done")
# Terminal Constraint
if len(reset_map)!=0:
print("A reset map is found. Are you sure?")
X[T].x=np.dot(reset_map,X[T].x)
X[T].G=np.dot(reset_map,X[T].G)
subset(prog,X[T],sys.X0)
# Optimization
J=0
for t in range(T):
print(t,"u cost")
J+=sum(u_tilde[t].flatten()**2)
# J+=sum(theta[t].flatten()**2)
for t in range(1,T):
print(t,"x cost")
J+=sum(X[t].x.flatten()**2)
# J+=sum(X[t].G.flatten()**2)
prog.AddQuadraticCost(J)
print("Now solving the Quadratic Program")
start=time.time()
result=gurobi_solver.Solve(prog,None,None)
print("time to solve",time.time()-start)
if result.is_success():
print("Synthesis Success!","\n"*5)
theta_n={t:result.GetSolution(theta[t]).reshape(theta[t].shape) for t in range(0,T)}
u_tilde_n={t:result.GetSolution(u_tilde[t]).reshape(u_tilde[t].shape) for t in range(0,T)}
X_n={t:zonotope(sym.Evaluate(result.GetSolution(X[t].x)),\
sym.Evaluate(result.GetSolution(X[t].G))) for t in range(1,T+1)}
if H_U!=None:
U_n={t:zonotope(sym.Evaluate(result.GetSolution(U[t].x)),\
sym.Evaluate(result.GetSolution(U[t].G))) for t in range(T)}
else:
U_n={}
return u_tilde_n,theta_n,X_n,U_n
else:
print("Synthesis Failed!")
def output_feedback_synthesis(sys,T):
prog=MP.MathematicalProgram()
# Add Variables
phi,theta,Phi,Theta={},{},{},{}
y_tilde,u_tilde={},{}
Y_tilde,U_tilde={},{}
z_bar,u_bar={},{}
Z,U={},{} # Z matrices
# Initial Condition
y_tilde[0]=np.zeros((sys.o,1))
phi[0]=np.eye(sys.o)
# Main variables
for t in range(T):
theta[t]=prog.NewContinuousVariables(sys.m,sys.o*(t+1),"theta%d"%t)
u_tilde[t]=prog.NewContinuousVariables(sys.m,1,"u_tilde%d"%t)
# Now we the dynamics
Phi[0],Theta[0]=phi[0],theta[0]
theta[T]=np.zeros((sys.m,sys.o*(T+1)))
u_tilde[T]=np.zeros((sys.m,1))
for t in range(T):
Y_tilde[t]=np.vstack([y_tilde[tau] for tau in range(t+1)])
U_tilde[t]=np.vstack([u_tilde[tau] for tau in range(t+1)])
y_tilde[t+1]=np.dot(sys.M[t],Y_tilde[t])+np.dot(sys.N[t],U_tilde[t])
phi[t+1]=np.hstack(( np.dot(sys.M[t],Phi[t])+np.dot(sys.N[t],Theta[t]),np.eye(sys.o) ))
Phi[t+1]=triangular_stack(Phi[t],phi[t+1])
Theta[t+1]=triangular_stack(Theta[t],theta[t+1])
Y_tilde[T]=np.vstack([y_tilde[tau] for tau in range(T+1)])
U_tilde[T]=np.vstack([u_tilde[tau] for tau in range(T+1)])
# Performance variables
for t in range(T+1):
# Z zonotope
print("time construction",t)
z_bar=np.linalg.multi_dot([sys.R[t],Y_tilde[t]])+\
np.linalg.multi_dot([sys.R[t],Phi[t],sys.Xi[t].x])+\
np.linalg.multi_dot([sys.S[t],U_tilde[t]])+\
np.linalg.multi_dot([sys.S[t],Theta[t],sys.Xi[t].x])+\
sys.F[t].x
Gz1=np.linalg.multi_dot([sys.R[t],Phi[t],sys.Xi["x",t]])+\
np.linalg.multi_dot([sys.S[t],Theta[t],sys.Xi["x",t]])+\
np.dot(sys.D[t],sys.P["x",t])-np.dot(sys.R[t],sys.Q["x",t])
Gz2=np.linalg.multi_dot([sys.R[t],Phi[t],sys.Xi["w",t]])+\
np.linalg.multi_dot([sys.S[t],Theta[t],sys.Xi["w",t]])+\
np.hstack(( np.dot(sys.D[t],sys.P["w",t]),np.zeros((sys.z,sys.n)) ))-\
np.dot(sys.R[t],sys.Q["w",t])
Gz3=np.linalg.multi_dot([sys.R[t],Phi[t],sys.Xi["v",t]])+\
np.linalg.multi_dot([sys.S[t],Theta[t],sys.Xi["v",t]])+\
-np.dot(sys.R[t],sys.Q["v",t])
Gw=spa.block_diag(*[sys.W[t].G for tau in range(0,t+1)])
Gv=spa.block_diag(*[sys.V[t].G for tau in range(0,t+1)])
Gz=np.hstack(( np.dot(Gz1,sys.X0.G), np.dot(Gz2,Gw), np.dot(Gz3,Gv) ))
Z[t]=zonotope(z_bar,Gz)
for t in range(T):
# U zonotope
u_bar=u_tilde[t]+np.dot(theta[t],sys.Xi[t].x)
Gu=np.dot(theta[t],sys.Xi[t].G)
U[t]=zonotope(u_bar,Gu)
# Proxy Quadratic cost
for t in range(1,T):
print(t,"cost")
prog.AddQuadraticCost(sum(U[t].x.flatten()**2))
prog.AddQuadraticCost(sum(U[t].G.flatten()**2))
prog.AddQuadraticCost(sum(Z[t].x.flatten()**2))
prog.AddQuadraticCost(sum(Z[t].G.flatten()**2))
print("Now solving the QP")
result=gurobi_solver.Solve(prog,None,None)
if result.is_success():
print("Synthesis Success!","\n"*5)
# print "D=",result.GetSolution(D)
theta_n={t:result.GetSolution(theta[t]).reshape(theta[t].shape) for t in range(0,T)}
u_tilde_n={t:result.GetSolution(u_tilde[t]).reshape(u_tilde[t].shape) for t in range(0,T)}
return u_tilde_n,theta_n
else:
print("Synthesis Failed!")
from pypolytrajectory.reduction import reduced_order, order_reduction_error, error_construction, error_construction_old
from pypolycontain.lib.objects import zonotope
from pypolycontain.lib.zonotope_order_reduction.methods import G_cut, Girard
from pypolytrajectory.synthesis import output_feedback_synthesis,outputfeedback_synthesis_zonotope_solution,\
triangular_stack_list,output_feedback_synthesis_lightweight_many_variables,invariance_time
from pypolytrajectory.system import LQG_LTV, LTV, LQG
import scipy.linalg as spa
np.random.seed(0)
S = LTV()
n = 6
m = 2
o = 1
z = 1
T = 22
S.X0 = zonotope(np.ones((n, 1)) * 0, np.eye(n) * 1)
B = np.random.randint(0, 2, size=(n, m))
B[0, 0] = 0
#B[1,0]=0
A = 0.3 * np.eye(n) + np.random.randint(-100, 100, size=(n, n)) * 0.01 * 0.2
C = np.zeros((o, n))
C[0, 0] = 1
#C[1,1]=1
D = np.eye(n)[0:z, :]
#A=np.array([[0.28,0.25,-0.19,-0.22,0.03,-0.50],
# [0.25,-0.47,0.30,0.17,-0.11,-0.11],
# [-0.19,0.30,0.46,0.09,-0.02,-0.08],
# [-0.22,0.17,0.09,0.60,-0.06,0.14],
# [0.03,-0.11,-0.02,-0.06,0.46,-0.13],
# [-0.50,-0.11,-0.08,0.14,-0.13,-0.23]]).reshape(6,6)
#B=np.array([[1.0159,0,0.5988,1.8641,0,-1.2155]]).reshape(6,1)
def output_feedback_synthesis_lightweight_many_variables(sys,T,H_Z={},H_U={}):
prog=MP.MathematicalProgram()
# Add Variables
phi,theta,Phi,Theta={},{},{},{}
y_tilde,u_tilde={},{}
Y_tilde,U_tilde={},{}
z_bar,u_bar={},{}
Z,U={},{} # Z matrices
Gz={}
# Main variables
for t in range(T+1):
theta[t]=prog.NewContinuousVariables(sys.m,sys.o*(t+1),"theta%d"%t)
# T_last=50
# for i in range(sys.o*(t+1)):
# if sys.o*(t+1)-i>sys.o*T_last:
# theta[t][:,i]=0
u_tilde[t]=prog.NewContinuousVariables(sys.m,1,"u_tilde%d"%t)
y_tilde[t]=prog.NewContinuousVariables(sys.o,1,"y_tilde%d"%t)
phi[t]=prog.NewContinuousVariables(sys.o,sys.o*(t+1),"Phi%d"%t)
Gz["var",t]=prog.NewContinuousVariables(sys.z,sys.Xi_reduced[t].G.shape[1],"Gz%d"%t)
Theta[0]=theta[0]
y_tilde[0]=np.zeros((sys.o,1))
phi[0]=np.eye(sys.o)
Phi[0]=phi[0]
# Aggragates
for t in range(T+1):
Y_tilde[t]=np.vstack([y_tilde[tau] for tau in range(t+1)])
U_tilde[t]=np.vstack([u_tilde[tau] for tau in range(t+1)])
for t in range(T):
# Dynamics
s=np.dot(sys.M[t],Y_tilde[t])+np.dot(sys.N[t],U_tilde[t])
prog.AddLinearConstraint(np.equal(y_tilde[t+1],s,dtype='object').flatten())
# prog.AddLinearConstraint((y_tilde[t+1]==s).flatten())
S=np.hstack((np.dot(sys.M[t],Phi[t])+np.dot(sys.N[t],Theta[t]),np.eye(sys.o)))
prog.AddLinearConstraint(np.equal(phi[t+1],S,dtype='object').flatten())
# prog.AddLinearConstraint((phi[t+1]==S).flatten())
Phi[t+1]=triangular_stack(Phi[t],phi[t+1])
Theta[t+1]=triangular_stack(Theta[t],theta[t+1])
# Performance variables
for t in range(T+1):
# Z zonotope
print("time construction",t)
z_bar=np.linalg.multi_dot([sys.R[t],Y_tilde[t]])+\
np.linalg.multi_dot([sys.R[t],Phi[t],sys.Xi[t].x])+\
np.linalg.multi_dot([sys.S[t],U_tilde[t]])+\
np.linalg.multi_dot([sys.S[t],Theta[t],sys.Xi_reduced[t].x])+\
sys.F[t].x
Gz[t]=np.linalg.multi_dot([sys.R[t],Phi[t],sys.Xi_reduced[t].G])+\
np.linalg.multi_dot([sys.S[t],Theta[t],sys.Xi_reduced[t].G])
Z[t]=zonotope(z_bar,np.hstack((Gz[t],sys.F_reduced[t].G)))
# Z[t]=zonotope(z_bar,Gz[t])
prog.AddLinearConstraint(np.equal(Gz[t],Gz["var",t],dtype='object').flatten())
for t in range(T):
# U zonotope
u_bar=u_tilde[t]+np.dot(theta[t],sys.Xi_reduced[t].x)
Gu=np.dot(theta[t],sys.Xi_reduced[t].G)
U[t]=zonotope(u_bar,Gu)
# Constraints
for t,value in H_U.items():
print("performing subset for U",t,value)
subset(prog,U[t],H_U[t])
for t,value in H_Z.items():
print("performing subset for Z",t,value)
subset(prog,Z[t],H_Z[t])
# Proxy Linear Cost
prog.AddLinearConstraint(np.equal(u_tilde[T-1],np.zeros((sys.m,1)),dtype='object').flatten())
if True:
r={}
# r["u-max"]=prog.NewContinuousVariables(1,1,"ru-max")
# r["z-max"]=prog.NewContinuousVariables(1,1,"rz-max")
# prog.NewContinuousVariables(1,1,"r")
# prog.AddLinearCost(r["u-max"][0,0])
# prog.AddLinearCost(r["z-max"][0,0])
for t in range(1,T+1):
print(t,"adding cost for z")
r["z",t]=prog.NewContinuousVariables(1,1,"r")
R=hyperbox(sys.z)
ZT=H_polytope(R.H_polytope.H,np.dot(R.H_polytope.h,r["z",t]),symbolic=True)
subset(prog,Z[t],ZT)
# prog.AddLinearConstraint(np.less_equal(r["z",t],r["z-max"],dtype='object').flatten())
prog.AddQuadraticCost(r["z",t][0,0]*r["z",t][0,0])
for t in range(T):
print(t,"adding cost for u")
r["u",t]=prog.NewContinuousVariables(1,1,"r")
R=hyperbox(sys.m)
UT=H_polytope(R.H_polytope.H,np.dot(R.H_polytope.h,r["u",t]),symbolic=True)
subset(prog,U[t],UT)
prog.AddQuadraticCost(r["u",t][0,0]*r["u",t][0,0])
# subset(prog,U[t],H_polytope(R.H_polytope.H,np.dot(R.H_polytope.h,3)))
# prog.AddLinearConstraint(np.less_equal(r["u",t],r["u-max"],dtype='object').flatten())
# Proxy Quadratic cost
elif False:
J=0
for t in range(T):
print(t,"cost")
J+=sum(U[t].x.flatten()**2)
J+=sum(U[t].G.flatten()**2)
J+=sum(Z[t+1].x.flatten()**2)
J+=sum(Gz[t+1].flatten()**2)
prog.AddQuadraticCost(J)
print("Now solving the Linear Program")
start=time.time()
result=gurobi_solver.Solve(prog,None,None)
print("time to solve",time.time()-start)
if result.is_success():
print("Synthesis Success!","\n"*5)
# print "D=",result.GetSolution(D)
theta_n={t:result.GetSolution(theta[t]).reshape(theta[t].shape) for t in range(0,T)}
u_tilde_n={t:result.GetSolution(u_tilde[t]).reshape(u_tilde[t].shape) for t in range(0,T)}
r_z=np.array([result.GetSolution(r["z",t][0,0]) for t in range(1,T+1)])
r_u=np.array([result.GetSolution(r["u",t][0,0]) for t in range(0,T)])
print("Upper Cost",np.linalg.norm(r_z,ord=2)**2+np.linalg.norm(r_u,ord=2)**2)
# print "Upper Cost",np.linalg.norm(r_u,ord=2)**2
# print r_z
print(r_u)
return(u_tilde_n,theta_n)
else:
print("Synthesis Failed!")
tau_g = g * np.array([[m_h * l + m * a + m * l, 0], [0, -m * b]])
for t in range(T):
S.A[t] = np.eye(n)
S.A[t][0, 2], S.A[t][1, 3] = dt, dt
S.A[t][2:, 0:2] = np.dot(Minv, tau_g) * dt
Bq = np.dot(Minv, np.array([1, 1])) * dt
S.B[t] = np.array([0, 0, Bq[0], Bq[1]]).reshape(4, 1)
# S.B[t]=np.array([0,0,1,1]).reshape(4,1)
# S.B[t]=np.array([[0,0,1,0],[0,0,0,1]]).T
# S.B[t]=np.array([0,0,1,1]).reshape(4,1)
# S.C[t]=np.array([[1,0,0,0],[0,1,0,0]]).reshape(2,4)
S.C[t] = np.array([[1, 1, 0, 0]]).reshape(1, 4)
S.D[t] = np.eye(n)
S.d[t] = np.zeros((n, 1))
S.W[t] = zonotope(np.ones((n, 1)) * 0, np.ones((n, 1)) * 0)
# S.V[t]=zonotope(np.zeros((2,1)),np.ones((2,1))*0.0000)
S.V[t] = zonotope(np.zeros((1, 1)), np.eye(1) * 0.00)
if True:
prog = MP.MathematicalProgram()
T = 20
R = np.array([[0, -1, 0, 0], [-1, 0, 0, 0], [0, 0, 0, -1], [0, 0, -1, 0]])
x, u = {}, {}
for t in range(T + 1):
x[t] = prog.NewContinuousVariables(4, 1, "x%d" % t)
u[t] = prog.NewContinuousVariables(1, 1, "u%d" % t)
x[0][0, 0] = 0.12
x[0][1, 0] = 0.12
for t in range(T):
x_new = np.dot(S.A[0], x[t]) + np.dot(S.B[0], u[t])
from pypolycontain.lib.objects import zonotope
from pypolycontain.lib.zonotope_order_reduction.methods import G_cut, Girard
from pypolytrajectory.synthesis import output_feedback_synthesis,outputfeedback_synthesis_zonotope_solution,\
triangular_stack_list,output_feedback_synthesis_lightweight_many_variables
from pypolytrajectory.system import LQG_LTV, LTV, LQG
import scipy.linalg as spa
np.random.seed(0)
S = LTV()
n = 20
m = 1
o = 1
z = 20
T = 58
S.X0 = zonotope(np.ones((n, 1)) * 0, np.eye(n) * 1)
B = np.random.randint(0, 2, size=(n, m))
B[0, 0] = 1
B[1, 0] = 0
A = 0.9 * np.eye(n) + np.random.randint(-100, 100, size=(n, n)) * 0.01 * 0.15
C = np.zeros((o, n))
C[0, 0] = 1
#C[1,1]=1
D = np.eye(n)[0:z, :]
#A=np.array([[0.28,0.25,-0.19,-0.22,0.03,-0.50],
# [0.25,-0.47,0.30,0.17,-0.11,-0.11],
# [-0.19,0.30,0.46,0.09,-0.02,-0.08],
# [-0.22,0.17,0.09,0.60,-0.06,0.14],
# [0.03,-0.11,-0.02,-0.06,0.46,-0.13],
# [-0.50,-0.11,-0.08,0.14,-0.13,-0.23]]).reshape(6,6)
#B=np.array([[1.0159,0,0.5988,1.8641,0,-1.2155]]).reshape(6,1)
import scipy.linalg as spa
# pypolycontain
from pypolycontain.visualization.visualize_2D import visualize_2D_zonotopes_ax as visZax
np.random.seed(0)
S=LTV()
n=10
m=2
o=1
z=2
T=42
x0_bar=np.ones((n,1))*10
x0_bar[0,0]=25
x0_bar[1,0]=-25
S.X0=zonotope(x0_bar,np.eye(n)*1)
B=np.random.randint(0,2,size=(n,m))*1
#B[0,0]=0.05
#B[1,1]=0.05
A=0.95*np.eye(n)+np.random.randint(-100,100,size=(n,n))*0.01*0.1
#n_2=8
#A[n-n_2:,n-n_2:]=np.eye(n_2)
#A[n-n_2:,:n-n_2]=np.zeros((n_2,n-n_2))
for i in range(n):
for j in range(n):
if i>j:
A[i,j]=0
C=np.zeros((o,n))
C[0,0]=1
#C[1,1]=1
D=np.eye(n)[0:z,:]
import numpy as np
from pypolytrajectory.LTV import system, test_controllability
from pypolytrajectory.reduction import reduced_order, order_reduction_error, error_construction, error_construction_old
from pypolycontain.lib.objects import zonotope
from pypolycontain.lib.zonotope_order_reduction.methods import G_cut, Girard
from pypolytrajectory.synthesis import synthesis_disturbance_feedback, zonotopic_controller, synthesis
import pydrake.systems.controllers as PC
S = system()
n = 6
m = 1
o = 1
T = 48
#np.random.seed(1)
S.X0 = zonotope(np.array(([0, 0, 0, 0, 0, 0])).reshape(6, 1), np.eye(n) * 1)
for t in range(T):
S.A[t] = np.array([[0.28, 0.25, -0.19, -0.22, 0.03, -0.50],
[0.25, -0.47, 0.30, 0.17, -0.11, -0.11],
[-0.19, 0.30, 0.46, 0.09, -0.02, -0.08],
[-0.22, 0.17, 0.09, 0.60, -0.06, 0.14],
[0.03, -0.11, -0.02, -0.06, 0.46, -0.13],
[-0.50, -0.11, -0.08, 0.14, -0.13,
-0.23]]).reshape(6, 6)
S.B[t] = np.array([[1.0159, 0, 0.5988, 1.8641, 0, -1.2155]]).reshape(6, 1)
#S.C[t]=np.array([[1.292,0,0,0.2361,0.8428,0]]).reshape(1,6)
S.C[t] = np.array([[1.29, 0.24, 0, 0, 0, 0]]).reshape(1, 6)
S.W[t] = zonotope(np.zeros((n, 1)), np.eye(n) * 0.05)
S.V[t] = zonotope(np.zeros((o, 1)), np.eye(o) * 0.05)
S.U_set = zonotope(np.zeros((m, 1)), np.eye(m) * 1)
from pypolytrajectory.reduction import reduced_order, order_reduction_error, error_construction, error_construction_old
from pypolycontain.lib.objects import zonotope
from pypolycontain.lib.zonotope_order_reduction.methods import G_cut, Girard
from pypolytrajectory.synthesis import output_feedback_synthesis,outputfeedback_synthesis_zonotope_solution,\
triangular_stack_list,output_feedback_synthesis_lightweight_many_variables
from pypolytrajectory.system import LQG_LTV, LTV, LQG
import scipy.linalg as spa
np.random.seed(1000)
S = LTV()
n = 6
m = 1
o = 1
z = 1
T = 42
S.X0 = zonotope(np.ones((n, 1)) * 0, np.eye(n) * 1)
B = np.random.randint(0, 2, size=(n, m))
B[0, 0] = 0
A = 0.0 * np.eye(n) + np.random.randint(-100, 100, size=(n, n)) * 0.01
C = np.zeros((o, n))
C[0, 0] = 1
D = np.eye(n)[0:z, :]
#A=np.array([[0.28,0.25,-0.19,-0.22,0.03,-0.50],
# [0.25,-0.47,0.30,0.17,-0.11,-0.11],
# [-0.19,0.30,0.46,0.09,-0.02,-0.08],
# [-0.22,0.17,0.09,0.60,-0.06,0.14],
# [0.03,-0.11,-0.02,-0.06,0.46,-0.13],
# [-0.50,-0.11,-0.08,0.14,-0.13,-0.23]]).reshape(6,6)
#B=np.array([[1.0159,0,0.5988,1.8641,0,-1.2155]]).reshape(6,1)
#C=np.array([[1.292,0,0,0.2361,0.8428,0]]).reshape(1,6)
for t in range(T):
import numpy as np from pypolytrajectory.LTV import system,test_controllability from pypolytrajectory.reduction import reduced_order,order_reduction_error,error_construction,error_construction_old from pypolycontain.lib.objects import zonotope from pypolycontain.lib.zonotope_order_reduction.methods import G_cut,Girard from pypolytrajectory.synthesis import synthesis_disturbance_feedback,zonotopic_controller,synthesis,zonotopic_controller_soft from pypolytrajectory.system import LQG_LTV,LTV,LQG S=LTV() n=5 m=1 o=1 T=58 S.X0=zonotope(np.ones((n,1))*0,np.eye(n)*1) B=np.random.randint(0,2,size=(n,m)) B[0,0]=1 B[1,0]=0 A=0.0*np.eye(n)+np.random.normal(size=(n,n))*0.6 C=np.zeros((o,n)) C[0,0]=1 #A=np.array([[0.28,0.25,-0.19,-0.22,0.03,-0.50], # [0.25,-0.47,0.30,0.17,-0.11,-0.11], # [-0.19,0.30,0.46,0.09,-0.02,-0.08], # [-0.22,0.17,0.09,0.60,-0.06,0.14], # [0.03,-0.11,-0.02,-0.06,0.46,-0.13], # [-0.50,-0.11,-0.08,0.14,-0.13,-0.23]]).reshape(6,6) #B=np.array([[1.0159,0,0.5988,1.8641,0,-1.2155]]).reshape(6,1) #C=np.array([[1.292,0,0,0.2361,0.8428,0]]).reshape(1,6) for t in range(T):
T=int((K+d_0)/(v*dt))+1
gap_bar,step_var,gap_var=1,0.3,0.1
x0_bar=np.zeros((n,1))
G=np.zeros((n,n))
x0_bar[0,0],x0_bar[1,0]=-d_0,0
G[0,0]=0 # Variation in x
G[1,1]=0 # Variation in speed
G[2,2]=0 # Variation in height
for i in range(K):
x0_bar[3+2*i+1]=gap_bar
G[3+2*i+1,3+2*i+1]=gap_var
for j in range(K-i):
G[3+2*i+2*j,3+2*i]=step_var
S.X0=zonotope(x0_bar,G)
def visualize(ax,x):
# Add the patch to the Axes
for i in range(K):
rect_down = patches.Rectangle((i,x[3+2*i]-10),1,10,linewidth=1,edgecolor='black',facecolor='orange',alpha=0.59)
rect_up = patches.Rectangle((i,x[3+2*i]+x[3+2*i+1]),1,10,linewidth=1,edgecolor='black',facecolor='orange',alpha=0.59)
ax.set_xlim([-1,K-1])
ax.set_ylim([-3,3])
ax.add_patch(rect_down)
ax.add_patch(rect_up)
#zeta=2*(np.random.random((n,1))-0.5)
#x=np.dot(S.X0.G,zeta)+S.X0.x
#visualize(x)
def _error_zonotopes(self,T=0):
Sigma={}
Psi={}
if T==0:
T=max(self.A.keys())
# Construction of Xi:
self.Xi,self.Xi_reduced,self.F_reduced={},{},{}
self.Xi["x",0]=self.C[0]
self.Xi["w",0]=np.zeros((self.o,self.n))
self.Xi["v",0]=np.eye(self.o)
for t in range(T):
# print "t=",t
Gw=spa.block_diag(*[self.W[t].G for tau in range(0,t+1)])
Gv=spa.block_diag(*[self.V[t].G for tau in range(0,t+1)])
Wbar=np.vstack([self.W[tau].x for tau in range(0,t+1)])
Vbar=np.vstack([self.V[tau].x for tau in range(0,t+1)])
L1=np.linalg.multi_dot([self.C[t+1],self.P["x",t+1],self.X0.G])
L2=np.linalg.multi_dot([self.C[t+1],self.P["w",t+1],Gw])
L3=np.zeros((self.o,Gv.shape[1]))
L4=self.V[t+1].G
F1=np.dot(self.Q["x",t],self.X0.G)
F2=np.dot(self.Q["w",t],Gw)
F3=np.dot(self.Q["v",t],Gv)
F4=np.zeros(((t+1)*self.o,self.V[t+1].G.shape[1]))
Sigma["e",t]=np.hstack((L1,L2,L3,L4))
Psi["e",t]=np.hstack((F1,F2,F3,F4))
# print Sigma["e",t].shape
# print Psi["e",t].shape
K_1=np.linalg.multi_dot([self.D[t],self.P["x",t],self.X0.G])
K_2=np.dot(np.hstack((np.dot(self.D[t],self.P["w",t]),np.zeros((self.z,self.n)))),Gw)
K_3=np.zeros((self.z,Gv.shape[1]))
P_1=np.dot(self.Q["x",t],self.X0.G)
P_2=np.dot(self.Q["w",t],Gw)
P_3=-np.dot(self.Q["v",t],Gv)
Sigma["f",t]=np.hstack((K_1,K_2,K_3))
Psi["f",t]=np.hstack((P_1,P_2,P_3))
# print "f",Sigma["f",t].shape
# print "f",Psi["f",t].shape
self.M[t]=np.dot(Sigma["e",t],np.linalg.pinv(Psi["e",t]))
self.R[t]=np.dot(Sigma["f",t],np.linalg.pinv(Psi["f",t]))
self.N[t]=np.dot(self.C[t+1],self.P["u",t+1])-np.dot(self.M[t],self.Q["u",t])
self.S[t]=np.hstack(( np.dot(self.D[t],self.P["u",t]),np.zeros((self.z,self.m)) ))-np.dot(self.R[t],self.Q["u",t])
# print self.M[t].shape
# print self.N[t].shape
# print self.R[t].shape
# print self.S[t].shape
ebar=np.linalg.multi_dot([self.C[t+1],self.P["x",t+1],self.X0.x])\
-np.linalg.multi_dot([self.M[t],self.Q["x",t],self.X0.x])\
+np.linalg.multi_dot([self.C[t+1],self.P["w",t+1],Wbar])\
-np.linalg.multi_dot([self.M[t],self.Q["w",t],Wbar])\
+np.linalg.multi_dot([self.M[t],self.Q["v",t],Vbar])\
+self.V[t+1].x
eG=Sigma["e",t]-np.dot(self.M[t],Psi["e",t])
self.E[t]=zonotope(ebar,eG)
fbar=np.linalg.multi_dot([self.D[t],self.P["x",t],self.X0.x])\
-np.linalg.multi_dot([self.R[t],self.Q["x",t],self.X0.x])\
+np.dot(np.hstack((np.dot(self.D[t],self.P["w",t]),np.zeros((self.z,self.n)))),Wbar)\
-np.linalg.multi_dot([self.R[t],self.Q["w",t],Wbar])\
-np.linalg.multi_dot([self.R[t],self.Q["v",t],Vbar])\
+self.d[t]
fG=Sigma["f",t]-np.dot(self.R[t],Psi["f",t])
self.F[t]=zonotope(fbar,fG)
F_G_reduced=Girard(fG,fG.shape[0])
self.F_reduced[t]=zonotope(fbar,F_G_reduced)
for t in range(T):
self.Xi["x",t+1]=triangular_stack(self.Xi["x",t],np.dot(self.C[t+1],self.P["x",t+1])-np.dot(self.M[t],self.Q["x",t]))
self.Xi["w",t+1]=triangular_stack(self.Xi["w",t],np.hstack((np.dot(self.C[t+1],self.P["w",t+1])-np.dot(self.M[t],self.Q["w",t]),\
np.zeros((self.o,self.n)) )) )
self.Xi["v",t+1]=triangular_stack(self.Xi["v",t],np.hstack((np.dot(self.M[t],self.Q["v",t]),np.eye(self.o))))
for t in range(T+1):
Gw=spa.block_diag(*[self.W[t].G for tau in range(0,t+1)])
Gv=spa.block_diag(*[self.V[t].G for tau in range(0,t+1)])
Wbar=np.vstack([self.W[tau].x for tau in range(0,t+1)])
Vbar=np.vstack([self.V[tau].x for tau in range(0,t+1)])
# print "t=",t,"Xi_shapes",Xi["x",t].shape,Xi["w",t].shape,Xi["v",t].shape
# print "bar",Wbar.shape,Vbar.shape
Xi_bar=np.dot(self.Xi["x",t],self.X0.x)+np.dot(self.Xi["w",t],Wbar)+np.dot(self.Xi["v",t],Vbar)
Xi_G=np.hstack((np.dot(self.Xi["x",t],self.X0.G),np.dot(self.Xi["w",t],Gw),np.dot(self.Xi["v",t],Gv)))
self.Xi[t]=zonotope(Xi_bar,Xi_G)
Xi_G_reduced=Girard(Xi_G,Xi_G.shape[0])
self.Xi_reduced[t]=zonotope(Xi_bar,Xi_G_reduced)
