% Skript LQG_1.m in dem eine LQG-Regelung nach
% http://www-rohan.sdsu.edu/doc/matlab/toolbox/control/
% getstart/desig32a.html
% Arbeitet mit Modell LQG1.mdl
clear;
sys = ss(tf(100,[1 2 100])); % State-space plant model

% Design LQ-optimal gain K
K = lqry(sys,10,1) % u = -Kx minimizes J(u)

% Separate control input u and disturbance input d
P = sys(:,[1 1]);
% input [u;d], output y

% Design Kalman state estimator Kest.
Kest = kalman(P,1,0.01),

% Form LQG regulator = LQ gain + Kalman filter.
F = lqgreg(Kest,K),


% Close loop
clsys = feedback(sys,F,+1)    % Note positive feedback.

% Create the lowpass filter and add it in series with clsys.
s = tf('s');
lpf= 10/(s+10) ;
clsys_fin = 1.6*lpf*clsys;

figure(1);
% Open- vs. closed-loop impulse responses
step(sys,'r--',clsys_fin,'b-')





%####################################################
% ------ Ohne Störung w und mit Modell
sys1 = ss(tf(100,[1 2 100]));

% Design LQ-optimal gain K
% Gewichtung der Zustandsabweichung und der Steuerung
Q = 50;  R = 10
[K1,S1] = lqr(sys1,eye(2,2)*Q,R), % u = -Kx minimizes J(u)

% Design Kalman state estimator Kest.
% Varianz des Rauschens am Eingang und des Messrauschens
vnd = 0.1;  vny = 0.0001
Kest1 = kalman(sys1,vnd,vny),

% ------ Aufruf der Simulation
Tfinal = 20;     dt = 0.01;
t = [0,Tfinal];
myoptions = simset('Solver','ode45','MaxStep', dt);
sim('LQG1', t, myoptions);
% Ausgänge
yr = y.signals.values(:,1);   % Ausgang y
yg = y.signals.values(:,2);   % Geschätzter Ausgang
t = y.time;
% Zustandsvariablen
x1g = y1.signals.values(:,1);   % Erste geschätzte Zustandsvariable
x2g = y1.signals.values(:,2);   % Zweite geschätzte Zustandsvariable
x1r = y1.signals.values(:,3);   % Erste  Zustandsvariable
x2r = y1.signals.values(:,4);   % Zweite Zustandsvariable

figure(2);   clf;
plot(t, yr, t, yg);
title('Ausgang mit Messrauschen und geschätzter Ausgang');    
grid on;    xlabel('Zeit in s');
legend('y1r','y1g');

figure(3);   clf;
plot(t, [x1g, x2g], t, [x1r, x2r]);
title('Geschätzte und korrekte Zustandsvariablen');    
grid on;    xlabel('Zeit in s');
legend('x1g', 'x2g', 'x1r', 'x2r');

figure(4);   clf;
plot(t, sys1.c*[x1r, x2r]', t, sys1.c*[x1g, x2g]' )
title('Ausgang ohne Messrauschen und geschätzter Ausgang');
grid on;    xlabel('Zeit in s');
legend('y1r','y1g');













