% Skript kalman_2.m, in dem ein Kalman-Filter für einen Prozess
% mit Störung am Ausgang

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, [10, 1]);
H2 = H21*H22;
sys2 = ss(H2);
A2 = sys2.a;    B2 = sys2.b;
C2 = sys2.c;    D2 = sys2.d;
% -------- Prozess- und Messrauschen
vnd = 0.01;
vny = 0.001;

% -------- 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);   % Gesamt Modell
% -------- 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;                  % Simulationszeit
yr = y1.signals.values(:,1);  % Korrekter Ausgang y(t)
yg = y1.signals.values(:,2);  % Geschätzter Ausgang yg(t) 
xg1= y.signals.values(:,1:3); % Geschätzte Zustandsvariablen des Prozesses
xg2= y.signals.values(:,4:6); % Geschätzte Zustandsvariablen der Störung
xk1= y3.signals.values(:,1:3);% Korrekte Zustandsvariablen des Prozesses
xk2= y2.signals.values(:,1:3);% Korrekte Zustandsvariablen der Störung
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, xk1);
title('Korrekte Zustandsvariablen des Prozesses');
xlabel('Zeit in s');   grid on;
figure(3);    clf;
alink(1) = subplot(211), plot(t, xk2(:,3));
title('Korrekte Zustandsvariablen des Störungsmodells');
xlabel('Zeit in s');   grid on;
alink(2) = subplot(212), plot(t, xg2(:,3));
title('Geschätzte Zustandsvariablen des Störungsmodells');
xlabel('Zeit in s');   grid on;
linkaxes(alink,['x']);









