% Skript kalman_d1.m, in dem eine LQG-Regelung 
% für einen kontinuierlichen Prozess der Form
% x' = Ax + Bu + Gw
% y =  Cx + Du + v   
% E{ww´} = Q;   Ev^2 = R
% mit zeitdiskretem Kalman-Filter untersucht wird. 
% Arbeitet mit Modell kalmand1.mdl
clear;
% -------- Prozess und Zustandsmodell
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 zwei für w
G = eye(3,3);
sys2.b(:,2:4) = G;     % G = eye(3,3);
% Mit sys2 wird jetzt der State-Space Prozess parametriert
% ------ Zeitdiskretes Kalman-Filter
vnd = 1e-4;     vny = 1e-6; % Spektrale Leistungsdichten und 
dtn = 0.01;      % Abtastperiode für die Rauschgeneratoren
Qn = eye(3,3)*vnd/dtn;      Rn = vny/dtn;
Ts = 0.05;
[Kest,L,P] = kalmd(sys2, Qn, Rn, Ts);    % 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);
dtn = 0.01;       % Abtastperiode für die Rauschsignale
% -------- Aufruf der Simulation
Tfinal = 50;      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('kalmand1',[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
td = y1.time;

figure(1);    clf;
subplot(211), plot(t, yr)
hold on;   stairs(t, yg);
title(['Gemessener und geschätzter Ausgang ( vnd = ',num2str(vnd),...
    ' ; vny = ', num2str(vny),' )']);
xlabel('Zeit in s');   grid on;   hold off;
nd = fix(nt/5);
n = nd:2*nd; 
subplot(212), plot(t(n), yr(n))
hold on;    stairs(t(n), yg(n));
title('Gemessener und geschätzter Ausgang (Ausschnitt)');
xlabel('Zeit in s');   grid on;   hold off;
axis tight;       
figure(2);    clf;
stairs(td, xg);
title('Geschätzte Zustandsvariablen des Prozesses');
xlabel('Zeit in s');   grid on;

