clear all;
clc;

%Výpoèet øízení
%Je potøeba zadat:
%   1) Matice M a N ze stavového modelu
%   2) (n-1).stavový vektor a vektor parametrù
%   3) Váhy a kriteriální matici
%   4) Kolikakrokové øízení dìláme
%   5) Jakou køivku má výstup sledovat
%   6) Øád modelu
%   7) Podle øádu modelu musíme zadat nìkolik prvních výstupù a øízení

%Model: y(n)=4u(n)+1y(n-1)+2u(n-1)+3+e(n)
%Kritérium K=V1*y(n)^2+V2*u(n)^2

%Zadání modelu ve stavovém tvaru:
B0=1; A1=0.95; B1=0.2; K=-0.01;

M=[ A1 B1 K
    0  0  0
    0  0  1];

N=[B0 
   1 
   0]; 

%Zadání vah
V1=1;      %Dùležitost optimalizace výstupu
V2=5;      %Dùležitost optimalizace øízení

%Poèet krokù
nd=50;

%Jakou køivku má výstup sledovat (fialová)
g(1:10)=20;
g(11:15)=50;
g(16:30)=30;
g(31:nd-5)=30:-30/(nd-5-31):0;
g(nd-4:nd)=0;

%Øád modelu
Rad=1;

%Zadání posledních namìøených výstupù a øízení
y(1)=0;
u(1)=0;

%Výpoèet øízení
R=zeros(size(M));
for n=nd:-1:Rad+1
    
    Om=[ V1        0    -V1*g(n)       %Kriteriální matice pro dané kolo
         0         V2    0             %Hodnoty v rozích implementují 
        -V1*g(n)   0     V1*g(n)^2];   %èlen pro rychlost: V1*(y(n)-g(n))^2

    T=R+Om;                   %Souèet kriteriální matice a zbytku kritéria z minulého kola
    S{n}=-inv(N'*T*N)*N'*T*M; %Každá matice S je jednou maticí pro pøepoèet stavu na 
                              %pro jedno kolo optimálního øízení
                              %Složená závorka znamená, že se jedná o pole
                              %matic. Objekt je tedy tøírozmìrný. 
    R=M'*(T-T*N*inv(N'*T*N)*N'*T)*M; %Zbytková matice pro další kolo
end;

for n=Rad+1:nd     %!!!!!!!!!!!!Nutno zadat
    x=[y(n-1) u(n-1) 1]';       %(n-1).stavový vektor
    u(n)=S{n}*x;                %Spoètené øízení
    Psi=[u(n);x]';              %Vektor vstupù
    Theta=[B0 A1 B1 K];         %Vektor parametrù
    y(n)=Psi*Theta';            %Odhadované výstupy
end;

hold on;
plot(g,'m.-');  %Chtìný výstup
plot(y,'b.-');  %Odhadovaný výstup
plot(u,'g.-');  %Optimální øízení
plot(1:nd,zeros(1,nd),'k-');
legend('Chtìný výstup','Odhadovaný výstup','Optimální øízení')
hold off;






