% UE_2LQR   Initialisierung der I2-Strecke / Reglerentwurf
% G=(A,B,C,D)      Strecke
%
% CAD-Uebung zur Vorlesung Entwurf Robuster Regelungen
% Thema: Optimale Zustandsregelung

% M. Grobe  10/2003
% Technische Universitaet Braunschweig, IfR

% doppelter Integrator mit Anfangswerten
G=tf(1,[1 0 0]);
[A,B,C,D]=ssdata(G);

% Entwurfsparameter
r0=1;
q10=0;
q20=1;

% Q-Matrix (Gewicht fuer Sustandsgroessen)
Q=eye(size(A));
Q(1,1)=q10;
Q(2,2)=q20;

% R-Matrix (Gewicht fuer Ausgangsgroesse)
R=r0;

% LQR Reglerentwurf nach VL
% loesen der Riccati-Gleichung (CARE): A'*P + P*A - P*B*inv(R)*B'*P + Q = 0
P=are(A,B*inv(R)*B',Q);
% Regler berechnen
F=-inv(R)*B'*P;

% LQR Reglerentwurf mit MATLAB
F2=-lqr(A,B,Q,R); % MATLAB nimmt Gegenkopplung fuer Zustandsregler an, wir Mitkopplung

% Vorfilter fuer stationaere Genauigkeit
Rvor=1/dcgain(A+B*F,B,C,D);

% Gesamtsystem
Ag=A+B*F;
Bg=B*Rvor;
Cg=C;
Dg=D*Rvor;
Gg=ss(Ag,Bg,Cg,Dg);

figure(1)
clf
step(Gg)
title('Sprunganwort für gewähltes Q und R')

figure(2)
clf
figure(3)
clf
for q2=[0.1 0.25 0.5 1 2 5],
    Q(2,2)=q2;
    F_=-lqr(A,B,Q,R);
    R_=1/dcgain(A+B*F_,B,C,D);
    Gg_=ss(A+B*F_,B*R_,C,D*R_);
    Gu_=ss(A+B*F_,B*R_,F_,D*R_);
    figure(2)
    hold on
    step(Gg_)
    figure(3)
    hold on
    step(Gu_)
end
figure(2)
title('Sprungantwort mit q2 als Parameter')
figure(3)
title('Stellgröße mit q2 als Parameter')

Q(2,2)=q20;
figure(4)
clf
figure(5)
clf
for q1=[0.1 0.25 0.5 1 2 5],
    Q(1,1)=q1;
    F_=-lqr(A,B,Q,R);
    R_=1/dcgain(A+B*F_,B,C,D);
    Gg_=ss(A+B*F_,B*R_,C,D*R_);
    Gu_=ss(A+B*F_,B*R_,F_,D*R_);
    figure(4)
    hold on
    step(Gg_)
    figure(5)
    hold on
    step(Gu_)
end
figure(4)
title('Sprungantwort mit q1 als Parameter')
figure(5)
title('Stellgröße mit q1 als Parameter')

