1% bf = bf_design(bf) 2% 3% This script calculates beamformer filters with superdirective design 4% criteria. 5 6% SPDX-License-Identifier: BSD-3-Clause 7% 8% Copyright (c) 2020, Intel Corporation. All rights reserved. 9% 10% Author: Seppo Ingalsuo <seppo.ingalsuo@linux.intel.com> 11 12function bf = bf_design(bf) 13 14addpath('../../test/audio/test_utils'); 15addpath('../../test/audio/std_utils'); 16mkdir_check('plots'); 17mkdir_check('data'); 18 19if length(bf.steer_az) ~= length(bf.steer_el) 20 error('The steer_az and steer_el lengths need to be equal.'); 21end 22 23if length(find(bf.steer_az > 180)) || length(find(bf.steer_az < -180)) 24 error('The steer_az angles need to be -180 to +180 degrees'); 25end 26 27if length(find(bf.steer_el > 90)) || length(find(bf.steer_el < -90)) 28 error('The steer_el angles need to be -90 to +90 degrees'); 29end 30 31switch lower(bf.array) 32 case 'line' 33 bf = bf_array_line(bf); 34 case 'circular' 35 bf = bf_array_circ(bf); 36 case 'rectangle' 37 bf = bf_array_rect(bf); 38 case 'lshape' 39 bf = bf_array_lshape(bf); 40 case 'xyz' 41 bf = bf_array_xyz(bf); 42 otherwise 43 error('Invalid array type') 44end 45 46if isempty(bf.num_filters) 47 if isempty(bf.input_channel_select) 48 bf.num_filters = bf.mic_n; 49 else 50 bf.num_filters = length(bf.input_channel_select); 51 end 52end 53 54bf = bf_array_rot(bf); 55 56% The design function handles only single (az, el) value, so need to 57% loop every steer angle. 58bf.num_angles = length(bf.steer_az); 59all_az = bf.steer_az; 60all_el = bf.steer_el; 61all_array_id = bf.array_id; 62all_sinerot_fn = bf.sinerot_fn; 63all_diffuse_fn = bf.diffuse_fn; 64all_random_fn = bf.random_fn; 65all_mat_fn = bf.mat_fn; 66 67for n = 1:bf.num_angles 68 bf.steer_az = all_az(n); 69 bf.steer_el = all_el(n); 70 bf.array_id = all_array_id{n}; 71 bf.sinerot_fn = all_sinerot_fn{n}; 72 bf.diffuse_fn = all_diffuse_fn{n}; 73 bf.random_fn = all_random_fn{n}; 74 bf.mat_fn = all_mat_fn{n}; 75 bf = bf_one_design(bf); 76 w_all(:,:,n) = bf.w; 77end 78 79bf.steer_az = all_az; 80bf.steer_el = all_el; 81bf.array_id = all_array_id; 82bf.sinerot_fn = all_sinerot_fn; 83bf.diffuse_fn = all_diffuse_fn; 84bf.random_fn = all_random_fn; 85bf.mat_fn = all_mat_fn; 86bf.w = w_all; 87 88end 89 90function bf = bf_one_design(bf) 91 92 93%% Defaults 94j = complex(0,-1); 95fs = bf.fs; 96N = 1024; 97N_half = N/2+1; 98f = (0:N/2)*fs/N'; 99phi_rad = (-180:180)*pi/180; 100phi_rad = phi_rad(1:end-1); 101n_phi = length(phi_rad); 102steer_az = bf.steer_az*pi/180; 103steer_el = bf.steer_el*pi/180; 104mu = ones(1,N_half) * 10^(bf.mu_db/20); 105 106%% Source at distance r 107[src_x, src_y, src_z] = source_xyz(bf.steer_r, steer_az, steer_el); 108 109%% Default frequency domain weights 110W = zeros(N_half, bf.num_filters); 111 112%% Coherence matrix, diffuse field 113% Equation 2.11 114Gamma_vv = zeros(N_half, bf.num_filters, bf.num_filters); 115for n=1:bf.num_filters 116 for m=1:bf.num_filters 117 % Equation 2.17 118 lnm = sqrt( (bf.mic_x(n) - bf.mic_x(m))^2 ... 119 +(bf.mic_y(n) - bf.mic_y(m))^2 ... 120 +(bf.mic_z(n) - bf.mic_z(m))^2); 121 Gamma_vv(:,n,m) = sinc(2*pi*f*lnm/bf.c); 122 end 123end 124 125%% Delays from source to each mic 126dt = delay_from_source(bf, src_x, src_y, src_z); 127dt = dt-min(dt); 128 129%% Create array vector 130tau0 = zeros(n_phi, bf.num_filters); 131A = zeros(N_half, n_phi, bf.num_filters); 132d = zeros(N_half, bf.num_filters); 133for n=1:bf.num_filters 134 % Equation 2.27 135 d(:,n) = exp(-j*2*pi*f*dt(n)); % Delays to steer direction 136 for ip = 1:n_phi; 137 phi = phi_rad(ip); 138 x_phi = bf.steer_r*cos(phi)*cos(steer_el); 139 y_phi = bf.steer_r*sin(phi)*cos(steer_el); 140 z_phi = bf.steer_r*sin(steer_el); 141 tau0(ip, n) = sqrt((x_phi-bf.mic_x(n))^2 ... 142 + (y_phi-bf.mic_y(n))^2 ... 143 + (z_phi-bf.mic_z(n))^2)/bf.c; 144 end 145end 146tau = tau0-min(min(tau0)); 147 148for n=1:bf.num_filters 149 for ip = 1:n_phi 150 % N_half x n_phi x Nm 151 A(:, ip, n) = exp(-j*2*pi*f*tau(ip, n)); 152 end 153end 154 155switch lower(bf.type) 156 case 'sdb' 157 % Superdirective 158 for iw = 1:N_half 159 % Equation 2.33 160 I = eye(bf.num_filters, bf.num_filters); 161 d_w = d(iw,:).'; 162 Gamma_vv_w = squeeze(Gamma_vv(iw, :, :)); 163 Gamma_vv_w_diagload = Gamma_vv_w + mu(iw)*I; 164 Gamma_vv_w_inv = inv(Gamma_vv_w_diagload); 165 num = Gamma_vv_w_inv * d_w; 166 denom1 = d_w' * Gamma_vv_w_inv; 167 denom2 = denom1 * d_w; 168 W_w = num / denom2; 169 W(iw, :) = W_w.'; 170 end 171 case 'dsb' 172 % Delay and sum 173 for iw = 1:N_half 174 % Equation 2.31 175 % W = 1/N*d 176 d_w = d(iw, :); 177 W_w = 1/bf.num_filters * d_w; 178 W(iw, :) = W_w.'; 179 end 180 otherwise 181 error('Invalid type, use SDB or DSB'); 182end 183 184%% Convert w to time domain 185W_full = zeros(N, bf.num_filters); 186W_full = W(1:N_half, :); 187for i=N_half+1:N 188 W_full(i,:) = conj(W(N_half-(i-N_half),:)); 189end 190 191win = kaiser(bf.fir_length,bf.fir_beta); 192bf.w = zeros(bf.fir_length, bf.num_filters); 193w_tmp = zeros(N, bf.num_filters); 194idx_max = zeros(bf.num_filters, 1); 195for i=1:bf.num_filters 196 h = real(fftshift(ifft(W_full(:,i)))); 197 w_tmp(:,i) = h; 198 idx_max(i) = find(h == max(h)); 199end 200 201center_idx = round(mean(idx_max)); 202start = center_idx - floor(bf.fir_length/2); 203win0 = kaiser(bf.fir_length,bf.fir_beta); 204for i=1:bf.num_filters 205 win = zeros(bf.fir_length, 1); 206 win_shift = center_idx - idx_max(i) - 1; 207 if (win_shift >= 0) 208 win(1:end-win_shift) = win0(win_shift+1:end); 209 else 210 win(-win_shift:end) = win0(1:end+win_shift+1); 211 end 212 bf.w(:,i) = w_tmp(start:start + bf.fir_length - 1, i) .* win; 213end 214 215%% Back to frequency domain to check spatial response 216W2_full = zeros(N, bf.num_filters); 217for i=1:bf.num_filters 218 % Zero pad 219 h2 = zeros(1,N); 220 h2(start:start + bf.fir_length - 1) = bf.w(:,i); 221 W2_full(:,i) = fft(h2); 222end 223W2 = W2_full(1:N_half, :); 224B2 = zeros(N_half, n_phi); 225for iw = 1:N_half 226 WT = (W2(iw,:)').'; 227 AS = squeeze(A(iw,:,:)).'; 228 WA = WT * AS; 229 B2(iw,:) = WA; 230end 231bf.resp_fa = B2'; 232bf.resp_angle = phi_rad * 180/pi; 233 234%% Directivity in diffuse field 235% Equation 2.18 236% DI(exp(j Omega) = 10*log10( abs(W^H d)^2 / (W^H Gamma_vv W)) 237bf.f = f; 238bf.di_db = zeros(1, N_half); 239for iw = 1:N_half 240 W_w = W2(iw,:).'; 241 d_w = d(iw,:).'; 242 Gamma_vv_w = squeeze(Gamma_vv(iw,:,:)); 243 W_wh = W_w'; 244 num = abs(W_wh * d_w)^2; 245 denom1 = W_wh * Gamma_vv_w; 246 denom2 = denom1 * W_w; 247 di = num / denom2; 248 bf.di_db(iw) = 10*log10(abs(di)); 249end 250 251 252%% White noise gain 253for iw = 1:N_half 254 % WNG = abs(^w^H d)^2/(w^H w); 255 W_w = W2(iw,:).'; 256 d_w = d(iw,:).'; 257 W_wh = W_w'; 258 num = abs(W_wh * d_w)^2; 259 denom = W_wh * W_w; 260 wng = num / denom2; 261 wng_db(iw) = 10*log10(abs(wng)); 262end 263bf.wng_db = wng_db; 264 265if bf.do_plots 266 %% Array 267 fh(1) = figure(bf.fn); 268 plot3(bf.mic_x(1), bf.mic_y(1), bf.mic_z(1), 'ro'); 269 hold on; 270 plot3(bf.mic_x(2:end), bf.mic_y(2:end), bf.mic_z(2:end), 'bo'); 271 plot3(src_x, src_y, src_z, 'gx'); 272 plot3([0 src_x],[0 src_y],[0 src_z],'c--') 273 for n=1:bf.num_filters 274 text(bf.mic_x(n), bf.mic_y(n), bf.mic_z(n) + 20e-3, ... 275 num2str(n)); 276 end 277 hold off 278 pb2 = bf.plot_box / 2; 279 axis([-pb2 pb2 -pb2 pb2 -pb2 pb2]); 280 axis('square'); 281 grid on; 282 xlabel('x (m)'); ylabel('y (m)'); zlabel('z (m)'); 283 view(-50, 30); 284 title(['Geometry ' bf.array_id], 'Interpreter','none'); 285 286 %% Coef 287 fh(2) = figure(bf.fn + 1); 288 plot(bf.w) 289 grid on; 290 xlabel('FIR coefficient'); ylabel('Tap value'); 291 title(['FIR filters ' bf.array_id], 'Interpreter','none'); 292 ch_legend(bf.num_filters); 293 294 %% Frequency responses 295 fh(3) = figure(bf.fn + 2); 296 f = linspace(0, bf.fs/2, 256); 297 h = zeros(256, bf.num_filters); 298 for i = 1:bf.num_filters 299 h(:,i) = freqz(bf.w(:,i), 1, f, bf.fs); 300 end 301 plot(f, 20*log10(abs(h))); 302 grid on 303 ylabel('Magnitude (dB)'); 304 xlabel('Frequency (Hz)'); 305 title(['FIR magnitude responses ' bf.array_id], 'Interpreter','none'); 306 ch_legend(bf.num_filters); 307 308 %% Group delays 309 fh(4) = figure(bf.fn + 3); 310 gd = zeros(256, bf.num_filters); 311 for i = 1:bf.num_filters 312 gd(:,i) = grpdelay(bf.w(:,i), 1, f, bf.fs); 313 end 314 plot(f, gd/bf.fs*1e6); 315 grid on 316 ylabel('Group delay (us)'); 317 xlabel('Frequency (Hz)'); 318 title(['FIR group delays ' bf.array_id], 'Interpreter','none'); 319 ch_legend(bf.num_filters); 320 321 %% DI 322 bf.fh(5) = figure(bf.fn + 4); 323 semilogx(bf.f(2:end), bf.di_db(2:end)) 324 xlabel('Frequency (Hz)'); ylabel('DI (dB)'); grid on; 325 legend('Suppression of diffuse field noise','Location','SouthEast'); 326 title(['Directivity Index ' bf.array_id], 'Interpreter','none'); 327 328 %% WNG 329 bf.fh(6) = figure(bf.fn + 5); 330 semilogx(bf.f(2:end), bf.wng_db(2:end)) 331 xlabel('Frequency (Hz)'); ylabel('WNG (dB)'); grid on; 332 legend('Attenuation of uncorrelated noise','Location','SouthEast'); 333 title(['White noise gain ' bf.array_id], 'Interpreter','none'); 334 drawnow; 335 336 %% 2D 337 bf.fh(7) = figure(bf.fn + 6); 338 colormap(jet); 339 phi_deg = phi_rad*180/pi; 340 imagesc(bf.f, bf.resp_angle, 20*log10(abs(bf.resp_fa)), [-30 0]); 341 set(gca,'YDir','normal') 342 grid on; 343 colorbar; 344 xlabel('Frequency (Hz)'); ylabel('Angle (deg)'); 345 title(['Spatial response ' bf.array_id], 'Interpreter','none'); 346 347 %% Polar 348 bf.fh(8) = figure(bf.fn + 7); 349 flist = [1000 2000 3000 4000]; 350 idx = []; 351 for i = 1:length(flist) 352 idx(i) = find(f > flist(i), 1, 'first'); 353 end 354 bf.resp_polar = abs(B2(idx,:)); 355 if exist('OCTAVE_VERSION', 'builtin') 356 polar(phi_rad, bf.resp_polar); 357 else 358 polarplot(phi_rad, bf.resp_polar); 359 end 360 legend('1 kHz','2 kHz','3 kHz','4 kHz'); 361 title(['Polar response ' bf.array_id], 'Interpreter','none'); 362end 363 364%% Create data for simulation 1s per angle 365 366if isempty(bf.sinerot_fn) 367 fprintf(1, 'No file for 360 degree sine source rotate specified\n'); 368else 369 fprintf(1, 'Creating 360 degree sine source rotate...\n'); 370 fsi = 384e3; % Target interpolated rate 371 p = round(fsi / bf.fs); % Interpolation factor 372 fsi = p * bf.fs; % Recalculate high rate 373 ti = 1/fsi; % perid at higher rate 374 t_add = 10.0/bf.c; % Additional signal time for max 10m propagation 375 tt0 = bf.sinerot_t + t_add; % Total sine length per angle 376 nt = bf.fs * bf.sinerot_t; % Number samples output per angle 377 nti = p * nt; % Number samples output per angle at high rate 378 t_ramp = 20e-3; % 20 ms ramps to start and end of each angle tone 379 n_ramp = t_ramp * bf.fs; 380 win = ones(nt, 1); 381 win(1:n_ramp) = linspace(0, 1, n_ramp); 382 win(end-n_ramp+1:end) = linspace(1, 0, n_ramp); 383 si = multitone(fsi, bf.sinerot_f, bf.sinerot_a, tt0); 384 test_az = (bf.sinerot_az_start:bf.sinerot_az_step:bf.sinerot_az_stop) * pi/180; 385 test_n = length(test_az); 386 test_el = zeros(1, test_n); 387 [test_x, test_y, test_z] = source_xyz(bf.steer_r, test_az, test_el); 388 td = zeros(test_n * nt, bf.mic_n); 389 for i = 1:length(test_az) 390 dt = delay_from_source(bf, test_x(i), test_y(i), test_z(i)); 391 dn = round(dt / ti); 392 mi = zeros(nti, bf.num_filters); 393 for j = 1:bf.mic_n 394 mi(:,j) = mi(:,j) + si(end-dn(j)-nti+1:end-dn(j)); 395 end 396 i1 = (i - 1) * nt + 1; 397 i2 = i1 + nt -1; 398 for j = 1:bf.mic_n 399 m = mi(1:p:end, j) .* win; 400 td(i1:i2, j) = m; 401 end 402 end 403 myaudiowrite(bf.sinerot_fn, td, bf.fs); 404end 405 406if isempty(bf.diffuse_fn) 407 fprintf(1, 'No file for diffuse noise field specified\n'); 408else 409 fprintf(1, 'Creating diffuse noise field...\n'); 410 fsi = 384e3; % Target interpolated rate 411 p = round(fsi / bf.fs); % Interpolation factor 412 fsi = p * bf.fs; % Recalculate high rate 413 ti = 1/fsi; % period at higher rate 414 t_add = 10.0/bf.c; % Additional signal time for max 20m propagation 415 t0 = bf.diffuse_t + t_add; % Total sine length per angle 416 n0 = floor(bf.fs * t0); 417 nt = floor(bf.fs * bf.diffuse_t); % Number samples output per angle 418 nti = p * nt; % Number samples output per angle at high rate 419 el = 0; 420 for az_deg = -160:20:180 % Azimuth plane only noise with sources 421 az = az_deg * pi/180; 422 [nx, ny, nz] = source_xyz(bf.steer_r, az, el); 423 dt = delay_from_source(bf, nx, ny, nz); 424 dn = round(dt / ti); 425 ns = rand(n0, 1) + rand(n0, 1) - 1; 426 nsi = interp(ns, p); 427 nmi = zeros(nti, bf.mic_n); 428 for j = 1:bf.mic_n 429 nmi(:,j) = nmi(:,j) + nsi(end-dn(j)-nti+1:end-dn(j)); 430 end 431 end 432 nm = nmi(1:p:end, :); 433 nlev = level_dbfs(nm(:,1)); 434 nm = nm * 10^((bf.diffuse_lev - nlev)/20); 435 myaudiowrite(bf.diffuse_fn, nm, bf.fs); 436end 437 438if isempty(bf.random_fn) 439 fprintf(1, 'No file for random noise specified\n'); 440else 441 fprintf(1, 'Creating random noise ...\n'); 442 nt = bf.fs * bf.random_t; 443 rn = rand(nt, bf.num_filters) + rand(nt, bf.num_filters) - 1; 444 445 nlev = level_dbfs(rn(:,1)); 446 rn = rn * 10^((bf.random_lev - nlev)/20); 447 myaudiowrite(bf.random_fn, rn, bf.fs); 448end 449 450if isempty(bf.mat_fn) 451 fprintf(1, 'No file for beam pattern simulation data specified.\n'); 452else 453 fprintf(1, 'Saving design to %s\n', bf.mat_fn); 454 bf_copy = bf; 455 bf.fh = []; % Don't save the large figures, this avoids a warning print too 456 mkdir_check(bf.data_path); 457 save(bf.mat_fn, 'bf'); 458 bf = bf_copy; 459end 460 461fprintf(1, 'Done.\n'); 462 463end 464 465%% Helper functions 466 467function [x, y, z] = source_xyz(r, az, el) 468 469x = r * cos(az) .* cos(el); 470y = r * sin(az) .* cos(el); 471z = r * sin(el); 472 473end 474 475function dt = delay_from_source(bf, src_x, src_y, src_z) 476 477dm = zeros(1,bf.num_filters); 478for n=1:bf.num_filters 479 dm(n) = sqrt((src_x - bf.mic_x(n))^2 ... 480 + (src_y - bf.mic_y(n))^2 ... 481 + (src_z - bf.mic_z(n))^2); 482end 483dt = dm/bf.c; 484 485end 486 487function ch_legend(n) 488switch n 489 case 2 490 legend('1', '2'); 491 case 3 492 legend('1', '2', '3'); 493 case 4 494 legend('1', '2', '3', '4'); 495 case 5 496 legend('1', '2', '3', '4', '5'); 497 case 6 498 legend('1', '2', '3', '4', '5', '6'); 499 case 7 500 legend('1', '2', '3', '4', '5', '6', '7'); 501 otherwise 502 legend('1', '2', '3', '4', '5', '6', '7', '8'); 503end 504end 505 506function myaudiowrite(fn, x, fs) 507[~, ~, ext] = fileparts(fn); 508if strcmp(lower(ext), '.raw') 509 s = size(x); 510 xq = zeros(s(1) * s(2), 1, 'int16'); 511 scale = 2^15; 512 xs = round(x * scale); 513 xs(xs > scale - 1) = scale -1; 514 xs(xs < -scale) = -scale; 515 for i = 1:s(2) 516 xq(i:s(2):end) = xs(:,i); 517 end 518 fh = fopen(fn, 'wb'); 519 fwrite(fh, xq, 'int16'); 520 fclose(fh); 521else 522 audiowrite(fn, x, fs); 523end 524end 525
