% Skript kalman_12.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 wird. Die Zustandsvariablen werden 
% mit Kalman-Filter geschätzt. Arbeitet mit 
% Modell kalman12.mdl
clear;
% ------- Parameter des Systems
H1 = tf(200, [1, 5, 100]);
sys1 = ss(H1);
A1 = sys1.a;    B1 = sys1.b;
C1 = sys1.c;    D1 = sys1.d;
sys2 = sys1(:,[1,1,1]); % Ein Eingang für u und zwei für w
G = eye(2,2);
sys2.b(:,2:3) = G;
% Mit sys2 wird jetzt der Block State-Space parametriert
% ------ Kalman-Filter
vnd = 1e-4;     vny = 1e-6; % Spektrale Leistungsdichten und 
dtn = 0.01;      % Abtastperiode für die Rauschgeneratoren
Qn = G*vnd/dtn;  Rn = vny/dtn;   % Varianzen der Störung und des 
       % Messrauschens
%Qn = G*vnd/1000; Rn = vny/1000;   % Varianzen der Störung und des 
       % Messrauschens
[Kest, L, P] = kalman(sys2, Qn, Rn);
% ------ LQR-Regelung mit I-Anteil
An = [sys1.a, zeros(2,1); -sys1.c,0]; % Erweitertes Modell
Bn = [sys1.b;0];
Q = eye(3,3)*100;    R = 1;
Kn = lqr(An, Bn, Q, R);
Kr = Kn(1:2);    ki = -Kn(3);

% -------- Aufruf der Simulation
Tfinal = 20;      dt = 0.01;
my_options = simset('Solver','ode45', 'MaxStep', dt);
sim('kalman12',[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;

