% ov7oppg3.m
% Script for loesning av oppgave 3 paa oeving 7.
% David Di Ruscio.

a=0.9; b=0.5;                       % Modellparametre.
q=2; p=1; s=2;                      % Vektparametre.
x0=10;                              % Initialverdi for tilstanden.
N=10;                               % Slutt-tid (diskret tidspunkt).

R=zeros(N,1);                       % Setter av plass for løsningene av Riccati-ligningen.
G=zeros(N-1,1);                     % Setter av plass for tilbakekoplingskoeffisientene.
r=s;                                % Grensebetingelse, R_N=S_N.
R(N)=s;
for k=N-1:-1:1                      % Itererer fra slutt-tiden til initial-tidspunktet.
   k
   g=-b*a*r/(p+b^2*r);              % Optimal tilbakekoplings-konstant, g(k)=f(r(k+1)).
   r=a^2*r-a^2*b^2*r^2/(p+b^2*r)+q; % Den skalare Riccati-ligning, r(k)=f(r(k+1)).
   R(k)=r;
   G(k)=g;
end   

Y=zeros(N,1); U=zeros(N-1,1);       % Simulerer systemet med optimal LQ-regulator.
x=x0;                               % Initialverdi for tilstanden.
for k=1:N-1                         
   y=x;
   Y(k)=x;
   u=G(k)*x;
   U(k)=u;
   x=a*x+b*u;
end  
Y(N)=x;

Ys=zeros(N,1); Us=zeros(N-1,1);     % Simulerer systemet med suboptimal LQ-regulator.
x=x0;                               % Initialverdi for tilstandsvektoren.
for k=1:N-1            
   y=x;
   Ys(k)=x;
   u=G(1)*x;
   Us(k)=u;
   x=a*x+b*u;
end  
Ys(N)=x;

figure(1)
subplot(211), plot(1:length(Y),Y,'bo-'), ylabel('y_k')
subplot(212), plot(1:length(U),U,'bo-'), ylabel('u_k')

figure(2)
subplot(211), plot(1:length(R),R,'bo-'), ylabel('r_k')
subplot(212), plot(1:length(G),G,'bo-'), ylabel('g_k')
xlabel('Discrete time: 1 \leq k \leq 10')

