% Skript kalman_10.m, in dem ein Kalman Filter als Zustandsmodell
% untersucht wird. Arbeitet mit dem Modell kalman10.mdl
clear;
% ------- Parameter des Systems
b = 100;            % Koeffizienten der Übertragungsfunktion
a = [1, 5, 100];
% ------- Zustandsmodell
my_sys = ss(tf(b, a));
A = my_sys.a;   B = my_sys.b;
C = my_sys.c;   D = my_sys.d;
G = eye(2,2);
my_sys1 = my_sys(:,[1 1 1]);
my_sys1.b(1:2,2:3) = G;
% ------- Kalman-Filter
vnd = 1e-4;     vny = 1e-6;
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(my_sys1, Qn, Rn);
% ------ Aufruf der Simulation
Tfinal = 20;     dt = 0.01;
t = [0,Tfinal];
myoptions = simset('Solver','ode45','MaxStep', dt);
sim('kalman10', t, myoptions);
% Ausgänge
t = y.time;
% Zustandsvariablen
yk  = y.signals.values(:,1);   % Ausgang mit Messrauschen
yg  = y.signals.values(:,2);   % Geschätzter Ausgang
x1  = y1.signals.values(:,1);  % Erste  Zustandsvariable
x2  = y1.signals.values(:,2);  % Zweite Zustandsvariable
x1g = y2.signals.values(:,1);  % Erste  geschätzte Zustandsvariable
x2g = y2.signals.values(:,2);  % Zweite geschätzte Zustandsvariable

figure(1);   clf;
plot(t, yk, t, yg);
title('Ausgang mit Messrauschen und geschätzter Ausgang');    
grid on;    xlabel('Zeit in s');
legend('yk','yg');

figure(2);   clf;
plot(t, [x1g, x2g], t, [x1, x2]);
title('Geschätzte und korrekte Zustandsvariablen');    
grid on;    xlabel('Zeit in s');
legend('x1g', 'x2g', 'x1', 'x2');


