%% Setup the optimization problem
u = sdpvar(repmat(nu,1,N),repmat(1,1,N));
x = sdpvar(repmat(nx,1,N+1),repmat(1,1,N+1));
r = sdpvar(ny,1);
d = binvar(repmat(10,1,N+1),repmat(1,1,N+1));
constraints = [];
objective = 0;
for k = 1:N
objective = objective + (r-Cd{1}*x{k})'*Q*(r-Cd{1}*x{k})+u{k}*R*u{k};
Model1 = [x{k+1} == Ad{1}*x{k}+Bd{1}*u{k}, x{k}(4) <= ix(1)];
Model2 = [x{k+1} == Ad{2}*x{k}+Bd{2}*u{k}, ix(1) <= x{k}(4) <= ix(2)];
Model3 = [x{k+1} == Ad{3}*x{k}+Bd{3}*u{k}, ix(2) <= x{k}(4) <= ix(3)];
Model4 = [x{k+1} == Ad{4}*x{k}+Bd{4}*u{k}, ix(3) <= x{k}(4) <= ix(4)];
Model5 = [x{k+1} == Ad{5}*x{k}+Bd{5}*u{k}, ix(4) <= x{k}(4) <= ix(5)];
Model6 = [x{k+1} == Ad{6}*x{k}+Bd{6}*u{k}, ix(5) <= x{k}(4) <= ix(6)];
Model7 = [x{k+1} == Ad{7}*x{k}+Bd{7}*u{k}, ix(6) <= x{k}(4) <= ix(7)];
Model8 = [x{k+1} == Ad{8}*x{k}+Bd{8}*u{k}, ix(7) <= x{k}(4) <= ix(8)];
Model9 = [x{k+1} == Ad{9}*x{k}+Bd{9}*u{k}, ix(8) <= x{k}(4) <= ix(9)];
Model10 = [x{k+1} == Ad{10}*x{k}+Bd{10}*u{k}, ix(9) <= x{k}(4)];
constraints = [constraints,...
implies(d{k}(1), Model1),...
implies(d{k}(2), Model2),...
implies(d{k}(3), Model3),...
implies(d{k}(4), Model4),...
implies(d{k}(5), Model5),...
implies(d{k}(6), Model6),...
implies(d{k}(7), Model7),...
implies(d{k}(8), Model8),...
implies(d{k}(9), Model9),...
implies(d{k}(10), Model10),...
sum(d{k}) == 1];
constraints = [constraints,...
[umin] <= u{k}<= [umax],...
xmin <= x{k} <= xmax];
end
constraints = [constraints, xmin <= x{k+1} <= xmax];