% Skript beobachter_1.m, in dem ein Beobachter
% untersucht wird. Arbeitet mit Modell beobachter1.mdl
clear;
% -------- Parameter des Systems
m1 = 10; m2 = 10; m3 = 5;
k1 = 10;   k2 = 1;    k3 = 10;
c1 = 0.5;  c2 = 0.5; 
n = 6;     % Ordnung des Systems
A = [0 1 0 0 0 0;
    -(k1+k3)/m1 -c1/m1 k1/m1 c1/m1 k3/m1 0;
    0 0 0 1 0 0;
    k1/m2 c1/m2 -(k1+k2)/m2 -(c1+c2)/m2 k2/m2 c2/m2;
    0 0 0 0 0 1;
    k3/m3 0 k2/m3 c2/m3 -(k2+k3)/m3 -c2/m3];
B = [0 1 0 0 0 0]';
C = [0 0 0 0 1 0];
D = 0;

figure(1);    clf;
my_sys = ss(A, B, C, D);    Tf = 50;
impulse(my_sys, Tf);
title('Impulsantwort des Prozesses');  
xlabel('s'); grid on;

% ------- Beobachter Entwicklung
p = [-1.25, -1.5, -2, -2.5, -1+j*0.8,...
    -1-j*0.8];                    % Gewählte Pole
Ke = (place(A',C',p))';
x0 = [30,-20,0,0,0,0];

Tfinal = 50;      dt = 0.01;
my_options = simset('Solver','ode45','MaxStep',dt);
sim('beobachter1', [0, Tfinal], my_options);

xt = x.signals.values;
xg = x1.signals.values;
t = x.time;

figure(2);    clf;
subplot(211), plot(t, xt);
title('Korrekte Zustandsvariablen mit Anfangsbedingungen x0');
xlabel('s');    grid on;
legend('x1','v1','x2','v2','x3','v3')

subplot(212), plot(t, xg);
title('Geschätzte Zustandsvariablen x0 = 0');
xlabel('s');    grid on;
legend('x1g','v1g','x2g','v2g','x3g','v3g')

figure(3);    clf;
subplot(211), plot(t, [xt(:,5), xg(:,5)]);
title('Korrekte und geschätzte Zustandsvariable x3 (Ausgang)')
xlabel('Zeit in s');     grid on;

subplot(212), plot(t, [xt(:,1), xg(:,1)]);
title('Korrekte und geschätzte Zustandsvariable x1 ')
xlabel('Zeit in s');     grid on;

