1clc;
2clear;
3close all;
4
5pkg load signal;
6pkg load control;
7
8faustDSPtarget  = fullfile('..', '..', 'Faust', 'modelFilters', 'model_IIR.dsp');
9permission      = 'w';
10precision       = '%.18f';
11antialFIRTaps   = 16;
12windowFcn       = @chebwin;
13iirNumOrder     = 4;
14iirDenOrder     = 4;
15
16finalFrequency  = csvread('/tmp/fFinalFrequency.csv');
17sampleRate      = csvread('/tmp/fSampleRate.csv');
18alpha           = csvread('/tmp/fAlpha.csv');
19g               = csvread('/tmp/mKernelsTaps.csv');
20
21% Calcualte the best possible frequency response targets for implementation.
22gCenter         = floor(size(g, 2) / 2) + 1;
23%g               = circshift(g, gCenter, 2);
24G               = fft(g, [], 2);
25shift           = -gCenter; % - (antialFIRTaps - 1) / 2;
26wSize           = size(G, 2);
27nyq             = floor(wSize / 2) + 1;
28phFactor        = ones(1, size(G, 2));
29phFactor(1:nyq) = exp(-1i * 2 * pi * ((1:nyq) - 1) * shift / wSize);
30phFactor(mod(wSize + 1 - (1:nyq), wSize) + 1) = conj(phFactor(1:nyq));
31G               = bsxfun(@times, G, phFactor);
32
33% Useful stuff:
34nyquistSample   = floor(size(G, 2) / 2.0) + 1;
35frequency       = (0:(nyquistSample - 1)) * sampleRate  / (2.0 * nyquistSample);
36frequencyPI     = (0:(nyquistSample - 1)) * pi  / nyquistSample;
37frequencyStep   = sampleRate / (2.0 * nyquistSample);
38
39% Normalize gains with respect the linear order.
40% G               = G / max(abs(G(1, 1:nyquistSample)));
41
42% Get all normalized gains:
43gains           = max(abs(G(:, 1:nyquistSample)), [], 2);
44
45% This is the minimum adjustment gain applicable to the model:
46minGain         = exp(log(max(gains(2:end))) / size(G, 1));
47
48% Calculate IIR coefficiens:
49B               = cell(size(G, 1), 1);
50A               = cell(size(G, 1), 1);
51sosData         = cell(size(G, 1), 1);
52sosGains        = zeros(size(G, 1), 1); % For Plot
53antialFIRs      = cell(size(G, 1), 1);
54
55% Actual IIR responses:
56H_Kernels       = zeros(size(G, 1), nyquistSample);
57H_antial        = zeros(size(G, 1), nyquistSample);
58
59for n = 1:size(G, 1)
60
61    [B{n, 1}, A{n, 1}] = invfreqz(                          ...
62                      G(n, 1:nyquistSample), .../ gains(n),     ...
63                      frequencyPI,                          ...
64                      iirNumOrder,                          ...
65                      iirDenOrder                           ...
66                      );
67
68    sosItem = struct();
69
70    [sosItem.SOS, sosItem.G] = tf2sos(B{n, 1}, A{n, 1});
71
72    sosGains(n) = sosItem.G;
73
74    sosData{n, 1} = sosItem;
75
76    antialFIRs{n, 1} = fir1(                                              ...
77                       antialFIRTaps - 1,                           ...
78                       (2.0 * finalFrequency) / (n * sampleRate),   ...
79                       'lowpass',                                   ...
80                       windowFcn(antialFIRTaps)                     ...
81                       );
82
83    % sosItem.G       = sosItem.G * gains(n);
84    H_Kernels(n, :) = freqz(B{n, 1}, A{n, 1}, nyquistSample); %gains(n) * freqz(B{n, 1}, A{n, 1}, nyquistSample);
85    H_antial(n, :)  = freqz(antialFIRs{n, 1}, 1, nyquistSample);
86
87endfor
88
89groupDelays = -diff(unwrap(angle(H_Kernels), [], 2), 1, 2) / (2 * pi * frequencyStep);
90
91% Plot the results.
92maxPlotLevel = 10.0 * ceil(20.0 * log10(max(abs(H_Kernels(:)))) / 10);
93minPlotLevel = -120;
94
95figure;
96subplot(2, 1, 1);
97semilogx(frequency(2:end), transpose(20.0 * log10(abs(G(:, 2:nyquistSample)))));
98xlim([frequency(2) frequency(end)]);
99ylim([minPlotLevel maxPlotLevel]);
100grid on;
101xlabel('Frequency [Hz]');
102ylabel('Measured Kernels [dB]');
103subplot(2, 1, 2);
104semilogx(frequency(2:end), transpose(angle(G(:, 2:nyquistSample))));
105xlim([frequency(2) frequency(end)]);
106grid on;
107xlabel('Frequency [Hz]');
108ylabel('Measured Kernels [rad]');
109
110figure;
111subplot(2, 1, 1);
112semilogx(frequency(2:end), transpose(20.0 * log10(abs(H_Kernels(:, 2:end)))));
113xlim([frequency(2) frequency(end)]);
114ylim([minPlotLevel maxPlotLevel]);
115grid on;
116xlabel('Frequency [Hz]');
117ylabel('Kernels IIR [dB]');
118subplot(2, 1, 2);
119semilogx(frequency(2:end), transpose(angle(H_Kernels(:, 2:end))));
120xlim([frequency(2) frequency(end)]);
121grid on;
122xlabel('Frequency [Hz]');
123ylabel('Kernels IIR [rad]');
124
125figure;
126semilogx(frequency(2:(end - 1)), transpose(groupDelays(:, 2:end)));
127xlim([frequency(2) frequency(end)]);
128grid on;
129xlabel('Frequency [Hz]');
130ylabel('Kernels IIR Group Delay [s]');
131
132figure;
133subplot(2, 1, 1);
134semilogx(frequency(2:end), transpose(20.0 * log10(abs(H_antial(:, 2:end)))));
135xlim([frequency(2) frequency(end)]);
136ylim([minPlotLevel maxPlotLevel]);
137grid on;
138xlabel('Frequency [Hz]');
139ylabel('Antialising FIR [dB]');
140subplot(2, 1, 2);
141semilogx(frequency(2:end), transpose(unwrap(angle(H_antial(:, 2:end)), [], 2)));
142xlim([frequency(2) frequency(end)]);
143grid on;
144xlabel('Frequency [Hz]');
145ylabel('Antialising FIR [rad]');
146
147figure;
148stem(sosGains);
149grid on;
150xlabel('Order');
151ylabel('SOS Gain');
152