% Skript kalman_pegel_1.m, in dem ein zeitdiskretes Kalman-Filter
% zur Schätzung eines Pegels untersucht wird.
% Arbeitet mit Modell kalman_pegel1.mdl
clear;
% ------- Parameter des Systems
kp = 0.5
% ------- Zustandsmodell
Ts = 0.1;
A1 = 0;
B1 = [kp,1]; 
C1 = 1;  D1 = [0,0];
my_sys1 = ss(A1, B1, C1, D1);   
my_sys11 = c2d(my_sys1,Ts,'zoh');
% Zustandsmodell des Systems
% das die idealen Zustandsvariablen liefert als Referenzmodell
% ------ Kalman-Filter
vnd = 0.01;  vny = 1;  % Varianzen der Prozess-Störung
% und Messrauschen
Qn = vnd;    Rn = vny; % Kovarianzen
[Kest, L, P] = kalman(my_sys11, Qn, Rn,'current');
% -------- Aufruf der Simulation
Tfinal = 100;      
my_options=simset('Solver','FixedStepDiscrete','FixedStep',Ts);
sim('kalman_pegel1',[0,Tfinal],my_options);
t = y.time;     nt = length(t);       % Simulationszeit
ym = y.signals.values(:,1);% Ausgang y(t) mit Messrauschen 
yg = y.signals.values(:,2);% Geschätzter Ausgang
xg = y.signals.values(:,3);% Geschätzte Zustandsvariable
yk = y.signals.values(:,4);% Korrekte Zustandsvariable

figure(1);   clf;
subplot(211), plot(t, [ym,yg,yk]);
title(['Ausgang mit Messrauschen, geschätzter Ausgang',...
    ' und korrekter Ausgang (Lage Masse)'])
xlabel('s');   grid on;
nt = length(t);   nd = 1:fix(nt/4);
subplot(212), plot(t(nd), [ym(nd),yg(nd),yk(nd)]);
title(['Ausgang mit Messrauschen, geschätzter Ausgang',...
    ' und korrekter Ausgang (Ausschnitt)'])
xlabel('s');   grid on;

