Illustrated C/C++ source sheets, symbol index, API relationship graph, compiler console and debug trace

LIVE AI / FLEXIBLE SENSING / FLIGHT CONTEXT

Sensoraia API

Live AI and sensing, connected to the flight.
See, listen, inspect and return supported results while airborne.

Loading...
Searching...
No Matches
ear_fusion.c
Go to the documentation of this file.
1/*
2 * Copyright 2026 OpenUAS
3 * SPDX-License-Identifier: Apache-2.0
4 *
5 * Licensed under the Apache License, Version 2.0 (the "License");
6 * you may not use this file except in compliance with the License.
7 * You may obtain a copy of the License at
8 *
9 * http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 */
17
18/**
19 * @file ear_fusion.c
20 * @brief Bounded ground-source localization from a geotagged acoustic survey.
21 * @details Relative band power supplies a conservative starting centroid;
22 * inverse-distance fitting improves it when survey geometry supports a fit.
23 * Constants below preserve the existing estimator policy. They are documented
24 * heuristics, not universal field-calibration values or flight-safety limits.
25 */
26#include "earcam_core.h"
27
28#include <math.h>
29#include <pthread.h>
30#include <string.h>
31
32#define EAR_MIN_SAMPLES 8U
33#define EARTH_RADIUS_M 6378137.0
34#define DEG_TO_RAD 0.017453292519943295
35#define RAD_TO_DEG 57.29577951308232
36
37/** Legacy near-zero-latitude placeholder gate; it also excludes equatorial tracks. */
38#define EAR_POSE_LATITUDE_EPSILON_DEG 1e-6
39/** Gaussian-consistent conversion from median absolute deviation to sigma. */
40#define EAR_MAD_TO_SIGMA 1.4826
41#define EAR_MIN_LEVEL_SIGMA_DB 0.5
42#define EAR_QUIET_OUTLIER_SIGMAS 4.0
43#define EAR_PEAK_QUARTILE_DIVISOR 4
44#define EAR_MIN_PEAK_WINDOWS 4
45#define EAR_DB_LOG_BASE 10.0
46#define EAR_POWER_DB_PER_DECADE 10.0
47#define EAR_DISTANCE_DB_PER_DECADE 20.0
48#define EAR_TREND_BONUS_LIMIT_DB 6.0
49#define EAR_TREND_BONUS_SCALE_DB 12.0
50/** Bound representative-height scratch space independently of survey size. */
51#define EAR_HEIGHT_WINDOW_LIMIT 64
52#define EAR_MIN_USABLE_HEIGHT_M 0.5
53#define EAR_FALLBACK_HEIGHT_M 10.0
54#define EAR_REFERENCE_DISTANCE_M 1.0
55/** Fit east, north and unknown source level; altitude is supplied per window. */
56#define EAR_FIT_PARAMETER_COUNT 3
57#define EAR_FIT_EAST 0
58#define EAR_FIT_NORTH 1
59#define EAR_FIT_SOURCE_LEVEL 2
60#define EAR_FIT_ITERATION_LIMIT 8
61#define EAR_FIT_RESIDUAL_LIMIT_DB 6.0
62#define EAR_FIT_DAMPING 1e-3
63#define EAR_FIT_DETERMINANT_EPSILON 1e-12
64#define EAR_FIT_HORIZONTAL_STEP_LIMIT_M 25.0
65#define EAR_FIT_LEVEL_STEP_LIMIT_DB 6.0
66#define EAR_FIT_CONVERGENCE_M 0.05
67#define EAR_FIT_CENTROID_DRIFT_LIMIT_M 60.0
68/** These three weights form a heuristic score, not calibrated detection odds. */
69#define EAR_CONFIDENCE_PROMINENCE_WEIGHT 0.5
70#define EAR_CONFIDENCE_CONCENTRATION_WEIGHT 0.3
71#define EAR_CONFIDENCE_SUPPORT_WEIGHT 0.2
72#define EAR_CONFIDENCE_PROMINENCE_SCALE_DB 12.0
73#define EAR_CONFIDENCE_SPREAD_SCALE_M 40.0
74#define EAR_CONFIDENCE_SUPPORT_WINDOWS 100.0
75
76/** @brief Restore a zero-based binary max heap without allocating scratch memory. */
77static void sift_down(double *values, size_t root, size_t count)
78{
79 for (;;) {
80 size_t child = root * 2 + 1;
81 if (child >= count) return;
82 if (child + 1 < count && values[child] < values[child + 1]) ++child;
83 if (values[root] >= values[child]) return;
84 const double temporary = values[root];
85 values[root] = values[child];
86 values[child] = temporary;
87 root = child;
88 }
89}
90
91/** @brief In-place heapsort keeps worst-case workspace bounded for a full survey. */
92static void sort_double(double *values, size_t count)
93{
94 for (size_t root = count / 2; root > 0; --root) sift_down(values, root - 1, count);
95 for (size_t end = count; end > 1; --end) {
96 const double temporary = values[0];
97 values[0] = values[end - 1];
98 values[end - 1] = temporary;
99 sift_down(values, 0, end - 1);
100 }
101}
102
103/** @brief Median of nonempty scratch values; sorting intentionally destroys their order. */
104static double median_of(double *values, size_t count)
105{
106 sort_double(values, count);
107 if (count % 2 == 1) {
108 return values[count / 2];
109 }
110 return 0.5 * (values[count / 2 - 1] + values[count / 2]);
111}
112
113/** @brief Solver-local horizontal coordinates plus each window's own squared height. */
115 double east;
116 double north;
117 double height2;
118 double level;
119 double weight;
120 const struct ear_sample *source;
121};
122
123static pthread_mutex_t solver_mutex = PTHREAD_MUTEX_INITIALIZER;
124/* One workspace avoids per-session heap or stack growth. Public calls serialize
125 * here; audio acquisition and USHER's UART thread do not run this computation. */
128
130 struct ear_loudest_spot *result)
131{
132 if (result == NULL) return -1;
133 memset(result, 0, sizeof(*result));
134 result->sample_count = (uint32_t)count;
136 return -1;
137 }
138 pthread_mutex_lock(&solver_mutex);
139
140 /* Prefer detector-confirmed windows only when enough survive. The fallback
141 * remains useful for unclassified tones, but is not an absolute silence gate. */
142 size_t alarm_count = 0;
143 for (size_t index = 0; index < count; index++) {
144 const struct ear_sample *sample = &samples[index];
145 if (sample->alarm && !sample->clipped && isfinite(sample->lat_deg)
146 && isfinite(sample->lon_deg) && fabs(sample->lat_deg) > EAR_POSE_LATITUDE_EPSILON_DEG) {
147 alarm_count++;
148 }
149 }
150 bool require_alarm = alarm_count >= EAR_MIN_SAMPLES;
151 struct enu_sample *points = solver_points;
152 double *levels = solver_levels;
153 size_t used = 0;
154 for (size_t index = 0; index < count; index++) {
155 const struct ear_sample *sample = &samples[index];
156 if (sample->clipped || !isfinite(sample->lat_deg) || !isfinite(sample->lon_deg)
157 || !isfinite(sample->level_db) || fabs(sample->lat_deg) <= EAR_POSE_LATITUDE_EPSILON_DEG
158 || (require_alarm && !sample->alarm)) {
159 continue;
160 }
161 points[used].source = sample;
162 points[used].level = sample->level_db;
163 levels[used] = sample->level_db;
164 used++;
165 }
166 if (used < EAR_MIN_SAMPLES) {
167 pthread_mutex_unlock(&solver_mutex);
168 return -1;
169 }
170
171 /* A one-sided Hampel gate removes implausibly quiet evidence without deleting
172 * the loud peak that the survey is trying to find. Preserve a noise-scale floor
173 * so nearly identical levels do not make the rejection threshold collapse. */
174 double median_level = median_of(levels, used);
175 for (size_t index = 0; index < used; index++) {
176 levels[index] = fabs(points[index].level - median_level);
177 }
178 double mad = median_of(levels, used) * EAR_MAD_TO_SIGMA;
179 if (mad < EAR_MIN_LEVEL_SIGMA_DB) {
181 }
182 size_t kept = 0;
183 for (size_t index = 0; index < used; index++) {
184 if (points[index].level < median_level - EAR_QUIET_OUTLIER_SIGMAS * mad) {
185 continue;
186 }
187 points[kept++] = points[index];
188 }
189 used = kept;
190 if (used < EAR_MIN_SAMPLES) {
191 pthread_mutex_unlock(&solver_mutex);
192 return -1;
193 }
194
195 size_t peak_index = 0;
196 for (size_t index = 1; index < used; index++) {
197 if (points[index].level > points[peak_index].level) {
198 peak_index = index;
199 }
200 }
201 /* Work in a local east/north plane around the strongest window. This is a
202 * small-survey approximation, not a global geodesic or terrain model. */
203 double origin_lat = points[peak_index].source->lat_deg;
204 double origin_lon = points[peak_index].source->lon_deg;
205 double cos_lat = cos(origin_lat * DEG_TO_RAD);
206 double max_level = points[peak_index].level;
207 for (size_t index = 0; index < used; index++) {
208 points[index].east = (points[index].source->lon_deg - origin_lon) * DEG_TO_RAD
209 * EARTH_RADIUS_M * cos_lat;
210 points[index].north = (points[index].source->lat_deg - origin_lat) * DEG_TO_RAD
212 }
213
214 for (size_t index = 0; index < used; index++) {
215 levels[index] = points[index].level;
216 }
217 sort_double(levels, used);
218 size_t top_count = used / EAR_PEAK_QUARTILE_DIVISOR;
219 if (top_count < EAR_MIN_PEAK_WINDOWS) {
220 top_count = used < EAR_MIN_PEAK_WINDOWS ? used : EAR_MIN_PEAK_WINDOWS;
221 }
222 double threshold = levels[used - top_count];
223 double sum_weight = 0.0;
224 double east0 = 0.0;
225 double north0 = 0.0;
226 for (size_t index = 0; index < used; index++) {
227 if (points[index].level < threshold) {
228 points[index].weight = 0.0;
229 continue;
230 }
231 /* Normalizing power to the loudest window avoids huge absolute weights.
232 * A bounded positive trend bonus mildly favors approaching the source. */
233 double weight = pow(EAR_DB_LOG_BASE, (points[index].level - max_level) / EAR_POWER_DB_PER_DECADE);
234 if (points[index].source->trend_db > 0.0) {
235 weight *= 1.0 + fmin(points[index].source->trend_db, EAR_TREND_BONUS_LIMIT_DB) / EAR_TREND_BONUS_SCALE_DB;
236 }
237 points[index].weight = weight;
238 sum_weight += weight;
239 east0 += weight * points[index].east;
240 north0 += weight * points[index].north;
241 }
242 if (sum_weight <= 0.0) {
243 pthread_mutex_unlock(&solver_mutex);
244 return -1;
245 }
246 east0 /= sum_weight;
247 north0 /= sum_weight;
248
249 /* A representative loud-window height is only a fallback and output summary.
250 * Each usable window keeps its own AGL in the fit, so changing survey height
251 * is not mistaken for a lateral change in source distance. */
252 double agl_values[EAR_HEIGHT_WINDOW_LIMIT];
253 size_t agl_count = 0;
254 for (size_t index = 0; index < used && agl_count < EAR_HEIGHT_WINDOW_LIMIT; index++) {
255 if (points[index].weight > 0.0 && isfinite(points[index].source->agl_m)
256 && points[index].source->agl_m > EAR_MIN_USABLE_HEIGHT_M) {
257 agl_values[agl_count++] = points[index].source->agl_m;
258 }
259 }
260 double height = agl_count > 0 ? median_of(agl_values, agl_count) : EAR_FALLBACK_HEIGHT_M;
261 for (size_t index = 0; index < used; index++) {
262 double h = points[index].source->agl_m;
263 if (!isfinite(h) || h < EAR_MIN_USABLE_HEIGHT_M) {
264 h = height;
265 }
266 points[index].height2 = h * h;
267 }
268 double east = east0;
269 double north = north0;
270 double peak_height = sqrt(points[peak_index].height2);
271 double level0 = max_level + EAR_DISTANCE_DB_PER_DECADE * log10(fmax(peak_height, EAR_REFERENCE_DISTANCE_M));
272 bool fit_ok = true;
273 /* Fit L = L0 - 10*log10(dx*dx + dy*dy + h*h). L0 absorbs unknown source
274 * strength/gain; soft residual weighting and diagonal damping stabilize the
275 * three-parameter normal equations when a track provides weak geometry. */
276 for (int iteration = 0; iteration < EAR_FIT_ITERATION_LIMIT; iteration++) {
278 double jtr[EAR_FIT_PARAMETER_COUNT] = {0};
279 for (size_t index = 0; index < used; index++) {
280 double dx = points[index].east - east;
281 double dy = points[index].north - north;
282 double r2 = dx * dx + dy * dy + points[index].height2;
283 double predicted = level0 - EAR_POWER_DB_PER_DECADE * log10(r2);
284 double residual = points[index].level - predicted;
285 double w = fabs(residual) > EAR_FIT_RESIDUAL_LIMIT_DB ? EAR_FIT_RESIDUAL_LIMIT_DB / fabs(residual) : 1.0;
286 double k = EAR_DISTANCE_DB_PER_DECADE / (r2 * log(EAR_DB_LOG_BASE));
287 double j[EAR_FIT_PARAMETER_COUNT] = {k * dx, k * dy, 1.0};
288 for (int a = 0; a < EAR_FIT_PARAMETER_COUNT; a++) {
289 jtr[a] += w * j[a] * residual;
290 for (int b = 0; b < EAR_FIT_PARAMETER_COUNT; b++) {
291 jtj[a][b] += w * j[a] * j[b];
292 }
293 }
294 }
295 for (int a = 0; a < EAR_FIT_PARAMETER_COUNT; a++) {
296 jtj[a][a] += EAR_FIT_DAMPING * (jtj[a][a] + 1.0);
297 }
298 double det = jtj[0][0] * (jtj[1][1] * jtj[2][2] - jtj[1][2] * jtj[2][1])
299 - jtj[0][1] * (jtj[1][0] * jtj[2][2] - jtj[1][2] * jtj[2][0])
300 + jtj[0][2] * (jtj[1][0] * jtj[2][1] - jtj[1][1] * jtj[2][0]);
301 if (!isfinite(det) || fabs(det) < EAR_FIT_DETERMINANT_EPSILON) {
302 fit_ok = false;
303 break;
304 }
305 /* The fixed 3x3 system uses Cramer's rule. Named dimensions and parameter
306 * offsets are intentionally fixed; adding a parameter needs a new solver. */
307 double delta[EAR_FIT_PARAMETER_COUNT];
308 for (int column = 0; column < EAR_FIT_PARAMETER_COUNT; column++) {
310 for (int row = 0; row < EAR_FIT_PARAMETER_COUNT; row++) {
311 for (int col = 0; col < EAR_FIT_PARAMETER_COUNT; col++) {
312 m[row][col] = (col == column) ? jtr[row] : jtj[row][col];
313 }
314 }
315 double d = m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1])
316 - m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0])
317 + m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]);
318 delta[column] = d / det;
319 }
320 double step = sqrt(delta[EAR_FIT_EAST] * delta[EAR_FIT_EAST] + delta[EAR_FIT_NORTH] * delta[EAR_FIT_NORTH]);
324 }
325 east += delta[EAR_FIT_EAST];
326 north += delta[EAR_FIT_NORTH];
328 if (step < EAR_FIT_CONVERGENCE_M) {
329 break;
330 }
331 }
332 /* An unconstrained fit can wander far outside a narrow/flat survey. Keep the
333 * robust seed rather than presenting that extrapolation as improved accuracy. */
334 double drift = hypot(east - east0, north - north0);
335 if (!fit_ok || !isfinite(east) || !isfinite(north) || drift > EAR_FIT_CENTROID_DRIFT_LIMIT_M) {
336 east = east0;
337 north = north0;
338 }
339
340 double prominence = fmin(1.0, fmax(0.0, (max_level - median_level) / EAR_CONFIDENCE_PROMINENCE_SCALE_DB));
341 double spread = 0.0;
342 for (size_t index = 0; index < used; index++) {
343 if (points[index].weight > 0.0) {
344 spread += points[index].weight * hypot(points[index].east - east, points[index].north - north);
345 }
346 }
347 spread /= sum_weight;
348 double concentration = fmin(1.0, fmax(0.0, 1.0 - spread / EAR_CONFIDENCE_SPREAD_SCALE_M));
349 double support = fmin(1.0, (double)used / EAR_CONFIDENCE_SUPPORT_WINDOWS);
350 double confidence = EAR_CONFIDENCE_PROMINENCE_WEIGHT * prominence
353
354 result->valid = true;
355 result->lat_deg = origin_lat + (north / EARTH_RADIUS_M) * RAD_TO_DEG;
356 result->lon_deg = origin_lon + (east / (EARTH_RADIUS_M * cos_lat)) * RAD_TO_DEG;
357 result->agl_m = height;
358 result->alt_m = points[peak_index].source->alt_m - sqrt(points[peak_index].height2);
359 result->level_db = max_level;
360 result->confidence = confidence;
361 result->used_count = (uint32_t)used;
362 pthread_mutex_unlock(&solver_mutex);
363 return 0;
364}
static pid_t child
#define EAR_MIN_PEAK_WINDOWS
Definition ear_fusion.c:44
#define EAR_TREND_BONUS_SCALE_DB
Definition ear_fusion.c:49
#define EAR_CONFIDENCE_SPREAD_SCALE_M
Definition ear_fusion.c:73
#define EAR_FIT_HORIZONTAL_STEP_LIMIT_M
Definition ear_fusion.c:64
#define DEG_TO_RAD
Definition ear_fusion.c:34
#define EAR_CONFIDENCE_SUPPORT_WEIGHT
Definition ear_fusion.c:71
#define EAR_POWER_DB_PER_DECADE
Definition ear_fusion.c:46
#define EAR_FIT_PARAMETER_COUNT
Definition ear_fusion.c:56
#define EAR_QUIET_OUTLIER_SIGMAS
Definition ear_fusion.c:42
#define EAR_HEIGHT_WINDOW_LIMIT
Definition ear_fusion.c:51
#define EAR_FIT_DETERMINANT_EPSILON
Definition ear_fusion.c:63
static void sift_down(double *values, size_t root, size_t count)
Restore a zero-based binary max heap without allocating scratch memory.
Definition ear_fusion.c:77
#define EAR_REFERENCE_DISTANCE_M
Definition ear_fusion.c:54
#define EAR_FALLBACK_HEIGHT_M
Definition ear_fusion.c:53
static pthread_mutex_t solver_mutex
Definition ear_fusion.c:123
static void sort_double(double *values, size_t count)
In-place heapsort keeps worst-case workspace bounded for a full survey.
Definition ear_fusion.c:92
#define EAR_FIT_LEVEL_STEP_LIMIT_DB
Definition ear_fusion.c:65
#define EAR_CONFIDENCE_PROMINENCE_SCALE_DB
Definition ear_fusion.c:72
#define EAR_CONFIDENCE_SUPPORT_WINDOWS
Definition ear_fusion.c:74
int calculated_loudestspot(const struct ear_sample *samples, size_t count, struct ear_loudest_spot *result)
Estimate a ground source from relative loudness along a survey track.
Definition ear_fusion.c:129
#define EAR_FIT_RESIDUAL_LIMIT_DB
Definition ear_fusion.c:61
#define EAR_FIT_SOURCE_LEVEL
Definition ear_fusion.c:59
#define EAR_MIN_SAMPLES
Definition ear_fusion.c:32
#define EAR_PEAK_QUARTILE_DIVISOR
Definition ear_fusion.c:43
#define RAD_TO_DEG
Definition ear_fusion.c:35
static struct enu_sample solver_points[EAR_MAX_SAMPLES]
Definition ear_fusion.c:126
#define EAR_FIT_EAST
Definition ear_fusion.c:57
#define EAR_FIT_NORTH
Definition ear_fusion.c:58
#define EAR_MIN_LEVEL_SIGMA_DB
Definition ear_fusion.c:41
#define EAR_DB_LOG_BASE
Definition ear_fusion.c:45
#define EAR_FIT_CENTROID_DRIFT_LIMIT_M
Definition ear_fusion.c:67
#define EAR_MIN_USABLE_HEIGHT_M
Definition ear_fusion.c:52
#define EARTH_RADIUS_M
Definition ear_fusion.c:33
#define EAR_CONFIDENCE_CONCENTRATION_WEIGHT
Definition ear_fusion.c:70
#define EAR_POSE_LATITUDE_EPSILON_DEG
Definition ear_fusion.c:38
#define EAR_FIT_DAMPING
Definition ear_fusion.c:62
#define EAR_FIT_CONVERGENCE_M
Definition ear_fusion.c:66
#define EAR_CONFIDENCE_PROMINENCE_WEIGHT
Definition ear_fusion.c:69
static double solver_levels[EAR_MAX_SAMPLES]
Definition ear_fusion.c:127
#define EAR_TREND_BONUS_LIMIT_DB
Definition ear_fusion.c:48
static double median_of(double *values, size_t count)
Median of nonempty scratch values; sorting intentionally destroys their order.
Definition ear_fusion.c:104
#define EAR_MAD_TO_SIGMA
Definition ear_fusion.c:40
#define EAR_DISTANCE_DB_PER_DECADE
Definition ear_fusion.c:47
#define EAR_FIT_ITERATION_LIMIT
Definition ear_fusion.c:60
static struct ear_sample samples[EAR_MAX_SAMPLES]
Sensor-independent acoustic samples and ground-source estimates.
#define EAR_MAX_SAMPLES
Definition earcam_core.h:34
uint8_t row[LWIR_MAX_WIDTH *3U]
Definition lwir_image.c:63
static size_t count
Definition pose_log.c:79
Estimated ground source, not a surveyed target or guaranteed alarm detection.
Definition earcam_core.h:37
One audio window associated with microphone position and provenance.
Definition earcam_core.h:55
double lat_deg
Definition earcam_core.h:58
double lon_deg
Definition earcam_core.h:59
double level_db
Definition earcam_core.h:62
Solver-local horizontal coordinates plus each window's own squared height.
Definition ear_fusion.c:114
double east
Definition ear_fusion.c:115
double weight
Definition ear_fusion.c:119
double level
Definition ear_fusion.c:118
const struct ear_sample * source
Definition ear_fusion.c:120
double height2
Definition ear_fusion.c:117
double north
Definition ear_fusion.c:116
struct usher_control_result result
Definition usher.c:377