% Skript diff_filter_2.m, in dem ein Filter zur Bildung
% der Ableitung eines Signals untersucht wird. 
% Die Idee stammt aus ''Adaptive Compensation of Friction 
% Forces with Differential Filter'' Kouichi Mitsunaga, 
% Takami Matsuo. International Journal of Computer, 
% Communications & Control % Vol III (2008), No. 1, pp 80-89
% Arbeitet mit Modell diff_filter2.mdl
clear;

% ------- Parameter des Filters
f_pass = 1;            % Bandbreite des Filters
w_pass = 2*pi*f_pass;  % für die Erzeugung der Geschwindigkeit
noise_g = 1;   % Varianz der unabhängigen Sequenz für die Geschwindigkeit

varianz = 1e-8;     % Varianz des Messrauschens der Lage 
noise_m = sqrt(varianz);  % Verstärkung des Messrauschens
%noise_m = 0;          % Ohne Messrauschen
%k = 100;     gama = 0.1e4;  % Parameter für 0.1 Hz Bandbreite
k = 250;     gama = 1e4;  % Parameter für 1 Hz Bandbreite
%k = 500;     gama = 10e4;  % Parameter für 10 Hz Bandbreite

% ------ Aufruf der Simulation
dt1 = 0.01;   % Schrittweite für den Rauschgenerator (Random Number)
Tfinal = 100;
dt = 0.001;     ts = 0:dt:Tfinal-dt;
nt = length(ts);
%my_options = simset('solver','ode4','FixedStep',dt);
my_options = simset('solver','ode45','MaxStep',dt);

sim('diff_filter2',ts,my_options);
vy  = y.signals.values(:,1);    % Korrekte Geschwindigkeit der Masse
vy_diff = y.signals.values(:,2);% Geschätzte Geschwindigkeit der Masse
vy_dydt = y.signals.values(:,3);% Durch Ableitung geschätzte Geschwindigkeit
eT = y.signals.values(:,3);     % yT - y
t = y.time;                     % Simulationszeit

figure(1);   clf;
a(1) = subplot(211),  plot(t, vy, t, vy_diff);
title('Korrekte und mit Beobachter geschätzte Geschwindigkeit');
xlabel('Zeit in s');    grid on;
a(2) = subplot(212),  plot(t, vy_dydt);
title('Durch Ableitung geschätzte Geschwindigkeit');
xlabel('Zeit in s');    grid on;
linkaxes(a,'x');

% ------- Signale ohne Einschwingen
ntrans = round(nt/5);
if dt == 0.001
    dez = 20;
else dez = 10;
end;
vy_diff = vy_diff(ntrans:dez:end);
vy_dydt = vy_dydt(ntrans:dez:end);          nfft = length(vy);
nfft = 512;
[P_diff, f] = pwelch(vy_diff, hamming(nfft),50,nfft,1/(dez*dt));    
[P_dydt, f] = pwelch(vy_dydt, hamming(nfft),50,nfft,1/(dez*dt)); 

figure(2);    clf;
plot(f, 10*log10(P_diff), f, 10*log10(P_dydt));
title('Spektrale Leistungsdichte der geschätzten Geschwindigkeiten');
xlabel('Hz');    grid on;     legend('Mit Diff-Filter', 'Ableitung');
% -------- Satz von Parseval
P_diff_f = sum(P_diff)/(dez*dt*nfft),
P_diff_t = std(vy_diff)^2,

P_dydt_f = sum(P_dydt)/(dez*dt*nfft),
P_dydt_t = std(vy_dydt)^2,


