%% ordinary least squares, constrained least squares:

m=16; n=8;
A = randn(m,n);
b = randn(m,1);

x_ls = inv(A'*A)*A'*b;  % = A\b

cvx_begin
variable x(n);
minimize( norm(A*x - b, 2) );
cvx_end


l = zeros(n,1);
u = ones(n,1);

cvx_begin
variable x(n);
minimize( norm(A*x - b, 2) );
subject to
x >= l;
x <= u;
cvx_end




%% a generic linear control problem:
nX = 5;
nU = 1;

A = randn(nX, nX);
B = randn(nX, nU);

Q = eye(nX);
R = eye(nU);

x1 = rand(nX,1);

T = 20;

cvx_begin
variables x(5,T)  u(1,T) s(1,T);
minimize( sum(s) )
subject to  
  for t=1:T-1
	  x(:,t+1) == A*x(:,t) + B*u(:,t);
  end
  for t=1:T
	  s(1,t) >= x(:,t)'*Q*x(:,t) + u(:,t)'*R*u(:,t);
  end
  x(:,1) == x1;
cvx_end


figure; plot(x');


% let's execute control policy:

x_run(:,1) = x1;

for t=1:T-1
	x_run(:,t+1) = A*x_run(:,t) + B*u(:,t);
end

figure; plot(x_run');

% let's execute with noise

x_run2(:,1) = x1;

for t=1:T-1
	x_run2(:,t+1) = A*x_run2(:,t) + B*u(:,t) + randn(nX,1);
end

figure; plot(x_run2');


% let's execute a closed loop control policy by using MPC:

x_run3(:,1) = x1;


for k=1:T-1
	cvx_begin
	variables x(nX,T)  u(nU,T) s(1,T);
	minimize( sum(s(k:T)) )
	subject to
	for t=k:T-1
		x(:,t+1) == A*x(:,t) + B*u(:,t);
	end
	for t=k:T
		s(1,t) >= x(:,t)'*Q*x(:,t) + u(:,t)'*R*u(:,t);
	end
	
	x(:,k) == x_run3(:,k);
	cvx_end
	
	x_run3(:,k+1) = A*x_run3(:,k) + B*u(:,k) + randn(nX,1);
	
	

end

figure; plot(x_run3');
