% Skript kalman_d4.m, in dem ein zeitdiskretes Kalman-Filter
% zur Schätzung der Lage und Geschwindigkeit eines 
% Feder-Masse-Systems aus der Lage, die mit Messrauschen
% überlagert ist, untersucht wird.
clear;
% ------- Parameter des Systems
m = 2;       k = 10;      c = 0.5;
fanreg = 0.1; ampl = 2; % Frequenz und Amplitude der Anregung
randn('seed', 1378);
% ------- Zustandsmodell
A = [0,1;-k/m,-c/m];
B = [0;1/m]; 
C = [1,0];  D = [0];
my_sys = ss(A, B, C, D);   % Zustandsmodell des Systems
% mit einem Eingang
Ts = 0.1;
my_sys1 = c2d(my_sys, Ts, 'zoh'); % Zeitdiskretes Modells 
A1 = my_sys1.a;    B1 = my_sys1.b;
C1 = my_sys1.c;    D1 = my_sys1.d;
G1 = [B1,B1];      % Eingänge für die Prozess-Störung
vnd = 0.01;  vny = 0.01;  % Varianzen der Prozess-Störung
% und des Messrauschens
Qn = eye(2,2)*vnd;    Rn = vny; % Kovarianzen
nfinal = 1000;
% -------- Antwort des Systems mit Prozess- und Messrauschen
x = zeros(2,nfinal);
y = zeros(1,nfinal);
w = randn(2,nfinal)*sqrt(vnd);  % Prozess-Störung
v = randn(1,nfinal)*sqrt(vny);  % Messrauschen
x0 = [0;0];               % Anfangsbedingungen
x(:,1) = x0;
u = ampl*sin(2*pi*fanreg*(0:nfinal-1)*Ts);   % Anregung
for k = 1:nfinal-1
   x(:,k+1) = A1*x(:,k) + B1*u(k) + G1*w(:,k);
   y(k) = C1*x(:,k) + v(k);
end;

% ------ Kalman-Filter
% Prediction und Filtering
xk_ = zeros(2,nfinal);    % Vorausgesagte Zustandsvariablen
xk = xk_;                 % Geschätzte Zustandsvariablen
Pk_ = zeros(2,2,nfinal);     Pk = Pk_; % Vorausgesagte 
         % und geschätzte Kovarianzen des Fehlers
Lk = zeros(2,nfinal);   % Rückführungsmatrix
xk_(:,1) = x0;            % Anfangsbedingungen
Pk_(:,:,1) = eye(2,2);      
for k = 1:nfinal-1
   xk_(:,k+1) = A1*xk(:,k) + B1*u(k);
   Pk_(:,:,k+1) = A1*Pk(:,:,k)*A1' + G1*Qn*G1';
   xk(:,k+1) = xk_(:,k+1) + Lk(:,k+1)*(y(k+1) - C1*xk_(:,k+1));
   Pk(:,:,k+1) = (eye(2,2) - Lk(:,k+1)*C1)*Pk_(:,:,k+1);
   Lk(:,k+1) = Pk_(:,:,k+1)*C1'*inv(C1*Pk_(:,:,k+1)*C1' + Rn);
end;
yk = C1*xk;               % Geschätzter Ausgang
figure(1);       clf;
subplot(211), plot((0:nfinal-1)*Ts, x');
title('Zustandsvariblen mit Prozess-Störung');
xlabel('Zeit in s');    grid on;
subplot(212), plot((0:nfinal-1)*Ts, y);
title('Ausgang mit Messrauschen');
xlabel('Zeit in s');    grid on;

figure(2);       clf;
subplot(211), plot((0:nfinal-1)*Ts, xk');
title('Geschätzte Zustandsvariblen');
xlabel('Zeit in s');    grid on;
subplot(212), plot((0:nfinal-1)*Ts, yk);
title('Geschätzter Ausgang');
xlabel('Zeit in s');    grid on;






