% Skript kalman_11.m, in dem ein Zustandsmodell mit 
% zwei Eingängen für einen Block State-Space gebildet wird
% x' = Ax + Bu + Gw
% y =  Cx + Du + v   % Ein Ausgang
% E{ww´} = xQx´;   Ev^2 = R
% Arbeitet mit Modell kalman11.mdl
clear;
% ------- Parameter des Systems
H1 = tf(200, [1, 5, 100]);
sys1 = ss(H1);     % Zustandsmodell mit einem Eingang
A1 = sys1.a;    B1 = sys1.b;
C1 = sys1.c;    D1 = sys1.d;
sys2 = sys1(:,[1,1,1]);       % Ein Eingang für u und zwei für w
G = eye(2,2);
sys2.b(:,2:3) = G;     % G = eye(2,2);
% Mit sys2 wird jetzt der Block State-Space parametriert
% ------ Kalman-Filter
vnd = 1e-4;     vny = 1e-6; % Spektrale Leistungsdichten
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(sys2, Qn, Rn);
% -------- Aufruf der Simulation
Tfinal = 20;      dt = 0.01;
my_options = simset('Solver','ode45', 'MaxStep', dt);
sim('kalman11',[0,Tfinal],my_options);
t = y.time;                  % Simulationszeit
nt = length(t);
yr = y.signals.values(:,1);  % Korrekter Ausgang y(t)
yg = y.signals.values(:,2);  % Geschätzter Ausgang yg(t) 
xg= y1.signals.values(:,:); % Geschätzte Zustandsvariablen des Prozesses
figure(1);    clf;
subplot(211), plot(t, yr, t, yg);
title(['Gemessener und geschätzter Ausgang ( vnd = ',num2str(vnd),...
    ' ; vny = ', num2str(vny),' )']);
xlabel('Zeit in s');   grid on;
nd = fix(nt/4);
n = nd:2*nd; 
subplot(212), plot(t(n), yr(n), t(n), yg(n));
title('Gemessener und geschätzter Ausgang (Ausschnitt)');
xlabel('Zeit in s');   grid on;
axis tight;
figure(2);    clf;
plot(t, xg);
title('Geschätzte Zustandsvariablen des Prozesses');
xlabel('Zeit in s');   grid on;

