Array_WAH/effect_of_source_motion.m at main · raviumadi/Array_WAH · GitHub

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
%% demo_compare_velocity_effects.m
% ------------------------------------------
% Evaluate localisation errors for FM and CF calls
% at different source velocities.
% ------------------------------------------

clear; clc;
rng(42);
% --- Fixed Parameters ---
fs = 192e3;                % Sampling rate
d = 100e-3;                 % Duration of call (20 ms)
f0 = 25000;                % FM start frequency
f1 = 80000;                % FM end / CF tone frequency
tail = 90;                 % Padding
snr_db = 60;               % Signal-to-noise ratio
micSpacing = 0.5;          % Array size (metres)
source_position = [3.2, -3.8, 1.5];

velocities = [-10:0.1:10];   % m/s
types = {'FM', 'CF'};

% --- Microphone Configuration ---
cfg = mic_array_configurator(4, 'Tetrahedron', micSpacing);
mic_positions = cfg.mic_positions;

% --- Prepare Results Struct ---
results = struct();

for ti = 1:length(types)
    callType = types{ti};
    if callType == 'FM'
        d = 0.01;
    else
        d = 0.1;
    end
    fprintf('\n===== Call Type: %s =====\n', callType);

    for vi = 1:length(velocities)
        v = velocities(vi);

        % --- Assemble Parameters ---
        params = struct();
        params.fs = fs;
        params.d = d;
        params.f0 = f0;
        params.f1 = f1;
        params.tail = tail;
        params.snr_db = snr_db;
        params.micSpacing = micSpacing;
        params.callType = callType;
        params.velocity = [v, 0, 0];  % Motion along x-axis
        params.mic_positions = mic_positions;

        % --- Create Localiser & Simulate ---
        localiser = BatCallLocaliser(params);
        result = localiser.simulate(source_position);
        out = localiser.test(result, 0, 0);

        % --- Compute Ground Truth Angles ---
        ref_mic = mic_positions(1,:);
        rel_vector = source_position - ref_mic;
        az_true = atan2d(rel_vector(2), rel_vector(1));
        el_true = asind(rel_vector(3) / norm(rel_vector));

        % --- Estimate & Errors ---
        az_est = out.tdoa.azimuth;
        el_est = out.tdoa.elevation;
        ang_error = sqrt((az_true - az_est)^2 + (el_true - el_est)^2);
        pos_error = out.tdoa.error * 100;  % cm

        % --- Save Results ---
        results.(callType)(vi).velocity = v;
        results.(callType)(vi).az_error = az_est - az_true;
        results.(callType)(vi).el_error = el_est - el_true;
        results.(callType)(vi).ang_error = ang_error;
        results.(callType)(vi).pos_error = pos_error;

        % --- Print Summary ---
        % fprintf('v = %+3.1f m/s | Position Error: %5.2f cm | Angular Error: %5.2f°\n', ...
        %     v, pos_error, ang_error);
    end
end

% --- Optional: Plotting ---
figure('Position', [100, 100, 1200, 300]);
for ti = 1:length(types)
    callType = types{ti};
    vels = arrayfun(@(r) r.velocity, results.(callType));
    posErrs = arrayfun(@(r) r.pos_error, results.(callType));
    angErrs = arrayfun(@(r) r.ang_error, results.(callType));

    subplot(1,2,1); hold on;
    plot(vels, posErrs, '-', 'LineWidth', 2);
    ylabel('Position Error (cm)'); xlabel('Velocity (m/s)');
    title('Effect of Velocity on Position Error'); formatLatex(gca);
    legend({'FM', 'CF'}, 'FontSize', 14, 'Interpreter','latex')

    subplot(1,2,2); hold on;
    plot(vels, angErrs, '-','LineWidth', 2);
    ylabel('Angular Error (deg)'); xlabel('Velocity (m/s)');
    title('Effect of Velocity on Angular Error'); formatLatex(gca);
    legend({'FM', 'CF'}, 'FontSize', 14, 'Interpreter','latex')

end
subplot(1,2,1); legend;
subplot(1,2,2); legend;

%% export graphics