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)]);