N = 2;
U = sdpvar(2,N,'full');
W = sdpvar(2,N,'full');
x = sdpvar(8,N+1,'full');
d = binvar(4,N,'full');
C = [1 0 0 0 0 0 0 0;
0 1 0 0 0 0 0 0;
0 0 0 0 1 0 0 0;
0 0 0 0 0 1 0 0;];
objective = 0;
constraints = [];
for k = 1:N
constraints = [constraints, implies(d(1,k), [(x(7,k)-x(5,k)+a1*x(6,k)) >= 0, ...
1000*(x(7,k)-x(5,k)+a1*x(6,k)) <= U(1,k) <= 10000*(x(7,k)-x(5,k)+a1*x(6,k))])];
constraints = [constraints, implies(d(2,k), [(x(7,k)-x(5,k)+a1*x(6,k)) <= 0, ...
10000*(x(7,k)-x(5,k)+a1*x(6,k)) <= U(1,k) <= 1000*(x(7,k)-x(5,k)+a1*x(6,k))])];
constraints = [constraints, d(1,k) + d(2,k) == 1];
constraints = [constraints, implies(d(3,k), [(x(8,k)-x(5,k)-a2*x(6,k)) >= 0, ...
1000*(x(8,k)-x(5,k)-a2*x(6,k)) <= U(2,k) <= 10000*(x(8,k)-x(5,k)-a2*x(6,k))])];
constraints = [constraints, implies(d(4,k), [(x(8,k)-x(5,k)-a2*x(6,k)) <= 0, ...
10000*(x(8,k)-x(5,k)-a2*x(6,k)) <= U(2,k) <= 1000*(x(8,k)-x(5,k)-a2*x(6,k))])];
constraints = [constraints, d(3,k) + d(4,k) == 1];
constraints = [constraints, x(:,k+1) == Ad*x(:,k) + Bd*U(:,k)];
constraints = [constraints, -25000 <= U(:,k) <= 25000];
constraints = [constraints, -5 <= x(:,k) <= 5];
constraints = [constraints, -5 <= x(:,k+1) <=5];
objective = objective + (C*x(:,k+1))'*diag([1e10 1e11 1e7 1e7])*(C*x(:,k+1));
end
sol = solvemp(constraints, objective, sdpsettings('verbose',1,'showprogress',1),x(:,1),U(:,1));