% Skript diff_filter_1.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_filter1.mdl
clear;

% ------- Parameter des Filters
fsig = 1;        % Frequenz des Eingangssignals in Hz
wsig = 2*pi*fsig;   % rad/s
Tsig = 1/fsig;      % Periode
ampl = 1;           % Amplitude des Signals
% ampl = 0;           % Für den Einfluss des Messrauschens

deltaT = Tsig/4;    % Intervalle für das dreieckiges Signal
tdreieck = [0, deltaT, 2*deltaT, 3*deltaT, 4*deltaT];
dreieck = [0, ampl, 0, -ampl, 0];   % Werte des dreieck. Signals
varianz = 1e-6;     % Varianz des Messrauschens der Lage 
noise = sqrt(varianz);  % Verstärkung des Messrauschens
%noise = 0;          % Ohne Messrauschen
%k = 100;     gama = 0.1e4;  % Parameter des Filters für 0.1 Hz
k = 250;     gama = 1e4;  % Parameter des Filters für 1 Hz
%k = 500;     gama = 10e4;  % Parameter des Filters für 10 Hz

% ------ Aufruf der Simulation
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_filter1',ts,my_options);
vy  = y.signals.values(:,1);   % Geschätzte Geschwindigkeit der Masse
yy = y.signals.values(:,2);    % Lage der Masse (Eingangssignal)
eT = y.signals.values(:,3);    % yT - y
t = y.time;                    % Simulationszeit
dydt = y1.signals.values;      % Direkte Ableitung des Eingangssignals

figure(1);   clf;
a(1) = subplot(311),  plot(t, yy);
title('Lage der Masse (Eingangssignal)');
xlabel('Zeit in s');    grid on;
a(2) = subplot(312),  plot(t, vy);
title('Mit Differential-Filter geschätzte Geschwindigkeit ');
xlabel('Zeit in s');    grid on;
a(3) = subplot(313),  plot(t, dydt);
title('Durch Ableitung geschätzte Geschwindigkeit');
xlabel('Zeit in s');    grid on;
linkaxes(a,'x');

% ------- Signale ohne Einschwingen und Spektren
ntrans = round(nt/5);       % Einschwingteil
dez = 20;                   % Dezimierung und Entfernung des
yy = yy(ntrans:dez:end);    % Einschwingens
vy = vy(ntrans:dez:end);          nfft = length(vy);
vdydt = dydt(ntrans:dez:end);
nfft = 512;
[P_diff, f] = pwelch(vy, hamming(nfft),50,nfft,1/(dez*dt));    
[P_dydt, f] = pwelch(vdydt, hamming(nfft),50,nfft,1/(dez*dt)); 

P_diff_t = sum(P_diff)/(dez*dt*nfft),  % Leistung der Signale
P_dydt_t = sum(P_dydt)/(dez*dt*nfft),  % aus der spektralen 
                                       % Leistungsdichte
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');



