1clc; clear; close all;
2
3dataTarget = fullfile(                             ...
4                            '..',                       ...
5                            '..',                       ...
6                            'Faust',                    ...
7                            'testBeds',                 ...
8                            'wienerHammerstein2.mat'     ...
9                            );
10
11% Select inner and outer windows and their parameters
12outerWindowFcn  = @hanning; % @(x) ones(x, 1);
13innerWindowFcn  = @novakWindow; % @(x, y, z) ones(x, 1);
14innerFadeIn     = Inf;
15innerFadeOut    = Inf;
16
17% Decide if scaling the coeff matrix columns (preScaling = true) or the end
18% result (the kernels).
19preScaling      = false;
20
21% Use  this threshold to check if higher order responses have high enough
22% Magnitude [dB]
23checkThreshold  = 100.0;
24
25hcpp    = csvread('/tmp/cResult.csv');
26Hcpp    = csvread('/tmp/mHigherRe.csv') + 1i * csvread('/tmp/mHigherIm.csv');
27Gcpp    = csvread('/tmp/mKernelsRe.csv') + 1i * csvread('/tmp/mKernelsIm.csv');
28fs      = csvread('/tmp/fSampleRate.csv');
29gamma   = csvread('/tmp/gamma.csv');
30order   = csvread('/tmp/fOrder.csv');
31wSize   = 2^csvread('/tmp/fWindowOrder.csv');
32offset  = csvread('/tmp/fOffset.csv');
33alpha   = csvread('/tmp/fAlpha.csv');
34fstart  = csvread('/tmp/fInitialFrequency.csv');
35fend    = csvread('/tmp/fFinalFrequency.csv');
36
37hStackCpp = real(ifft(Hcpp, [], 2));
38
39center  = floor(length(hcpp) / 2);
40
41hStack  = zeros(order, wSize);
42HStack  = zeros(order, wSize);
43
44nyq     = floor(wSize / 2) + 1;
45
46f       = (0:(nyq - 1)) * fs / (2 * nyq);
47fpi     = (0:(nyq - 1)) * pi / nyq;
48
49data    = load(dataTarget);
50
51for o = 1:order
52
53    w               = zeros(wSize, 1);
54
55    fCenter         = center - (fs * gamma * log(o));
56
57    fGap2Next       = fs * gamma * log(1 + 1 / o);
58    fGap2Previous   = fs * gamma * log(o / (o - 1));
59
60    % Optimal Gap
61
62    fOptHead        = fCenter - min(fGap2Next / 2, wSize / 2);
63    fOptTail        = fCenter + min(fGap2Previous / 2, wSize / 2);
64    fOptLength      = fOptTail - fOptHead;
65    nMaxData        = floor(fOptLength);
66
67    nOptHead        = floor(fOptHead);
68    nOptTail        = floor(fOptTail);
69    fOptCount       = floor(fOptLength);
70
71    fHead           = fOptHead;
72    nHead           = nOptHead;
73
74    fWhead          = 0.5 * (wSize - min(fGap2Next, wSize)) + 1;
75    nWhead          = floor(fWhead);
76
77    innerFadeIn     = min(innerFadeIn, floor(min(fGap2Next, wSize) / 2));
78    innerFadeOut    = min(innerFadeOut, floor(min(fGap2Previous, wSize) / 2));
79
80%    % Simmetric up to +/- Gap2Next / 2 with respect impulse response center.
81%
82%    fMaxWindowable  = fGap2Next;
83%
84%    fMaxData        = min(fMaxWindowable, wSize);
85%    nMaxData        = round(fMaxData);
86%
87%    fHead           = fCenter - (fMaxData / 2);
88%    nHead           = floor(fHead);
89%
90%    fWhead          = ((wSize - fMaxData) / 2) + 1;
91%    nWhead          = floor(fWhead);
92
93%    innerFadeIn     = min(innerFadeIn, floor(fMaxData / 2));
94%    innerFadeOut    = min(innerFadeOut, floor(fMaxData / 2));
95
96    cShift          = nHead - fHead;
97    wShift          = fWhead - nWhead;
98
99    % This is the overall shift to align the responses to the middle of the
100    % window.
101    alignShift      = cShift + wShift;
102    totalShift      = alignShift;
103
104%    % If there are antialising FIRs, this will remove their added delay
105%    % (if the direct loop latency has been assessed independently before the
106%    % measurement). This just to check that the phase response is correct (
107%    % expect deviation at high frequency, as the resulting Hammerstein kernel
108%    % is actually made equivalent to a Wiener Hammerstein branch).
109%    if isfield(data, 'antialFIRTaps')
110%        totalShift  = totalShift - ((data.antialFIRTaps - 1) / 2);
111%    endif
112
113    w(nWhead:(nWhead + nMaxData - 1)) = ...
114        hcpp(nHead:(nHead + nMaxData - 1)) .* ...
115        innerWindowFcn(length(nHead:(nHead + nMaxData - 1)), innerFadeIn, innerFadeOut);
116
117    phFactor        = ones(size(w));
118    phFactor(1:nyq) = exp(-1i * 2 * pi * ((1:nyq) - 1) * totalShift / wSize);
119    phFactor(mod(wSize + 1 - (1:nyq), wSize) + 1) = conj(phFactor(1:nyq));
120
121    HStack(o, :)    = fft(w .* outerWindowFcn(length(w))) .* phFactor;
122    hStack(o, :)    = real(ifft(HStack(o, :)));
123
124endfor
125
126C = zeros(order);
127
128if preScaling
129    scaleAmplitude = alpha;
130else
131    scaleAmplitude = 1.0;
132endif
133
134for r = 1:order
135    for c = r:order
136
137        if isequal(mod(c + r, 2), 0)
138
139            C(r, c) = ...
140                    scaleAmplitude^(c - 1) * ...
141                    2^(1 - c) * ...
142                    nchoosek(c, (c - r) / 2) * ...
143                    (-1)^(2 * c - ((r - 1) / 2));
144
145        end
146
147    end
148end
149
150% Let's check the levels:
151Hlevels         = 20.0 * log10(abs(HStack));
152HlevelsCheck    = max(Hlevels, [], 2) < checkThreshold;
153
154% And Let's derive the Kernel Targets.
155GStack = C \ HStack;
156
157% Scaling the end results durectly if prescaling of coefficents was not made.
158if ~preScaling
159    GStack = bsxfun(@times, GStack, alpha.^(transpose(1 - (1:order))));
160endif
161
162% Forcing hermitian symmetry, as the system above is for the positive frequency.
163GStack(:, mod(wSize + 1 - (1:nyq), wSize) + 1) = conj(GStack(:, 1:nyq));
164
165% Let's make the kernels impulse responses before time shift, as they are easier
166% to look at if centered.
167gStack = real(ifft(GStack, [], 2));
168
169% This is the shift to remove the latency that we would artificially add by
170% having the higher order responses aligned in the middle of the window.
171latencyShift = wSize / 2;
172phFactor2 = ones(1, size(GStack, 2));
173phFactor2(1:nyq) = exp(-1i * 2 * pi * ((1:nyq) - 1) * latencyShift / wSize);
174phFactor2(mod(wSize + 1 - (1:nyq), wSize) + 1) = conj(phFactor2(1:nyq));
175GStack = bsxfun(@times, GStack, phFactor2);
176
177expected = transpose(freqz(data.b, data.a, nyq));
178
179% If there are antialising FIRs, this will add their latency to the expected
180% result, to ease comparison.
181refShift = 0;
182
183if isfield(data, 'antialFIRTaps')
184    refShift  = (data.antialFIRTaps - 1) / 2;
185endif
186
187phFactor3   = exp(-1i * 2 * pi * ((1:nyq) - 1) * refShift / wSize);
188expected    = expected .* phFactor3;
189
190figure;
191subplot(2, 1, 1);
192semilogx(f(2:end), 20 * log10(abs(GStack(:, 2:nyq).')));
193hold on;
194semilogx(f(2:end), 20 * log10(abs(expected(2:end))), 'k--');
195hold off;
196grid on;
197xlim([f(2) f(end)]);
198xlabel('Frequency [Hz]');
199ylabel('Measured Kernels [dB]');
200legend([cellfun(@num2str,num2cell(1:order),'un',0) 'Ref'], "Location", "northeastoutside");
201subplot(2, 1, 2);
202semilogx(f(2:end), angle(GStack(:, 2:nyq)).');
203hold on;
204semilogx(f(2:end), angle(expected(2:end)), 'k--');
205hold off;
206grid on;
207xlim([f(2) f(end)]);
208xlabel('Frequency [Hz]');
209ylabel('Measured Kernels [rad]');
210
211figure;
212subplot(2, 1, 1);
213semilogx(f(2:end), 20 * log10(abs(HStack(:, 2:nyq).')));
214grid on;
215xlim([f(2) f(end)]);
216xlabel('Frequency [Hz]');
217ylabel('Measured HORs [dB]');
218legend(cellfun(@num2str,num2cell(1:order),'un',0), "Location", "northeastoutside");
219subplot(2, 1, 2);
220semilogx(f(2:end), unwrap(angle(bsxfun(@times, HStack(:, 2:nyq), phFactor2(2:nyq))), [], 2).');
221grid on;
222xlim([f(2) f(end)]);
223xlabel('Frequency [Hz]');
224ylabel('Measured HORs [rad]');
225
226figure;
227subplot(2, 1, 1);
228semilogx(f(2:end), 20 * log10(abs(Hcpp(:, 2:nyq).')));
229grid on;
230xlim([f(2) f(end)]);
231xlabel('Frequency [Hz]');
232ylabel('Measured HORs C++ [dB]');
233legend(cellfun(@num2str,num2cell(1:order),'un',0), "Location", "northeastoutside");
234subplot(2, 1, 2);
235semilogx(f(2:end), unwrap(angle(bsxfun(@times, Hcpp(:, 2:nyq), phFactor2(2:nyq))), [], 2).');
236grid on;
237xlim([f(2) f(end)]);
238xlabel('Frequency [Hz]');
239ylabel('Measured HORs C++ [rad]');
240
241figure;
242subplot(2, 1, 1);
243semilogx(f(2:end), 20 * log10(abs(HStack(:, 2:nyq).')), 'LineWidth', 2);
244hold on;
245semilogx(f(2:end), 20 * log10(abs(Hcpp(:, 2:nyq).')), '--', 'LineWidth', 2);
246hold off;
247grid on;
248xlim([f(2) f(end)]);
249xlabel('Frequency [Hz]');
250ylabel('HORs Test VS C++ [dB]');
251subplot(2, 1, 2);
252semilogx(f(2:end), (angle(bsxfun(@times, HStack(:, 2:nyq), phFactor2(2:nyq)))).', 'LineWidth', 2);
253hold on;
254semilogx(f(2:end), (angle(bsxfun(@times, Hcpp(:, 2:nyq), phFactor2(2:nyq)))).', '--', 'LineWidth', 2);
255hold off;
256grid on;
257xlim([f(2) f(end)]);
258xlabel('Frequency [Hz]');
259ylabel('HORs Test VS C++ [rad]');
260
261%% Test some biquad section identification
262targetIdx   = 1;
263bqdOrder    = 32;
264
265targetIdx   = min(targetIdx, size(GStack, 1));
266
267target      = GStack(targetIdx, 1:nyq);
268
269W           = zeros(size(target));
270W(f > 20.0 & f < 20000.0) = 1.0;
271
272[b, a]      = invfreqz(target, fpi, bqdOrder, bqdOrder, W, 1e6, 1e-12, 'trace');
273idH         = freqz(b, a, nyq);
274
275[SOS, G]    = tf2sos (b, a);
276
277figure;
278subplot(2, 1, 1);
279semilogx(f(2:end), 20 * log10(abs([idH(2:end) target(2:end).'])));
280xlabel('Frequency [Hz]');
281ylabel('[dB]');
282legend('Identified', 'Target');
283grid on;
284xlim([f(2) f(end)]);
285subplot(2, 1, 2);
286semilogx(f(2:end), angle([idH(2:end) target(2:end).']))
287xlabel('Frequency [Hz]');
288ylabel('[rad]')
289grid on;
290xlim([f(2) f(end)]);