%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),22*ones(1,Tp)];	%reference
ref=[zeros(1,n/3),ones(1,2*n/3)];

K = 2;											%Pocet simulaci
Ts = 0.1
Umax = 200 										%Omezeni ridici veliciny pro jednotlive simulace
Umin = -200

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

Pd = c2d(P,Ts);
[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(1,t) = u;
   yh(1,t) = y;
end

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

%-------------- Vystupy -----------
figure(7);
barvy = ['b','g','m'];
subplot(1,2,1); 
for k = 1 : K
   plot(1:n,uh(k,:),barvy(k)); hold on;
end
title('Ridici velicina'); xlabel('k'); ylabel('u');
legend('neznama ref.','znama ref.');

subplot(1,2,2); 
plot(1:n,yh(1,:),'b',1:n,yh(2,:),'g',1:n,ref(1:n),'r');
title('Regulovana velicina'); xlabel('k'); ylabel('y,ref');
legend('neznama ref.','znama ref.','Reference',2);