% Skript kalman_d2.m, in dem ein zeitdiskretes Kalman-Filter
% zur Schätzung der Lage und Geschwindigkeit eines 
% Feder-Masse-Systems aus der Beschleunigung untersucht wird.
% Arbeitet mit Modell kalman2d.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 = [-k/m,-c/m];  D1 = [1/m];
my_sys1 = ss(A1, B1, C1, D1);   % Zustandsmodell des Systems
% mit einem Eingang
Ts = 0.1;
% ------ Zustandsmodell mit drei Eingänge u(t), Gw(t)
my_sys2 = my_sys1(:, [1,1,1]);
my_sys2.b = [my_sys2.b(:,1),eye(2,2)];
% ------ Zeitdiskretisierung
my_sys11 = c2d(my_sys1, Ts, 'zoh'); % Zeitdiskretes Modells 
my_sys11 = ss(my_sys11.a,my_sys11.b,eye(2,2),[0;0]);
% das die idealen Zustandsvariablen liefert 
my_sys3 = c2d(my_sys2, Ts, 'zoh'); % Zeitdiskretes Modell
% für das Kalman-Filter
% ------ Kalman-Filter
vnd = 0.0001;  vny = 0.01;  % Varianzen der Prozess-Störung
% und Messrauschen
Qn = eye(2,2)*vnd;    Rn = vny; % Kovarianzen
[Kest, L, P] = kalman(my_sys3, Qn, Rn,'current');
% -------- Aufruf der Simulation
Tfinal = 100;      
my_options=simset('Solver','FixedStepDiscrete','FixedStep',Ts);
sim('kalmand2',[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',...
    ' (Beschleunigung)'])
xlabel('s');   grid on;
nt = length(t);     nd = 1:fix(nt/3);
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 Messrauschen')
xlabel('s');   grid on;

