% Control Theory 2020


%doppio integratore
% x(t+1)=PHIx(t)+GAMMAu(t)

PHI=[1 1;...
     0 1]
 
GAMMA=[0 1]'

xi=[10 0]' %alpha
xf=[0 0]'  %beta

%orizzonte di primo tentativo
N=2

n=size(PHI,1) %ordine del sistema

A=[eye(N)   , -ones(N,1);...
   -eye(N)  , -ones(N,1)]
b=zeros(2*N,1)

Aeq=[];
for ii=N-1:-1:0
    Aeq=[ Aeq PHI^ii*GAMMA]%#ok
end

Aeq =[Aeq zeros(n,1)]

beq = xf-PHI^N*xi

% f'*x = M
% min M al variare di x=[u(0),...u(T-1),M]
% soggetto ai vincoli (*) e (**)
f=[zeros(1,N), 1]'

[x,Mmin,exitflag]=linprog(f,A,b,Aeq,beq)

% se Mmin > 1 allora significa che bisogna aumentare l'orizzonte


uopt=x(1:N)
X=xi
for ii=1:N 
 X=[X PHI*X(:,ii)+GAMMA*uopt(ii)];
end
figure
plot(X(1,:),X(2,:),'ro-')
axis([-10 10 -10 10])
grid


% per esercizio: aggiungere il vincolo sull stato: |x2(t)|<= 2.4