% Skript kalman_3.m, in dem ein Kalman-Filter für einen Prozess
% mit Störung am Ausgang und Regelung mit I-Anteil

clear;
% -------- Modell des Prozesses
H11 = tf(100, [1, 5, 100]);
H12 = tf(1, [2, 1]);
H1 = H11*H12;
sys1 = ss(H1);
A1 = sys1.a;    B1 = sys1.b;
C1 = sys1.c;    D1 = sys1.d;

% -------- Modell der Störung am Ausgang
H21 = tf(10, [1, 5, 10]);
H22 = tf(1, [3, 1]);
H2 = H21*H22;
sys2 = ss(H2);
A2 = sys2.a;    B2 = sys2.b;
C2 = sys2.c;    D2 = sys2.d;

% -------- Prozess- und Messrauschen
vnd = 0.5;
vny = 0.01;

% -------- Modell des gesamten Systems
An = [A1, zeros(3,3);zeros(3,3),A2];
Bn = [B1 zeros(3,1);
    zeros(3,1), B2];

Cn = [C1, C2];
Dn = [0,0];

sys_g = ss(An, Bn, Cn, Dn);

% -------- Kalman Filter
[Kest, L, P] = kalman(sys_g, vnd, vny),
eigenwerte = eig(An - L*Cn),

% -------- Aufruf der Simulation
Tfinal = 100;      dt = 0.01;
my_options = simset('Solver','ode45', 'MaxStep', dt);
sim('kalman2',[0,Tfinal],my_options);
t = y1.time;
yr = y1.signals.values(:,1);
yg = y1.signals.values(:,2);

xg1 = y.signals.values(:,1:3);
xg2 = y.signals.values(:,4:6);

xk1 = y3.signals.values(:,1:3);
xk2 = y2.signals.values(:,1:3);


figure(1);    clf;
plot(t, yr, t, yg);
title('Gemessener und geschätzter Ausgang');
xlabel('Zeit in s');   grid on;

figure(2);    clf;
subplot(211), plot(t, xg1);
title('Geschätzte Zustandsvariablen des Prozesses');
xlabel('Zeit in s');   grid on;
subplot(212), plot(t, xg2);
title('Geschätzte Zustandsvariablen des Störungsmodells');
xlabel('Zeit in s');   grid on;

figure(3);    clf;
subplot(211), plot(t, xk1);
title('Korrekte Zustandsvariablen des Prozesses');
xlabel('Zeit in s');   grid on;
subplot(212), plot(t, xk2);
title('Korrekte Zustandsvariablen des Störungsmodells');
xlabel('Zeit in s');   grid on;

% -------- Regler mit I-Anteil
% LQR-Regelung für den Prozess
An = [A1, zeros(3,1); -C1, 0];    % Erweitertes Modell
Bn = [B1;0];
Q = eye(4,4)*100;      R = 0.1;    % Kosten der Zielfunktion
Kn = lqr(An, Bn, Q, R);          % LQR Rückführungsmatrix
pole = eig(An - Bn*Kn),          % Eigenwerte des erweiterten Systems
Kr = Kn(1:3),    ki = -Kn(4);

% -------- Aufruf der Simulation
Tfinal = 100;      dt = 0.01;
my_options = simset('Solver','ode45', 'MaxStep', dt);
sim('kalman3',[0,Tfinal],my_options);
t = y1.time;       nt = length(t);
yr = y1.signals.values(:,1);
yg = y1.signals.values(:,2);

xg1 = y.signals.values(:,1:3);
xg2 = y.signals.values(:,4:6);

xk1 = y3.signals.values(:,1:3);
xk2 = y2.signals.values(:,1:3);

figure(4);    clf;
plot(t, yr, t, yg);
title('Gemessener und geschätzter Ausgang');
xlabel('Zeit in s');   grid on;

figure(5);
plot(t, xg2(:,3), t, xk2(:,3));
title('Geschätzte und korrekte Störung');
xlabel('Zeit in s');   grid on;




std(yr(1000:end)),









