
%ottiSI.m
%Questo file calcola i vettori ottimi dello stato e dell' ingresso per raggiungere uno stato Xf partendo da uno stato X0, 
%in un certo numero di passi , con le limitazioni precedentemente descritte ; 

delete(figure(gcf))
%*****************matrici necessarie(a,b,X0,XF,Us,Ui,Xs,Xi,Np,f)
c=0;
dx= size(a,2);
du= size(b,2);
%*******************************calcolo di f devo fare in modo che prenda 
f=f1*ones(Np*(du+dx)+dx,1);
%**********************************H1 matrice di ottimizzazione
H1=[h1*eye(Np*dx+dx),zeros(Np*dx+dx,Np*du);zeros(Np*du,Np*dx+dx),h1*eye(Np*du)];
%***************************************vettore di matrici a
A7=[];
for i = 0 : Np-1;
    A7(dx*i+1:dx*i+dx,:)=[a^(i+1)];
end
A7;
%**********************************aggiornamento di W1
T=[];
for i = 0 : Np-1;
    T(dx*i+1:dx*i+dx,:)=[zeros(dx,i),a^i*b,zeros(dx,Np-1-i)];
end
R=rot90(triu(ones(Np)));
W1=T*R;
for i = 1 : Np-1;
    g=W1(:,i+1);
    g(dx*Np-i*dx+1:dx*Np)=[];
    W1(:,i+1)=[zeros(i*dx,1);g];
end
A=[eye(dx),zeros(dx,Np*dx+Np*du);-eye(dx),zeros(dx,Np*dx+Np*du);zeros(dx,Np*dx),eye(dx),zeros(dx,Np*du);zeros(dx,Np*dx),-eye(dx),zeros(dx,Np*du);eye(dx*Np+dx),zeros(dx*Np+dx,Np*du);-eye(dx*Np+dx),zeros(dx*Np+dx,Np*du);zeros(Np*du,dx*Np+dx),eye(Np*du);zeros(Np*du,dx*Np+dx),-eye(Np*du);A7,eye(Np*dx),W1;-A7,-eye(dx*Np),-W1];
B=[X0',-X0',XF',-XF',Xs*ones(1,dx*Np+dx),Xi*ones(1,dx*Np+dx),Us*ones(1,Np*du),-Ui*ones(1,Np*du),zeros(1,2*dx*Np)]';
[y,Fval,exitFl]=quadprog(H1,f,A,B);
%*********attenzione devo suddividere x in stato e in ingresso
y;
x7=y(1:dx*Np+dx,:);
disp('il vettore x rappresenta (x(0),x(1),.......x(Np)')
x=reshape(x7,dx,Np+1)
u7=y(dx*Np+dx+1:dx*Np+dx+du*Np,:);
disp('il vettore u rappresenta (u(0),u(1),.......u(Np-1)')
u=reshape(u7,du,Np)
%******************************numero passi
Np
