%Simulace vlivu omezeni ridici veliciny na kvalitu regulace

clear all;
%close all;

%------------- System --------------
P = tf([10],[1 -1 -1 1]);

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

%-------------- 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
Umax = [10,5,2]; 							%Omezeni ridici veliciny pro jednotlive simulace
Umin = [-10,-5,-2];

%-------------- 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 -------------
uh = zeros(K,n);
yh = zeros(K,n);
for i = 1 : K   
	umin = ones(Tp,1)*Umin(i);
	umax = ones(Tp,1)*Umax(i);
	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(i,t) = u;
	   yh(i,t) = y;
   end
end

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

subplot(2,1,2); 
for i = 1 : K
   plot(1:n,yh(i,:),barvy(i)); hold on;
end
plot(1:n,ref,'r');
title('Regulovana velicina'); xlabel('k'); ylabel('y,ref');
legend('u = (-10,10)','u = (-5,5)','u = (-2,2)','Reference',2);