% Problema di tempo minimo open loop 


%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=7


% per risolvere il problema di tempo minimo č necessario aumentare
% iterativamente l'orizzonte T fino a quando il problema non risulta
% risolvibile con controlli minori di uno (in modulo)
% il controllo massimo č il valore della variabile ausialiaria M all'ottimo
% (cioč Mmin restituito da linprog)
% cfr Zadeh, L. A. e Whalen, B.,  "On optimal control and linear
% programming," IRE Transactions on Automatic Control, 7(4), 45-46, 1962 

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

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

%%
%A=[A;...
%   zeros(1,T) 1 ]
%b=[b;...
%    1]
%%
Aeq=[];
for ii=N-1:-1:0
    Aeq=[ Aeq PHI^ii*GAMMA];
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]=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

c1=[0 -1]'
c10=2.4

Ai=zeros(N+1,N);
bi=zeros(N+1,1)
for ii=1:N+1
    for jj=1:ii-1
    Ai(ii,jj)=c1'*PHI^(ii-jj-1)*GAMMA;
    end
    bi(ii)= -c1'*PHI^(ii-1)*xi+c10;
end
Ai=[Ai zeros(size(Ai,1),1)]
%bi=c10*ones(size(Ai,1),1)

A=[A;Ai]
b=[b;bi]

[x,Mmin]=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