% main_ov4_oppg2.m
% Loesning til oeving 4 oppgave 2.

A=[-6,5;5,-6];           % Systemmatrise (matrise til tilstandsrommodellen).
B=eye(2);
Q=[135,60;60,135];       % Vektmatrise for tilstandsvekoren, x.
P=eye(2);                % vektmatrise for paadragsvektoren, u.

% a)
[K,R]=lqr(A,P,Q,P);      % Loeser Riccatiligningen (Control system toolbox funksjon).
                         % Loeser Riccatiligningen via Hamiltonmatrisen.
H=B*inv(P)*B';           % 
F=[A -H;-Q -A'];         % Hamiltonmatrisen.
t1=10; t=0;              % Velger en passe stor (uendelig) slutt-tid, t1 og t=0.
Phi=expm(F*(t1-t))       % Beregner transisjonsmatrisen.
P21=Phi(3:4,1:2);        
P22=Phi(3:4,3:4);
R_2=-inv(P22)*P21        % Loesning av Riccati-ligningen.

% b)
G=-K;                    % Merk: forskjellig fortegnsdefinisjon, pådrag, u=G*x.
G_2=-inv(P)*B'*R_2       % Optimalt pådrag, u=G_2*x.

% c) 
l_cl=eig(A+B*G)          % Egenverdiene til det lukkede systemet.
l_ol=eig(A)              % Egenverdiene til det åpne systemet.

