def compressed_sensing_tomography(measurement_ops,
measurement_avgs,
epsilon=.5):
"""Compressed Sensing based tomography, this should reproduce the results of standard_tomography
in the limit of more measurements."""
if not picos_available:
raise Exception(
"picos package not installed, please ensure picos and cvxopt are installed."
)
if not cvx_available:
raise Exception(
"cvxopt package not installed, please ensure picos and cvxopt are installed."
)
def e(dim=2, initial_state=0):
"""Return the state vector of a system in a pure state"""
assert dim > 0
state = np.matrix(np.zeros(dim, dtype='complex')).T
state[initial_state % dim] = 1
return state
n = np.shape(measurement_ops)[1] # dimension of space
cvx_pops = cvx.matrix(measurement_avgs)
reshaped_matrix = np.array(measurement_ops).reshape(-1, n)
F = picos.Problem()
Z = F.add_variable('Z', (n, n), 'hermitian')
# This does the trace of the measurement matricies * Z,
# which should result in the populations measured
meas_opt = picos.sum([
cvx.matrix(reshaped_matrix[i::n, :]) * Z * cvx.matrix(e(n, i))
for i in range(n)
], 'i')
F.set_objective("min", pic.trace(Z))
F.add_constraint(picos.norm(meas_opt - cvx_pops) < epsilon)
F.add_constraint(Z >> 0)
F.solve(verbose=0)
return np.matrix(Z.value) / trace(Z.value)
def optimization(UAV_config,
no_fly_zone,
initial_trajectory,
plot_procedure=False,
plot_ending=False):
nx = 4
nu = 3
Ts = UAV_config.sf / UAV_config.N
ds_value = np.linspace(0, UAV_config.sf, num=UAV_config.N + 1)
ele = nx * (UAV_config.N + 1) + nu * UAV_config.N
No_Fly_zone_number = [
'1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '*'
]
result = np.vstack((initial_trajectory.x, initial_trajectory.y))
detal_s = np.zeros(UAV_config.N)
for i in range(UAV_config.N):
detal_s[i] = np.sqrt(UAV_config.ah_max /
np.square(initial_trajectory.V))
# coordinate transformation
[x0, y0, psi0, xf, yf, thita, x_initial, y_initial, x1, y1] \
= coordinate_transformation(UAV_config.x0, UAV_config.y0, UAV_config.psi0, UAV_config.xf,
UAV_config.yf, no_fly_zone.x_NFZ, no_fly_zone.y_NFZ, no_fly_zone.M)
np_ = 100
avoid_circle1 = np.zeros([2, np_ + 1, no_fly_zone.M]) # no - fly - zone 1
for j in range(no_fly_zone.M):
for i in range(np_ + 1):
new_x = np.cos(thita) * x1[j] + -np.sin(thita) * y1[j] + x_initial + \
no_fly_zone.a_NFZ[j] * np.cos(i / np_ * 2 * np.pi) # no - fly - zone 1
new_y = np.sin(thita) * x1[j] + np.cos(
thita) * y1[j] + y_initial + no_fly_zone.a_NFZ[j] * np.sin(
i / np_ * 2 * np.pi)
avoid_circle1[
0, i,
j] = np.cos(-thita) * (new_x - x_initial) + -np.sin(-thita) * (
new_y - y_initial) # no - fly - zone 1
avoid_circle1[1, i, j] = np.sin(-thita) * (
new_x - x_initial) + np.cos(-thita) * (new_y - y_initial)
for i in range(UAV_config.N + 1):
new_x = np.cos(-thita) * (result[
0, i] - x_initial) + -np.sin(-thita) * (result[1, i] - y_initial)
new_y = np.sin(-thita) * (result[0, i] - x_initial) + np.cos(
-thita) * (result[1, i] - y_initial)
result[0, i] = new_x
result[1, i] = new_y
flag = True
iter = 1
deta_x = 0
deta_y = 0
deta1_deta1 = 0
deta1_v = 0
while flag:
# print("================== iter={} ====================".format(iter))
prob = pic.Problem()
y = prob.add_variable('y', ele)
deta = prob.add_variable('deta', UAV_config.N)
deta1 = prob.add_variable('deta1', UAV_config.N)
k = prob.add_variable('k', UAV_config.N)
s = prob.add_variable('s', UAV_config.N)
k_x = prob.add_variable('k_x', 1)
k_y = prob.add_variable('k_y', 1)
temp1 = prob.add_variable('temp1', (UAV_config.N, 2))
temp2 = prob.add_variable('temp2', (UAV_config.N, 2))
temp3 = prob.add_variable('temp3', (UAV_config.N, 2))
temp4 = prob.add_variable('temp4', (UAV_config.N, 2))
constraints = []
for i in range(UAV_config.N): # 动力学方程
constraints.append(
prob.add_constraint(
xf * (y[(i + 1) * nx + 0] - y[i * nx + 0]) == Ts *
y[(UAV_config.N + 1) * nx + i * nu + 0]))
constraints.append(
prob.add_constraint(
xf * (y[(i + 1) * nx + 1] - y[i * nx + 1]) == Ts *
y[i * nx + 2]))
constraints.append(
prob.add_constraint(y[(i + 1) * nx + 2] - y[i * nx + 2] == Ts *
y[(UAV_config.N + 1) * nx + i * nu + 1]))
constraints.append(
prob.add_constraint(y[(i + 1) * nx + 3] - y[i * nx + 3] == Ts *
2 *
y[(UAV_config.N + 1) * nx + i * nu + 2]))
for i in range(UAV_config.N):
constraints.append(
prob.add_constraint(y[i * nx + 2] == temp1[i, 0]))
constraints.append(
prob.add_constraint(y[(UAV_config.N + 1) * nx + i * nu +
0] == temp1[i, 1]))
constraints.append(
prob.add_constraint(pic.norm(temp1[i, :], 2) <= 1))
constraints.append(
prob.add_constraint(
y[(i + 1) * nx +
3] <= np.float(np.square(UAV_config.v_max))))
constraints.append(
prob.add_constraint(
y[(i + 1) * nx +
3] >= np.float(np.square(UAV_config.v_min))))
constraints.append(
prob.add_constraint(y[(UAV_config.N + 1) * nx + i * nu +
2] <= UAV_config.at_max))
constraints.append(
prob.add_constraint(y[(UAV_config.N + 1) * nx + i * nu +
2] >= -UAV_config.at_max))
constraints.append(
prob.add_constraint(1 / np.sqrt(2) -
y[i * nx + 3] / np.sqrt(2) == temp2[i, 0]))
constraints.append(
prob.add_constraint(np.sqrt(2) * deta[i] == temp2[i, 1]))
constraints.append(
prob.add_constraint(
pic.norm(temp2[i, :], 2) <= 1 / np.sqrt(2) +
y[i * nx + 3] / np.sqrt(2)))
constraints.append(
prob.add_constraint(k[i] / np.sqrt(2) -
deta[i] / np.sqrt(2) == temp3[i, 0]))
constraints.append(prob.add_constraint(np.sqrt(2) == temp3[i, 1]))
constraints.append(
prob.add_constraint(
pic.norm(temp3[i, :], 2) <= k[i] / np.sqrt(2) +
deta[i] / np.sqrt(2)))
constraints.append(
prob.add_constraint(
y[(UAV_config.N + 1) * nx + i * nu + 0] / np.sqrt(2) -
s[i] / np.sqrt(2) == temp4[i, 0]))
constraints.append(
prob.add_constraint(
np.sqrt(2 / UAV_config.ah_max) * deta1[i] == temp4[i, 1]))
constraints.append(
prob.add_constraint(
pic.norm(temp4[i, :], 2) <= y[
(UAV_config.N + 1) * nx + i * nu + 0] / np.sqrt(2) +
s[i] / np.sqrt(2)))
constraints.append(
prob.add_constraint(
y[(UAV_config.N + 1) * nx + i * nu +
1] <= np.square(detal_s[i]) + 2 * detal_s[i] *
(deta1[i] - detal_s[i])))
constraints.append(
prob.add_constraint(
y[(UAV_config.N + 1) * nx + i * nu +
1] >= -np.square(detal_s[i]) - 2 * detal_s[i] *
(deta1[i] - detal_s[i])))
constraints.append(
prob.add_constraint(
s[i] + 1 / np.square(np.square(initial_trajectory.V)) *
(y[i * nx + 3] - np.float(np.square(initial_trajectory.V)))
<= 1 / np.square(initial_trajectory.V)))
constraints.append(
prob.add_constraint(
y[i * nx +
3] == np.float(np.square(initial_trajectory.V))))
if iter >= 2:
constraints.append(
prob.add_constraint(
xf * y[i * nx + 0] - result[0, i] <= deta_x))
constraints.append(
prob.add_constraint(xf * y[i * nx + 0] -
result[0, i] >= -deta_x)) # 可信赖域约束x
constraints.append(
prob.add_constraint(
xf * y[i * nx + 1] - result[1, i] <= deta_y))
constraints.append(
prob.add_constraint(xf * y[i * nx + 1] -
result[1, i] >= -deta_y)) # 可信赖域约束x
constraints.append(
prob.add_constraint(deta1[i] - detal_s[i] <= deta1_deta1))
constraints.append(
prob.add_constraint(
deta1[i] - detal_s[i] >= -deta1_deta1)) # 可信赖域约束deta1
constraints.append(
prob.add_constraint(
y[i * nx + 3] -
np.float(np.square(initial_trajectory.V)) <= deta1_v))
constraints.append(
prob.add_constraint(
y[i * nx + 3] -
np.float(np.square(initial_trajectory.V)) >= -deta1_v)
) # 可信赖域约束v
constraints.append(
prob.add_constraint(
y[i * nx +
3] == np.float(np.square(initial_trajectory.V))))
for j in range(no_fly_zone.M):
if no_fly_zone.Flag_NFZ[j] == 1:
for i in range(1, UAV_config.N + 1):
x_i = result[0, i] # 0 1
y_i = result[1, i]
A1 = np.cos(thita)
B1 = -np.sin(thita)
C1 = x_initial - (np.cos(thita) * x1[j] -
np.sin(thita) * y1[j] + x_initial)
A2 = np.sin(thita)
B2 = np.cos(thita)
C2 = y_initial - (np.sin(thita) * x1[j] +
np.cos(thita) * y1[j] + y_initial)
af_ax = 2 * A1 * (A1 * x_i + B1 * y_i + C1) / np.square(
no_fly_zone.a_NFZ[j]) + 2 * A2 * (
A2 * x_i + B2 * y_i + C2) / np.square(
no_fly_zone.b_NFZ[j])
af_ay = 2 * B1 * (A1 * x_i + B1 * y_i + C1) / np.square(
no_fly_zone.a_NFZ[j]) + 2 * B2 * (
A2 * x_i + B2 * y_i + C2) / np.square(
no_fly_zone.b_NFZ[j])
f_xy = np.square((A1 * x_i + B1 * y_i + C1)) / np.square(
no_fly_zone.a_NFZ[j]) + (
np.square(A2 * x_i + B2 * y_i + C2)) / np.square(
no_fly_zone.b_NFZ[j]) - 1
constraints.append(
prob.add_constraint(
f_xy + af_ax * (xf * y[i * nx + 0] - x_i) + af_ay *
(xf * y[i * nx + 1] - y_i) >= 0))
constraints.append(
prob.add_constraint(y[nx * UAV_config.N + 1] * xf - yf <= k_y))
constraints.append(
prob.add_constraint(y[nx * UAV_config.N + 1] * xf - yf >= -k_y))
constraints.append(
prob.add_constraint(y[nx * UAV_config.N + 0] * xf - xf <= k_x))
constraints.append(
prob.add_constraint(y[nx * UAV_config.N + 0] * xf - xf >= -k_x))
constraints.append(prob.add_constraint(y[0] == x0 / xf))
constraints.append(prob.add_constraint(y[1] == y0 / xf))
constraints.append(prob.add_constraint(y[2] == np.sin(psi0)))
constraints.append(
prob.add_constraint(y[3] == np.float(np.square(UAV_config.v0))))
constraints.append(
prob.add_constraint(y[nx * UAV_config.N +
3] == np.float(np.square(UAV_config.vf))))
coe1 = 10
coe3 = 10
coe4 = 10
coe2 = 10
temp = np.matrix(np.ones([1, UAV_config.N]))
prob.set_objective(
"min", coe1 * temp * k + coe3 * k_x + coe4 * k_y + coe2 *
(result[0, -1] - y[nx * UAV_config.N + 0] * xf) *
(result[0, -1] - y[nx * UAV_config.N + 0] * xf) + coe2 *
(result[1, -1] - y[nx * UAV_config.N + 1] * xf) *
(result[1, -1] - y[nx * UAV_config.N + 1] * xf))
solution = prob.solve(verbose=0, solver='ecos')
'''print(prob)
print(solution["status"])
print(prob.obj_value())
print(k_y)'''
y = y.value
k = k.value
s = s.value
k_x = k_x.value
k_y = k_y.value
deta = deta.value
deta1 = deta1.value
x_pos = xf * y[0:(UAV_config.N + 1) * nx:nx]
y_pos = xf * y[1:(UAV_config.N + 1) * nx:nx]
fai_s = np.array(y[2:(UAV_config.N + 1) * nx:nx])
v_ba = np.array(y[3:(UAV_config.N + 1) * nx:nx])
fai_c = np.array(y[(UAV_config.N + 1) * nx +
0:(UAV_config.N + 1) * nx + UAV_config.N * nu:nu])
fai_d = np.array(y[(UAV_config.N + 1) * nx +
1:(UAV_config.N + 1) * nx + UAV_config.N * nu:nu])
at = np.array(y[(UAV_config.N + 1) * nx + 2:(UAV_config.N + 1) * nx +
UAV_config.N * nu:nu])
u12_square = np.array([(np.square(fai_c[i]) + np.square(fai_s[i]))
for i in range(UAV_config.N)])
v_value = np.array([np.sqrt(v_ba[i, 0]) for i in range(UAV_config.N)])
ah = v_ba[0:UAV_config.N] * fai_d / fai_c
thita_value = np.arcsin(fai_s)
temp1 = coe1 * temp * np.matrix(k)
if iter >= 1:
x_differ = np.zeros([UAV_config.N + 1, 1])
y_differ = np.zeros([UAV_config.N + 1, 1])
deta1_differ = np.zeros([UAV_config.N + 1, 1])
v_ba_differ = np.zeros([UAV_config.N + 1, 1])
for i in range(UAV_config.N):
x_differ[i] = xf * y[i * nx + 0] - result[0, i]
y_differ[i] = xf * y[i * nx + 1] - result[1, i]
if UAV_config.N == 1:
deta1 = np.array(deta1)
deta1_differ[i] = deta1[i] - detal_s[i]
v_ba_differ[i] = v_ba[i] - np.square(initial_trajectory.V)
deta_x = np.max(np.abs(x_differ))
deta_y = np.max(np.abs(y_differ))
deta1_deta1 = np.max(np.abs(deta1_differ))
deta1_v = np.max(np.abs(v_ba_differ))
# print('Maximum difference between two successive solutions are:\n')
# print("x: {}, y:{} deta1:{} v_ba:{}".format(np.max(np.abs(x_differ)), np.max(np.abs(y_differ)),
# np.max(np.abs(deta1_differ)), np.max(np.abs(v_ba_differ))))
if np.max(np.abs(x_differ)) <= 1 and np.max(np.abs(y_differ)) <= 1 and \
np.max(np.abs(deta1_differ)) <= 0.1 and np.max(np.abs(v_ba_differ)) <= 1:
flag = 0
else:
iter = iter + 1
if iter > 5:
break
for i in range(UAV_config.N):
detal_s[i] = deta1[i]
result = np.squeeze(np.array([x_pos, y_pos]))
x_pos_new = np.cos(thita) * x_pos + -np.sin(thita) * y_pos + x_initial
y_pos_new = np.sin(thita) * x_pos + np.cos(thita) * y_pos + y_initial
xf_new = np.cos(thita) * xf + -np.sin(thita) * yf + x_initial
yf_new = np.sin(thita) * xf + np.cos(thita) * yf + y_initial
x_pos = x_pos_new
y_pos = y_pos_new
thita_value = thita_value + thita
x1_new = np.zeros([no_fly_zone.M, 1])
y1_new = np.zeros([no_fly_zone.M, 1])
for i in range(no_fly_zone.M):
x1_new[i] = np.cos(thita) * x1[i] + -np.sin(thita) * y1[i] + x_initial
y1_new[i] = np.sin(thita) * x1[i] + np.cos(thita) * y1[i] + y_initial
x1[i] = x1_new[i]
y1[i] = y1_new[i]
np_ = 100
avoid_circle1 = np.zeros([2, np_ + 1, no_fly_zone.M]) # no - fly - zone 1
for j in range(no_fly_zone.M):
for i in range(np_ + 1):
avoid_circle1[0, i, j] = x1[j] + no_fly_zone.a_NFZ[j] * np.cos(
i / np_ * 2 * np.pi) # no - fly - zone 1
avoid_circle1[
1, i,
j] = y1[j] + no_fly_zone.b_NFZ[j] * np.sin(i / np_ * 2 * np.pi)
'''fig, ax = plt.subplots()
ax.plot(u12_square)
ax.set_title("u12_square")
ax.set_xlim((0, 1))
ax.set_ylim((0, 1.2))
ax.plot([0, 1], [1, 1])
plt.show()'''
return x_pos, y_pos, thita_value, v_value, at, ah, avoid_circle1
assert(cleanspace(str(pic.sum([AA[i]*Z[i] for i in range(5)],'i','[5]') == 0))
== cleanspace(str('# (2x2)-affine constraint: Σ_{i in [5]} A[i]*Z[i] = |0| #'))
)
#cons slacks
assert( (abs(x) < (2|x-1)).slack[0] == 2*sum(xv-1)- np.linalg.norm(xv))
assert( (1 < (t-1)*(x[2]+x[3]) ).slack[0] == ((tv-1) * (xv[2]+xv[3])-1)[0])
#powers
assert((Z[0][4]**(2./3)).value == Zv[0][4]**(2./3))
assert( cvxcomp(
((1-t)**0.6666 > x[0]).slack,
(1-tv)**0.6666 - xv[0]) < 1e-4)
assert((pic.norm(-x,'inf') < 2).slack[0] == -3)
M = prob.add_variable('M',(5,5),'symmetric')
M.value = [1+(i+j)+2*(i+j)**2-0.01*(i+j)**4 + (25 if i==j else 0) for i in range(5) for j in range(5)]
assert( cvxcomp((t < pic.detrootn(M)).slack, np.linalg.det(M.value)**(1./5) - tv[0] ) < 1e-6)
#---------------#
# Complex SDP #
#---------------#
P = pic.Problem()
Z = P.add_variable('Z',(3,2),'complex')
assert(cleanspace(str(Z.real))==cleanspace('# variable Z_RE:(3 x 2),continuous #'))
assert(cleanspace(str(Z.imag))==cleanspace('# variable Z_IM:(3 x 2),continuous #'))
assert(Z.vtype == 'complex')
str(pic.sum([AA[i] * Z[i] for i in range(5)], 'i', '[5]') == 0)) ==
cleanspace(
str('# (2x2)-affine constraint: Σ_{i in [5]} A[i]*Z[i] = |0| #')))
#cons slacks
assert ((abs(x) <
(2 | x - 1)).slack[0] == 2 * sum(xv - 1) - np.linalg.norm(xv))
assert ((1 < (t - 1) * (x[2] + x[3])).slack[0] == ((tv - 1) * (xv[2] + xv[3]) -
1)[0])
#powers
assert ((Z[0][4]**(2. / 3)).value == Zv[0][4]**(2. / 3))
assert (cvxcomp(((1 - t)**0.6666 > x[0]).slack,
(1 - tv)**0.6666 - xv[0]) < 1e-4)
assert ((pic.norm(-x, 'inf') < 2).slack[0] == -3)
M = prob.add_variable('M', (5, 5), 'symmetric')
M.value = [
1 + (i + j) + 2 * (i + j)**2 - 0.01 * (i + j)**4 + (25 if i == j else 0)
for i in range(5) for j in range(5)
]
assert (cvxcomp((t < pic.detrootn(M)).slack,
np.linalg.det(M.value)**(1. / 5) - tv[0]) < 1e-6)
#---------------#
# Complex SDP #
#---------------#
P = pic.Problem()
Z = P.add_variable('Z', (3, 2), 'complex')
def inventory_aarc(c, h, b, d_0, r):
T = len(c)
c_pic = pic.new_param('c', c)
h_pic = pic.new_param('h', h)
b_pic = pic.new_param('b', b)
r_pic = pic.new_param('r', r)
d0_pic = pic.new_param('d_0', d_0)
# Define auxiliary parameters
P_arr = []
for t in range(T):
P_t = np.concatenate((np.eye(t+1), np.zeros((t+1, T - (t + 1)))), axis=1)
P_arr.append(pic.new_param('P' + str(t+1), P_t))
ones_arr = []
for t in range(T):
ones_t = np.ones((t+1, 1))
ones_arr.append(pic.new_param('1_' + str(t+1), ones_t))
# Initialize picos problem.
pb = pic.Problem()
# Add variables.
x0 = pb.add_variable("x0", T)
x_arr = []
for t in range(T-1):
x_arr.append(pb.add_variable("x" + str(t + 1), t+1))
v0 = pb.add_variable("v0", T)
v_arr = []
for t in range(T):
v_arr.append(pb.add_variable("v" + str(t + 1), T))
u = pb.add_variable("u")
lamda = pb.add_variable("lambda")
eta_arr = []
for t in range(T):
eta_arr.append(pb.add_variable("eta" + str(t + 1)))
rho_arr = []
for t in range(T):
rho_arr.append(pb.add_variable("rho" + str(t + 1)))
# Define auxiliary variables
c_tilde = sum([c_pic[t + 1] * P_arr[t].T * x_arr[t] + v_arr[t] for t in range(T-1)]) + v_arr[-1]
c_h_arr = []
for t in range(T):
if t == 0:
c_h_arr.append(-h_pic[0] * P_arr[0].T * ones_arr[0] - v_arr[0])
else:
c_h_arr.append(h_pic[t] * sum([P_arr[s].T * x_arr[s] for s in range(t)]) -
h_pic[t] * P_arr[t].T * ones_arr[t] - v_arr[t])
c_b_arr = []
for t in range(T):
if t == 0:
c_b_arr.append(b_pic[0] * P_arr[0].T * ones_arr[0] - v_arr[0])
else:
c_b_arr.append(-b_pic[t] * sum([P_arr[s].T * x_arr[s] for s in range(t)]) +
b_pic[t] * P_arr[t].T * ones_arr[t] - v_arr[t])
# Add constraints.
pb.add_constraint(u >= (c_pic | x0) + (1 | v0) + (c_tilde | d0_pic) + lamda * r_pic)
for t in range(T):
pb.add_constraint(v0[t] >= h_pic[t] * ones_arr[t].T * P_arr[t] * x0 + (c_h_arr[t] | d0_pic) + eta_arr[t] * r_pic)
for t in range(T):
pb.add_constraint(v0[t] >= -b_pic[t] * ones_arr[t].T * P_arr[t] * x0 + (c_b_arr[t] | d0_pic) + rho_arr[t] * r_pic)
pb.add_constraint(lamda >= pic.norm(c_tilde, 1))
for t in range(T):
pb.add_constraint(eta_arr[t] >= pic.norm(c_h_arr[t], 1))
for t in range(T):
pb.add_constraint(rho_arr[t] >= pic.norm(c_b_arr[t], 1))
# Set objective.
pb.set_objective("min", u)
solution = pb.solve(solver='cvxopt')
return pb, solution