%Simulace vlivu periody vzorkovani na kvalitu regulace

clear all;
%close all;

%-------------- Parametry simulace ----------
Tp = 50;											%doba predikce
Q = eye(Tp)*1000;								%Vahova matice
n = 300;											%doba simulace
%ref = [5*ones(1,n/2),22*ones(1,n/2)];	%reference
ref=[zeros(1,n/3),ones(1,2*n/3)];

K = 3;											%Pocet simulaci
Ts = [0.15,0.1,0.05];
Umax = 10 										%Omezeni ridici veliciny pro jednotlive simulace
Umin = -10

uh = zeros(K,n);
yh = zeros(K,n);
for k = 1 : K
   k
%------------- System --------------
P = tf([10],[1 -1 -1 1]);

Pd = c2d(P,Ts(k));
[M,N,C,D]=ssdata(Pd);

%-------------- Inicializace ---------------
for i=0:1:Tp-1
   V(i+1,:) = C*M^i;
end

S = zeros(Tp);

for j=1:1:Tp-1
	for i=0:1:Tp-1-j
      S(i+j+1,i+1)=C*M^(j-1)*N;
   end
end

R = eye(Tp);

%------------- Matice MPC regulatoru -----------
H = S'*Q*S + R;
H = (H+H')/2;
W = zeros(Tp);
z = zeros(Tp,1);

%-------------- Simulace -------------
umin = ones(Tp,1)*Umin;
umax = ones(Tp,1)*Umax;
Ns = size(M,1);
Start = Ns+1;
x = zeros(Ns,1);
for t = Start : n
   RefVyber = ref(t)*ones(Tp,1);
   yx = V*x;
   j = (yx - RefVyber)'*Q*S;
   Up = qp(H,j,W,z,umin,umax);
   u = Up(1);
   y = C*x + D*u;
   x = M*x + N*u;
   uh(k,t) = u;
   yh(k,t) = y;
end
end

%-------------- Vystupy -----------
figure(4);
barvy = ['b','g','m'];
subplot(2,1,1); 
for k = 1 : K
   plot(1:n,uh(k,:),barvy(k)); hold on;
end
title('Ridici velicina'); xlabel('k'); ylabel('u');
legend('Ts = 0.15','Ts = 0.1','Ts = 0.05');

subplot(2,1,2); 
for k = 1 : K
   plot(1:n,yh(k,:),barvy(k)); hold on;
end
plot(1:n,ref,'r');
title('Regulovana velicina'); xlabel('k'); ylabel('y,ref');
legend('Ts = 0.15','Ts = 0.1','Ts = 0.05','Reference',2);