-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathtest_superdirct.m
120 lines (106 loc) · 4.06 KB
/
test_superdirct.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
clc
clear all
freq = 150;
kWave = 2*pi*freq/340;
%% Get mics
X_mic = 1;
Y_mic = 1;
nMic_x = 3;
nMic_y = 3;
nMic = nMic_x*nMic_y;
rect_mic = [-X_mic,X_mic;...
-Y_mic,Y_mic];
z_mic = 1;
[pos, ~] = get_grid(rect_mic, z_mic, nMic_x, nMic_y);
mic_pos = pos';
%% data
soc_pos = [1.2,-0.7,0.01];
dist_x = soc_pos(1)*ones(1,nMic) - mic_pos(1,:);
dist_y = soc_pos(2)*ones(1,nMic) - mic_pos(2,:);
dist_z = soc_pos(3)*ones(1,nMic) - mic_pos(3,:);
dist = sqrt(dist_x.^2 + dist_y.^2 + dist_z.^2);
soc_pos_noise1 = [1, 0.7, 0];
dist_x = soc_pos_noise1(1)*ones(1,nMic) - mic_pos(1,:);
dist_y = soc_pos_noise1(2)*ones(1,nMic) - mic_pos(2,:);
dist_z = soc_pos_noise1(3)*ones(1,nMic) - mic_pos(3,:);
dist_noise1 = sqrt(dist_x.^2 + dist_y.^2 + dist_z.^2);
soc_pos_noise2 = [-2, 0.7, 0];
dist_x = soc_pos_noise2(1)*ones(1,nMic) - mic_pos(1,:);
dist_y = soc_pos_noise2(2)*ones(1,nMic) - mic_pos(2,:);
dist_z = soc_pos_noise2(3)*ones(1,nMic) - mic_pos(3,:);
dist_noise2 = sqrt(dist_x.^2 + dist_y.^2 + dist_z.^2);
pres = - exp(1i*kWave*dist)./(4*pi*dist) - 1*exp(1i*kWave*dist_noise1)./(4*pi*dist_noise1)...
- 1*exp(1i*kWave*dist_noise2)./(4*pi*dist_noise2);
%% Get sources
nSoc_x = 500;
nSoc_y = 500;
lSoc = 20;
rect_soc = [-lSoc,lSoc;...
-lSoc,lSoc];
z_soc = 0;
[soc_pos_all, dA] = get_grid(rect_soc, z_soc, nSoc_x, nSoc_y);
E_all = compute_energy(soc_pos_all, mic_pos, kWave, dA);
%% Monitoring
nPix_x = 30;
nPix = nPix_x^2;
LX = 5;
LY = 5;
rect_moni = [-LX,LX;...
-LY,LY];
[soc_moni, ~] = get_grid(rect_moni, 0, nPix_x, nPix_x);
focus = 0.5;
nInt = 10;
energy = zeros(nPix,1);
parfor iPix = 1:nPix
iPix
center = soc_moni(iPix,:);
% noise
% [soc_pos_all, dA] = get_grid(rect_soc, z_soc, nSoc_x, nSoc_y);
% index_delet = find(norm([soc_pos_all(:,1)-center(1),soc_pos_all(:,2)-center(2),soc_pos_all(:,3)-center(3)])<focus);
% soc_pos_all(index_delet,:) = [];
% E_all = compute_energy(soc_pos_all, mic_pos, kWave, dA);
% focus
dist_x = center(1)*ones(1,nMic) - mic_pos(1,:);
dist_y = center(2)*ones(1,nMic) - mic_pos(2,:);
dist_z = center(3)*ones(1,nMic) - mic_pos(3,:);
dist = sqrt(dist_x.^2 + dist_y.^2 + dist_z.^2).';
steering = -exp(1i*kWave*dist)./(4*pi*dist);
beamformer = (E_all\steering)/(steering'*(E_all\steering));
energy(iPix) = abs(beamformer'*pres.');
end
% energy = reshape(energy,[nPix_x,nPix_x]);
%% plot
figure
pcolor(reshape(soc_moni(:,1),[nPix_x,nPix_x]), reshape(soc_moni(:,2),[nPix_x,nPix_x]), reshape(energy,[nPix_x,nPix_x]))
shading interp;
hold on
scatter(mic_pos(1,:),mic_pos(2,:),'MarkerFaceColor',[0,0,0])
scatter(soc_pos(1),soc_pos(2), 60, 'd', 'MarkerEdgeColor',[0 .5 .5],...
'MarkerFaceColor',[0 .7 .7],...
'LineWidth',1.5)
scatter(soc_pos_noise1(1),soc_pos_noise1(2), 60, 's', 'MarkerEdgeColor',[0 .5 .5],...
'MarkerFaceColor',[0 .7 .7],...
'LineWidth',1.5)
scatter(soc_pos_noise2(1),soc_pos_noise2(2), 60, 's', 'MarkerEdgeColor',[0 .5 .5],...
'MarkerFaceColor',[0 .7 .7],...
'LineWidth',1.5)
function [pos, dA] = get_grid(rect, z, nX, nY)
x = linspace(rect(1,1), rect(1,2), nX);
y = linspace(rect(2,1), rect(2,2), nY);
[X,Y] = meshgrid(x,y);
pos = [X(:), Y(:), z*ones(nX*nY,1)];
dA = (rect(1,2) - rect(1,1))*(rect(2,2) - rect(2,1))/nX/nY;
end
function E = compute_energy(soc_pos, mic_pos, kWave, dA)
nMic = size(mic_pos,2);
nSoc = size(soc_pos,1);
%% Get distance
dist_x = soc_pos(:,1)*ones(1,nMic) - ones(nSoc,1)*mic_pos(1,:);
dist_y = soc_pos(:,2)*ones(1,nMic) - ones(nSoc,1)*mic_pos(2,:);
dist_z = soc_pos(:,3)*ones(1,nMic) - ones(nSoc,1)*mic_pos(3,:);
dist = sqrt(dist_x.^2 + dist_y.^2 + dist_z.^2);
%% Get pressure
pres = -exp(1i*kWave*dist)./(4*pi*dist);
%% Integration
E = pres'*pres*dA;
end