% Yalmip Test:
%% Option 1:
n = 6;
N = 100;
nu = 3;
A = sdpvar(n,n,N-1,'full');
B = sdpvar(n,nu,N-1,'full');
x = sdpvar(n,N,'full');
u = sdpvar(nu,N-1,'full');
x0 = sdpvar(6,1,'full');
xf = sdpvar(6,1,'full');
Constraints = [x0==x(:,1)];
Objective = 0;
for i = 1:N-1
Constraints = [Constraints,x(:,i+1) == A(:,:,i)*x(:,i) + B(:,:,i)*u(:,i)];
Objective = Objective + norm(u(:,i),2);
end
Constraints = [Constraints,xf == x(:,N)];
Optimizer1 = optimizer(Constraints,Objective,sdpsettings('solver','mosek'),{A,B,x0,xf},{x,u});
% Run Optimizer:
A = zeros(n,n,N-1);
B = zeros(n,nu,N-1);
for i = 1:N-1
A(:,:,i) = [eye(3) .1*eye(3);zeros(3,3) eye(3)];
B(:,:,i) = [.1*eye(3);eye(3)];
end
x0 = 1*eye(6,1);
xf = zeros(6,1);
Solution = Optimizer1{{A,B,x0,xf}};
%% Option 2:
z = sdpvar(n*N+nu*(N-1),1);
IndexX = @(z,i) z((i-1)*n+1:i*n);
IndexU = @(z,i) z(N*n+(i-1)*nu+1:N*n+i*nu);
H = sdpvar(n*(N+1),n*N+nu*(N-1));
g = sdpvar(n*(N+1),1);
Objective = 0;
for i = 1:N-1
Objective = Objective + norm(IndexU(z,i),2);
end
Constraints = [H*z==g];
Optimizer2 = optimizer(Constraints,Objective,sdpsettings('solver','mosek'),{H,g},{z});
% Run Optimizer:
H = zeros(n*(N+1),n*N+nu*(N-1));
for i = 1:N-1
H((i-1)*n+1:i*n,(i-1)*n+1:i*n) = [eye(3) .1*eye(3);zeros(3,3) eye(3)];
H((i-1)*n+1:i*n,(i)*n+1:(i+1)*n) = -eye(6);
H((i-1)*n+1:i*n,N*n+(i-1)*nu+1:N*n+i*nu) = [.1*eye(3);eye(3)];
g((i-1)*n+1:i*n) = zeros(6,1);
end
H((N-1)*n+1:N*n,1:n) = eye(6);
g((N-1)*n+1:N*n) = 1*eye(6,1);
H((N)*n+1:(N+1)*n,(N-1)*n+1:N*n) = eye(6);
g((N)*n+1:(N+1)*n) = zeros(6,1);
Solution = Optimizer2{{H,g}};