clc; clear all
%% Przebiegi czasowe dostarczone do badan
% wczytanie dostarczonych sygnalow
load('TRAC1(2).MAT');
load('TRAC2(3).MAT');
load('TRAC3(2).MAT');
% charakterystyki czasowe dostarczonych sygnalow
figure(1)
subplot(2,1,1)
plot(c1x,c1);
grid on;
title('Charakterystyka czasowa sygnalu wejściowego');
xlabel('Czas [s]');
ylabel('Wartosc sygnalu [m]');
subplot(2,1,2)
plot(c2x,c2);
grid on;
title('Charakterystyka czasowa sygnalu wyjściowego');
xlabel('Czas [s]');
ylabel('Wartosc sygnalu [m]');
% charakterysyka widomowa
figure(2)
plot(o2i1x, o2i1);
grid on;
title('Charakterystyka widmowa sygnalu wyjściowego');
xlabel('Częstotliwość [Hz]');
ylabel('Amplituda [m]')

%% Eksperymentalny wykres Bodego
o2i1_eksp=20*log10(abs(o2i1));
figure(3)
semilogx(o2i1x, o2i1_eksp);
grid on;
title('Eksperymentalna charakterystyka Bodego');
xlabel('Częstotliwość [Hz]');
ylabel('Amplituda [dB]')

%% Model estymowany
load ('arx68581.mat');
%utworzenie licznika i mianownika modelu
model=tf([arx68581.A], [arx68581.B]);
%macierze A,B,C,D modelu
[A,B,C,D]=tf2ss(arx68581.B, arx68581.A);
sys=ss(A,B,C,D,arx68581.Ts);
[mag_sys,phase_sys,wout_sys] = bode(sys);
figure(4)
semilogx(o2i1x, o2i1_eksp);
hold on;
semilogx(wout_sys/6.28,20*log10(abs(mag_sys(1,:))));
grid on;
title('Porównanie charakterystyk częstotliwościowych');
legend('Eksperyment', 'Estymacja');
xlabel('Częstotliwość [Hz]');
ylabel('Amplituda [dB]')

%% Redukcja w state space z balance method
%wyznaczenie nowego modelu obiektu 'sysb' oraz wektora wspolczynnikow
%wagowych zmiennych stanu 'k'
[sysb, g]=balreal(sys);
elim= (g<0.0001); %odrzucenie zmiennych stanu poza pasmem 1-400Hz
sys_red=modred(sysb,elim); %model zredukowany I
[mag_sys_red,phase_sys_red,wout_sys_red] = bode(sys_red);
%charakterystyki czestotliwosciowe
L=fft(c2);
M=fft(c1);
G=L./M;
n=12;
N=2^12; %ilosc probek
dt=4/N;
f=inv(dt)*(0:N/2-1)/N; % czas -> czestotliwosc
f1=f';
L_G=20*log10(abs(G(1:2048,1)));
figure(5)
semilogx(f1,L_G);

semilogx(o2i1x,o2i1_eksp);
semilogx(wout_sys/6.28,20*log10(abs(mag_sys(1,:))));
%semilogx(wout_sys/6.28,20*log10(abs(mag_sys_red(1,:))));
grid on;
title('Porównanie charakterystyk częstotliwościowych');
legend('G', 'Eksperyment', 'Estymacja', 'Zredukowany');
xlabel('Częstotliwość [Hz]');
ylabel('Amplituda [dB]')

%% Dalsza redukcja - odrzucenie zer i biegunow powzyzej 325Hz

[z_red,p_red,k_red]=ss2zp(sys_red.A,sys_red.B,sys_red.C,sys_red.D);
omega_anty=abs(log(z_red)./sys_red.Ts);
omega_rez =abs(log(p_red)./sys_red.Ts);
z_macierz=[z_red,omega_anty];
p_macierz = [p_red, omega_rez];
z_macierz_sort=sortrows(z_macierz,2);
p_macierz_sort=sortrows(p_macierz,2);
zera=z_macierz_sort([1:62]);
zera=zera';
bieguny=p_macierz_sort([1:62]);
bieguny=bieguny';
model_zred=zpk(zera,bieguny,k_red*0.001^(0.2),c2x(2)-c2x(1));
[mag_model_zred,phase_model_zred,w_model_zred] = bode(model_zred);
figure(6)
semilogx(o2i1x,20*log10(abs(o2i1)));
hold on;
semilogx(w_model_zred/6.28,20*log10(abs(mag_model_zred(1,:))));
grid on;
title('Porównanie charakterystyk amplitudowych');
ylabel('Amplituda [dB]');
xlabel('Częstotliwość [Hz]');
legend('Ekperymentalny','Zredukowany');