% Oeving 3
% Avansert reguleringsteknikk
% Oppgave 1b. Design av optimal PI regulator for scalar prosess.

clear all
a=-0.1; b=1; d=1; x0=5; % Prosessmodellparametre.
r=1;                    % referanse for y=x.
A=[a,0;-d,0]; B=[b;0];  % Utvidede matriser, prosess og regulator.

Q=[2,0;0,1]; P=1;       % Vektmatriser.

[K,R]=lqr(A,B,Q,P);     % Løser LQ optimalregulator problemet.
G=-K;                   % Regulatormatrise/konstant.
g1=G(1); g2=G(2);

% Simulering av tilbakekoplet/lukket system.
dt=0.01;                % Skrittlengde.
t0=0; t1=8;             % Start- og slutt-tid, t0 <= t <= t1                   
N=ceil((t1-t0)/dt)+1;        % Antall diskrete sampler i simuleringshorisonten.
                
X=zeros(N,2);           % Array for å holde/lagre tilstandene.
T=zeros(N,1);           % Array for å holde kontinuerlig tidsvariabel.

x=x0                    % Initialverdi for tilstanden.
us=-inv(d*inv(a)*b)*r;
zs=(us-g1*r)/g2;
z=zs;                   % Initialverdi for regulatortilstand. Også ok med z=0 !!!
t=t0;                    % Starttidspunkt
for k=1:N
   y=d*x;
   X(k,1)=x';           % Lagrer prosess-tilstanden
   X(k,2)=z';           % Lagrer regulatortilstanden.
   T(k,:)=t;            % Lagrer tidsvariabel
   
   u=g1*x+g2*z;         % Regulator
   x=x+dt*(a*x+b*u);    % Oppdaterer prosessen. Integrerer med (eksplisitt) Euler.
   z=z+dt*(r-y);        % Oppdaterer regulatortilstanden.
   t=t+dt;              % Oppdaterer tidsvariabel   
end

subplot(211), plot(T,X(:,1)), grid, title('Oeving 3b')
ylabel('x'), xlabel('Kontinuerlig tid')
subplot(212), plot(T,X(:,2)), grid, 
ylabel('z'), xlabel('Kontinuerlig tid')