% Skript LQG_10.m in dem eine LQG-Regelung nach
% http://www-rohan.sdsu.edu/doc/matlab/toolbox/control/
% getstart/desig32a.html
% Arbeitet mit Modell LQG10.mdl
%####################################################
% ----- System
sys1 = ss(tf(100,[1 2 100]));
% Design LQ-optimal gain K
% Gewichtung der Zustandsabweichung und der Steuerung
Q = 50;           R = 1;
[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,L,P] = kalman(sys1,vnd,vny);

% ------ Aufruf der Simulation
Tfinal = 20;     dt = 0.01;
t = [0,Tfinal];
myoptions = simset('Solver','ode45','MaxStep', dt);
%myoptions = simset('Solver','ode4','FixedStep', dt);
sim('LQG10', 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(1);   clf;
plot(t, yr, t, yg);
title('Ausgang mit Messrauschen und geschätzter Ausgang');    
grid on;    xlabel('Zeit in s');
legend('y1r','y1g');

figure(1);   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(3);   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');













