% Skript kalman_13.m, in dem eine LQG-Regelung 
% für einen Prozess der Form
% x' = Ax + Bu + Gw
% y =  Cx + Du + v   
% E{ww´} = xQx´;   Ev^2 = R
% untersucht. Arbeitet mit Modell kalman13.mdl

clear;
H1 = tf(200, [1, 5, 100]);
H2 = tf(1,[2 -1]);
sys1 = ss(H1*H2);
A1 = sys1.a;    B1 = sys1.b;
C1 = sys1.c;    D1 = sys1.d;
sys2 = sys1(:,[1,1,1,1]);     % Ein Eingang für u und drei für w
sys2.b(:,2:4) = eye(3,3);     % G = eyey(3,3);
% Mit sys2 wird jetzt der Block State-Space parametriert
% ------ Kalman-Filter
vnd = 0.01;            vny = 0.0001;
Qn = eye(3,3)*vnd;      Rn = vny;
[Kest,L,P] = kalman(sys2, Qn, Rn);    % Kalman-Filter
% ------ LQR-Regelung mit I-Anteil
An = [sys1.a, zeros(3,1); -sys1.c,0]; % Erweitertes Modell
Bn = [sys1.b;0];
Q = eye(4,4)*100;    R = 1;
%Q = eye(4,4)*1;    R = 10;
Kn = lqr(An, Bn, Q, R);
Kr = Kn(1:3);    ki = -Kn(4);

% -------- Aufruf der Simulation
Tfinal = 20;      dt = 0.01;
x0 = zeros(1,3);  step = 1;     % Sprungantwort
%x0 = [-1 -2 1];  step = 0;      % Homogene Antwort
my_options = simset('Solver','ode45', 'MaxStep', dt);
sim('kalman13',[0,Tfinal],my_options);
t = y.time;                  % Simulationszeit
nt = length(t);
yr = y.signals.values(:,1);  % Korrekter Ausgang y(t)
yg = y.signals.values(:,2);  % Geschätzter Ausgang yg(t) 
xg= y1.signals.values(:,:); % Geschätzte Zustandsvariablen des Prozesses
figure(1);    clf;
subplot(211), plot(t, yr, t, yg);
title(['Gemessener und geschätzter Ausgang ( vnd = ',num2str(vnd),...
    ' ; vny = ', num2str(vny),' )']);
xlabel('Zeit in s');   grid on;
nd = fix(nt/4);
n = nd:2*nd; 
subplot(212), plot(t(n), yr(n), t(n), yg(n));
title('Gemessener und geschätzter Ausgang (Ausschnitt)');
xlabel('Zeit in s');   grid on;
axis tight;
figure(2);    clf;
plot(t, xg);
title('Geschätzte Zustandsvariablen des Prozesses');
xlabel('Zeit in s');   grid on;

