Skip to content

Commit

Permalink
even better estimation for relative mode
Browse files Browse the repository at this point in the history
  • Loading branch information
@ben-wes
ben-wes committed Jan 3, 2025
1 parent 5f07250 commit ab2251f
Show file tree
Hide file tree
Showing 2 changed files with 70 additions and 62 deletions.
18 changes: 12 additions & 6 deletions tetra2pos-help.pd
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
#N canvas 194 225 841 812 10;
#X text 333 133 1 Front: [0.0 \, 408.2 \, 0.0] \; 2 Left: [-500.0 \, -204.1 \, 0.0] \; 3 Right: [500.0 \, -204.1 \, 0.0] \; 4 Top: [0.0 \, 0 \, 612.4];
#N canvas 579 194 841 812 10;
#X text 303 131 1 Front: [0.0 \, 408.2 \, 0.0] \; 2 Left: [-500.0 \, -204.1 \, 0.0] \; 3 Right: [500.0 \, -204.1 \, 0.0] \; 4 Top: [0.0 \, 0 \, 612.4];
#X listbox 68 651 39 0 0 0 - - - 0;
#X obj 68 52 vsl 16 136 -10000 10000 0 0 empty empty empty 0 -9 0 10 #fcfcfc #000000 #000000 0 1;
#X text 41 42 10m;
Expand All @@ -14,7 +14,7 @@
#X floatatom 168 196 7 0 0 0 - - - 0;
#X text 132 600 <-- edge length via arg (default is 1000mm);
#X listbox 68 544 39 0 0 0 - - - 0;
#X text 334 116 mic coordinates for edge length 1000mm;
#X text 304 114 mic coordinates for edge length 1000mm;
#X listbox 534 651 39 0 0 0 - - - 0;
#X obj 68 324 t a a a a, f 12;
#X obj 68 461 unpack f f f;
Expand All @@ -39,9 +39,9 @@
#X obj 137 502 +;
#X obj 534 466 hsl 136 16 -1000 1000 0 0 empty empty empty -2 -8 0 10 #fcfcfc #000000 #000000 0 1;
#X floatatom 531 487 5 0 0 0 - - - 0;
#X msg 334 222 5838.2 6506.4 6191.4 5890.7;
#X text 334 237 distances for point at (2 \, 5 \, 3) in meters;
#X obj 334 264 unpack f f f f;
#X msg 304 220 5838.2 6506.4 6191.4 5890.7;
#X text 304 235 distances for point at (2 \, 5 \, 3) in meters;
#X obj 304 262 unpack f f f f;
#X text 567 485 offset to test TDOA;
#X text 567 499 (time difference of arrival);
#X obj 531 424 loadbang;
Expand Down Expand Up @@ -136,6 +136,8 @@
#X obj 579 685 r \$0-pos;
#X obj 68 229 f;
#X obj 118 217 f;
#X msg 210 34 0;
#X obj 241 34 loadbang;
#X connect 1 0 47 0;
#X connect 2 0 10 0;
#X connect 5 0 11 0;
Expand Down Expand Up @@ -204,3 +206,7 @@
#X connect 58 0 57 1;
#X connect 59 0 9 0;
#X connect 60 0 7 0;
#X connect 61 0 6 0;
#X connect 61 0 5 0;
#X connect 61 0 2 0;
#X connect 62 0 61 0;
114 changes: 58 additions & 56 deletions tetra2pos.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -120,76 +120,78 @@ static void solve_position_tdoa(t_float distances[4], t_float positions[4][3], t
t_float best_result[3] = {0, 0, 0};
t_float calc_distances[4] = {0};

// Find maximum TDOA to help with range estimation
t_float max_tdoa = 0;
// Find maximum absolute TDOA to help with range estimation
t_float max_abs_tdoa = 0;
for (int i = 0; i < 3; i++) {
if (fabs(tdoa[i]) > max_tdoa) max_tdoa = fabs(tdoa[i]);
if (fabs(tdoa[i]) > max_abs_tdoa) max_abs_tdoa = fabs(tdoa[i]);
}

t_float r1_start = 500; // Start with a reasonable minimum distance
t_float r1_end = 50000; // Much larger maximum distance (50 meters)
t_float r1_step = 100; // Larger initial step for the wider range
// Start with a reasonable minimum distance that's at least max_abs_tdoa
t_float r1_start = fmax(1000, max_abs_tdoa);
t_float r1_end = 50000; // Maximum reasonable distance
t_float r1_step = r1_start * 0.1; // Initial step proportional to start distance

// Two-phase search: first coarse, then fine
for (int phase = 0; phase < 2; phase++) {
// Multiple passes with decreasing step sizes
for (int phase = 0; phase < 4; phase++) { // Back to 4 phases
// Adjust angular resolution based on phase
t_float angle_step = (phase < 2) ? M_PI / 8 :
(phase < 3) ? M_PI / 16 : M_PI / 32;

for (t_float r1 = r1_start; r1 < r1_end; r1 += r1_step) {
// Skip if base distance is too small compared to max TDOA
if (r1 < max_tdoa) continue;

t_float test_distances[4];
test_distances[0] = r1;
test_distances[1] = r1 + tdoa[0];
test_distances[2] = r1 + tdoa[1];
test_distances[3] = r1 + tdoa[2];

// Skip if any distance is negative or too small
if (test_distances[1] < max_tdoa ||
test_distances[2] < max_tdoa ||
test_distances[3] < max_tdoa) {
continue;
}

t_float test_result[3];
solve_position_toa(test_distances, positions, test_result);

// Calculate actual distances and error
t_float error = 0;
t_float max_calc_dist = 0;
for (int i = 0; i < 4; i++) {
t_float dx = test_result[0] - positions[i][0];
t_float dy = test_result[1] - positions[i][1];
t_float dz = test_result[2] - positions[i][2];
calc_distances[i] = sqrt(dx*dx + dy*dy + dz*dz);
if (calc_distances[i] > max_calc_dist) max_calc_dist = calc_distances[i];
}

// Calculate normalized error
for (int i = 0; i < 3; i++) {
t_float calc_tdoa = calc_distances[i+1] - calc_distances[0];
error += fabs(calc_tdoa - tdoa[i]) / (max_calc_dist + 1.0f);
}

if (error < min_error) {
min_error = error;
best_result[0] = test_result[0];
best_result[1] = test_result[1];
best_result[2] = test_result[2];
// Try multiple base angles for each radius
for (t_float angle = 0; angle < M_PI * 2; angle += angle_step) {
t_float test_distances[4];
test_distances[0] = r1;
test_distances[1] = r1 + tdoa[0];
test_distances[2] = r1 + tdoa[1];
test_distances[3] = r1 + tdoa[2];

// Skip if any distance is negative or too small
if (test_distances[1] < max_abs_tdoa ||
test_distances[2] < max_abs_tdoa ||
test_distances[3] < max_abs_tdoa) {
continue;
}

t_float test_result[3];
solve_position_toa(test_distances, positions, test_result);

t_float error = 0;
t_float max_calc_dist = 0;

for (int i = 0; i < 4; i++) {
t_float delta_x = test_result[0] - positions[i][0];
t_float delta_y = test_result[1] - positions[i][1];
t_float delta_z = test_result[2] - positions[i][2];
calc_distances[i] = sqrt(delta_x*delta_x + delta_y*delta_y + delta_z*delta_z);
if (calc_distances[i] > max_calc_dist) max_calc_dist = calc_distances[i];
}

// Calculate error with both absolute and relative components
for (int i = 0; i < 3; i++) {
t_float calc_tdoa = calc_distances[i+1] - calc_distances[0];
t_float abs_error = fabs(calc_tdoa - tdoa[i]);
error += abs_error + abs_error / (max_calc_dist + 1.0f);
}

if (phase == 1 && error < 0.0001) { // Tighter error threshold
goto search_complete;
if (error < min_error) {
min_error = error;
best_result[0] = test_result[0];
best_result[1] = test_result[1];
best_result[2] = test_result[2];
}
}
}

if (phase == 0) {
if (phase < 3) {
t_float best_r1 = calc_distances[0];
r1_start = best_r1 - r1_step;
r1_end = best_r1 + r1_step;
r1_step = 5;
t_float window_factor = (phase < 2) ? 5.0f : 3.0f;
r1_start = best_r1 - r1_step * window_factor;
r1_end = best_r1 + r1_step * window_factor;
r1_step *= 0.1; // Each phase reduces step size by factor of 10
}
}

search_complete:
result[0] = best_result[0];
result[1] = best_result[1];
result[2] = best_result[2];
Expand Down

0 comments on commit ab2251f

Please sign in to comment.