% Skript kalman_d3.m, in dem ein zeitdiskretes Kalman-Filter
% zur Schätzung der Lage und Geschwindigkeit eines 
% Feder-Masse-Systems aus der Lage mit Messrauschen
% untersucht wird. Arbeitet mit Modell kalman3d.mdl
clear;
% ------- Parameter des Systems
m = 2;       k = 10;      c = 0.5;
fanreg = 0.1; ampl = 2; % Frequenz und Amplitude der Anregung
% ------- Zustandsmodell
A1 = [0,1;-k/m,-c/m];
B1 = [0;1/m]; 
C1 = [1,0];  D1 = [0];
my_sys1 = ss(A1, B1, C1, D1);   % Zustandsmodell des Systems
% mit einem Eingang
my_sys2 = my_sys1(:,[1,1,1]); % mit drei Eingängen
Ts = 0.1;
my_sys3 = c2d(my_sys2, Ts, 'zoh'); % Zeitdiskretes Modells 
% ------ Zeitdiskretes ideale Modell mit den
% Zustandsvariablen als Ausgänge (ohne Störung und ohne 
% Messrauschen)
my_sys11 = ss(my_sys3.a, my_sys3.b(:,1), eye(2,2), 0);
% ------ Kalman-Filter
vnd = 0.01;  vny = 0.01;  % Varianzen der Prozess-Störung
% und des Messrauschens
Qn = eye(2,2)*vnd;    Rn = vny; % Kovarianzen
%[Kest, L, P] = kalman(my_sys12, Qn, Rn,'current');
[Kest, L, P] = kalman(my_sys3, Qn, Rn,'delayed');
% -------- Aufruf der Simulation
Tfinal = 100;      
my_options=simset('Solver','FixedStepDiscrete','FixedStep',Ts);
sim('kalmand3',[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
xg1 = y.signals.values(:,3); % Erste geschätzte Zustandsvariable
xg2 = y.signals.values(:,4); % Zweite geschätzte Zustandsvariable
xk1 = y.signals.values(:,5); % Erste korrekte Zustandsvariable
xk2 = y.signals.values(:,6); % Zweite korrekte Zustandsvariable

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

figure(2),   clf;
subplot(211), plot(t, [xg1,xg2]);
title('Geschätzte Zustandsvariablen')
xlabel('s');   grid on;
subplot(212), plot(t, [xk1,xk2]); 
title('Korrekte Zustandsvariablen ohne Störungen und ohne Messrauschen')
xlabel('s');   grid on;

