Compare commits

...

5 Commits

Author SHA1 Message Date
ruv 2b5a32ae31 docs(changelog): record #1050 Observatory persons position/motion fix
Co-Authored-By: claude-flow <ruv@ruv.net>
2026-06-14 00:14:08 -04:00
ruv bca5bd515b fix(sensing-server): emit real field-derived person position/motion to /ws/sensing (#1050)
The Observatory 3D figure never animated because the sensing_update WS
frame carried no per-person position/motion_score/pose — only image-space
keypoints. The FigurePool/PoseSystem (and demo-data.js's own contract)
animate each figure from persons[i].position (room-world), .motion_score
(0..100), and .pose; none were on the live stream.

Honest scope (Case 2): the pipeline has no calibrated per-person room
localizer or per-person skeletal pose. New field_localize module extracts
the strongest peak(s) from the real signal_field grid (subcarrier
variances x motion-band power) and maps the peak cell to Observatory world
coords with the exact _buildSignalField transform. motion_score is the
measured motion_band_power passed through; pose is set only from a real
aggregate posture estimate, else None (never a fabricated skeleton).
Empty/below-threshold field -> persons: [] (no phantom); present person
with no resolvable peak keeps position [0,0,0], not invented coords.

attach_field_positions runs after the tracker step at all five broadcast
sites. New position/motion_score/pose fields added to both PersonDetection
structs. No UI change needed — the Observatory already reads these fields.

Tests: field_localize peak/coordinate/empty/separation units +
observatory_persons_field_position_tests (known-peak -> emitted position,
empty-room -> no phantom, pose real-or-None, below-threshold honesty).
sensing-server bin 441->451, 0 failed.

Co-Authored-By: claude-flow <ruv@ruv.net>
2026-06-14 00:13:00 -04:00
ruv 68432b4c9b docs: record ESP32 vitals count/presence fixes (#998, #996)
CHANGELOG [Unreleased] Fixed: root cause + fix + named constants + test
+ explicit hardware/data-gated caveat for both bugs.

ADR-021 Implementation Notes: dated 2026-06 entry noting the edge-path
person-count + presence-flicker fixes are boolean/count emission-logic
fixes, not a validated-accuracy claim; thresholds pending on-device
calibration.

Co-Authored-By: claude-flow <ruv@ruv.net>
2026-06-14 00:10:01 -04:00
ruv 8661b6f560 test(firmware): host C99 tests for vitals count + presence logic (#998, #996)
test/test_vitals_count_presence.c pins the two fixes with deterministic
host-buildable tests (no ESP-IDF needed). 13 cases / 22 assertions, all
passing under gcc 13 -Wall -Wextra:

  #998 count gate: single strong signature + multipath -> count==1;
  two well-separated -> 2; two strong-but-adjacent -> 1 (dedup);
  no signal -> 0; three well-separated -> 3.
  #998 debounce: transient spike rejected; sustained change accepted;
  flapping count stays stable.
  #996 presence: dithering trace -> stable flag (no flicker); brief dips
  held by clear-debounce; genuine departure clears within hold window;
  dead-band holds state.

The named tuning constants are #include'd from the real
edge_processing.h so the test and firmware can never disagree on
thresholds. `make run_vitals` / `make host_tests` added; binaries
gitignored.

Hardware-gated caveat documented in the test header: these pin the
decision LOGIC; the exact energy/separation/hysteresis values that best
match a real room vs labelled occupancy remain on-device tuning.

Co-Authored-By: claude-flow <ruv@ruv.net>
2026-06-14 00:09:57 -04:00
ruv 8416a4d337 fix(firmware): gate phantom persons + add presence hysteresis (#998, #996)
Two ESP32 edge-vitals logic bugs in edge_processing.c. Both are
robustness/logic fixes — NOT validated-accuracy claims. True count/PCK
vs labelled ground truth remains hardware/data-gated (COM9 ESP32-S3).

#998 — n_persons over-counted (reported 4 for one person):
update_multi_person_vitals() split top-K subcarriers into top_k_count/2
groups and marked EVERY group active, so one body's multipath always
read the full EDGE_MAX_PERSONS. Added two pure, host-testable helpers:
  - count_distinct_persons(): per-group energy gate
    (EDGE_PERSON_MIN_ENERGY_RATIO) + spatial dedup
    (EDGE_PERSON_MIN_SC_SEP) so weak/adjacent multipath groups don't
    count as separate bodies. Strongest group always counts (>=1).
  - person_count_debounce(): a gated count must hold
    EDGE_PERSON_PERSIST_FRAMES consecutive frames before it's emitted,
    so a single noisy frame can't promote a phantom.
The active flags now mark only the strongest stable_count groups.

#996 — presence flag flickered at ~50cm despite high presence_score:
the bare `score > threshold` compare chattered on a noisy score
(field-observed 2.6-26.7 frame-to-frame). Replaced with a Schmitt
trigger + clear-debounce (presence_flag_update): assert above
threshold, hold in the dead band down to threshold *
EDGE_PRESENCE_HYST_RATIO, clear only after EDGE_PRESENCE_CLEAR_FRAMES
consecutive sub-low frames. presence_score itself is unchanged and
still emitted for consumer-side thresholding.

All thresholds are named, documented constants in edge_processing.h.
Firmware builds clean for esp32s3 (idf.py build RC=0).

Co-Authored-By: claude-flow <ruv@ruv.net>
2026-06-14 00:09:57 -04:00
12 changed files with 1215 additions and 9 deletions
+9
View File
@@ -16,6 +16,15 @@ firmware/esp32-csi-node/sdkconfig.defaults.bak
# ESP-IDF set-target backup (local only)
firmware/esp32-hello-world/sdkconfig.old
# Host-built firmware test binaries (compiled from test/*.c, not source)
firmware/esp32-csi-node/test/test_adr110
firmware/esp32-csi-node/test/test_vitals
firmware/esp32-csi-node/test/fuzz_serialize
firmware/esp32-csi-node/test/fuzz_edge
firmware/esp32-csi-node/test/fuzz_nvs
firmware/esp32-csi-node/test/*.exe
firmware/esp32-csi-node/test/*.obj
# Claude Flow swarm runtime state
.swarm/
+5
View File
@@ -15,6 +15,11 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
- **#3 JWT-in-URL (CWE-598) — VERIFIED ABSENT, regression-pinned.** `require_bearer` reads the token only from the `Authorization` header; the WebSocket handlers take no token query param and the sole `Query` extractor (`EdgeRegistryParams`) is a non-secret `refresh` flag. Added a regression proving `?token=`/`?access_token=` in the URL never authenticates while the header path still does.
### Fixed
- **ESP32 vitals: `n_persons` over-counted (reported 4 for one person) + presence flag flickered at close range (#998, #996).** Two firmware logic bugs in `firmware/esp32-csi-node/main/edge_processing.c`, both robustness/logic fixes — **not** validated-accuracy claims (true count/PCK vs labelled ground truth stays hardware/data-gated on the COM9 ESP32-S3).
- **#998 over-count — root cause + fix.** `update_multi_person_vitals()` split the top-K subcarriers into `top_k_count/2` groups and marked **every** group `active` unconditionally, so one body's multipath always reported the full `EDGE_MAX_PERSONS` (=4). New pure, host-testable `count_distinct_persons()` gates each candidate group: (1) **energy gate** — a group's phase variance must be ≥ `EDGE_PERSON_MIN_ENERGY_RATIO` (0.35) × the strongest group's, so weak multipath echoes don't count; (2) **spatial dedup** — groups whose representative subcarriers sit within `EDGE_PERSON_MIN_SC_SEP` (4) of each other are the same body. A `person_count_debounce()` then requires the gated count to hold `EDGE_PERSON_PERSIST_FRAMES` (3) consecutive frames before it's emitted, so a single noisy frame can't promote a phantom. The strongest group always counts (a present body yields ≥1). All thresholds are named, documented constants in `edge_processing.h`.
- **#996 presence flicker — root cause + fix.** Presence was a bare `score > threshold` compare on a noisy `presence_score` (field-observed 2.626.7 frame-to-frame for one stationary person), so the boolean chattered at the boundary while the score clearly indicated a person. New pure `presence_flag_update()` is a Schmitt trigger + clear-debounce: assert above `threshold`, **hold** in the dead band down to `threshold × EDGE_PRESENCE_HYST_RATIO` (0.5), and only clear after the score stays below the low threshold for `EDGE_PRESENCE_CLEAR_FRAMES` (5) consecutive frames. The score itself is unchanged (and still emitted at packet offset 20 for consumer-side thresholding). Constants named/documented in `edge_processing.h`.
- **Tests:** `firmware/esp32-csi-node/test/test_vitals_count_presence.c` (host C99, `make run_vitals`) — 13 cases / 22 assertions, all passing under gcc 13 `-Wall -Wextra`. Pins: single-strong-signature + multipath → count==1; two well-separated → count==2; two strong-but-adjacent → 1 (dedup); transient count spike rejected; sustained change accepted; dithering presence trace → stable flag (no flicker); genuine departure → clears within hold window. The named tuning constants are `#include`d from the real header so the test and firmware can't disagree. **Hardware-gated caveat:** these pin the decision *logic*; the exact energy/separation/hysteresis values that best match a real room vs labelled occupancy remain on-device tuning (COM9 ESP32-S3 + ground truth).
- **Observatory 3D figure never animated — `/ws/sensing` omitted per-person `position`/`motion_score`/`pose` (#1050).** The `sensing_update` frame shipped `nodes`/`features`/`classification`/`signal_field` and a `persons[]` carrying only image-space `keypoints`/`bbox`/`zone`; the Observatory's `FigurePool`/`PoseSystem` (and `demo-data.js`'s own contract) animate each figure from `persons[i].position` (room-world `[x,y,z]`), `persons[i].motion_score` (0..100), and `persons[i].pose`, none of which the live stream emitted — so the figure sat static while signal metrics updated. **Honest scope (Case 2 — no calibrated per-person localizer exists):** a single ESP32 link does not produce calibrated room-coordinate localization or per-person skeletal pose, so the fix emits only what is *truthfully derivable*. New `field_localize` module reads the **strongest peak(s)** out of the frame's real `signal_field` grid (already built from measured subcarrier variances × measured motion-band power) and maps the peak cell to Observatory world coordinates with the **exact** `_buildSignalField` transform (`x=(ixnx/2)·0.6`, `z=(iznz/2)·0.5`, `y=0`), so the figure lands on the field hotspot it stands on. `motion_score` is the measured `motion_band_power` passed through (clamped 0..100); `pose` is set **only** from a real aggregate `posture` estimate when one exists, else `None` (never a fabricated skeleton — per-person pose keypoints in room coordinates stay gated on the pose model + ADR-079 paired data). An empty / below-threshold field yields `persons: []` (no phantom person); a present person on a field with no resolvable peak keeps `position=[0,0,0]` (not invented coords) while `motion_score` stays real. `attach_field_positions` runs after the tracker step at all five broadcast sites. **No UI change required** — the Observatory already reads these fields and defaults `pose``'standing'` when absent. New `PersonDetection.position`/`motion_score`/`pose` fields added to both the `main.rs`-local and `types.rs` structs. Pinned by 10 tests: `field_localize` peak-extraction/coordinate-mapping/empty-field/separation unit tests + `observatory_persons_field_position_tests` (`sensing_update_emits_persons_with_field_derived_position` feeds a synthetic field with a known peak at cell (15,4) and asserts the emitted `position` = `[3.0, 0, 3.0]` within tolerance; `empty_room_yields_no_phantom_person`; `pose_is_real_when_posture_present_and_absent_otherwise`; `present_but_below_threshold_field_keeps_position_at_origin_not_fabricated`). `wifi-densepose-sensing-server --no-default-features`: bin **441→451**, 0 failed; workspace green; Python proof unchanged (off the deterministic proof path).
- **ADR-155 Milestone-1b — metric-definition unification, the §8 backlog subset (Goals A/B/C).** Closed the two §8 metric-integrity items; every change pinned by a test, graded MEASURED. The audit (Goal A) also surfaced findings the §1 table under-counted — recorded honestly in ADR-155 §8.1, not hidden. Workspace stays green; Python proof unchanged (metrics are not on the deterministic proof's signal path).
- **Goal B — `test_metrics.rs` now validates the production metric, not a reimplementation.** The integration test previously asserted properties of its OWN local `compute_pck`/`compute_oks` (a test that can't catch a canonical-impl bug — both could be wrong the same way). Hoisted the canonical core (`pck_canonical`/`oks_canonical`/`canonical_torso_size`/sigmas/`bounding_box_diagonal`) into a new **un-gated** `metrics_core` module so the single definition is reachable under `cargo test --no-default-features` (the `metrics` module is `tch-backend`-gated); `metrics` re-exports it → still exactly ONE implementation. Rewrote the test to assert the production `pck_canonical`/`oks_canonical` equal **hand-computed** fixtures (`canonical_pck_matches_hand_computed_fixture` = 3/4 correct ⇒ 0.75; hip↔hip normalizer pin; zero-visible⇒0.0; OKS perfect⇒1.0; fake-Gold pin) plus a differential cross-check (`test_kernel_agrees_with_canonical`: an independent raw-threshold kernel must AGREE with canonical where torso==1.0). `wifi-densepose-train --no-default-features`: test_metrics **10→12**, 0 failed.
- **Goal C — divergent live-server PCK/OKS relabelled so they're never conflated with canonical.** Goal C named `training_api.rs:804` (torso-HEIGHT PCK); the audit found that file is an **orphan (not `mod`-declared, does not compile)** and the **real** live `best_pck`/`best_oks` come from `trainer.rs` — a **raw, unnormalized** `pck_at_threshold` and an **`area=1.0` fake-Gold** `oks_map` (both MISSED by ADR-155 §1, both on the claim-inflating side, both serialized as bare "PCK@0.2"/"OKS"). Torso-height/raw math is load-bearing (pixel-space, different scale axis, no `ndarray`/train dep), so the honest fix is **relabel, not force-unify**: `training_api.rs` `compute_pck``compute_pck_torso_height` + field/log docs; `trainer.rs` kernels documented raw/fake-Gold; `main.rs` prints `pck_raw@0.2` / `oks_map(area=1.0 proxy)`. No wire-format field or `pub`-fn renames (no silent API break). Pinned by `torso_pck_is_labelled_distinctly_from_canonical` + `pck_at_threshold_is_raw_unnormalized_not_canonical`. `wifi-densepose-sensing-server --no-default-features`: lib **450→451**, 0 failed. True unification onto `pck_canonical`/`oks_canonical` remains a tracked ADR-155 §8 item.
@@ -1081,6 +1081,17 @@ The `wifi-densepose-vitals` crate (ESP32 CSI-grade vital signs) has not yet been
- SONA-based environment adaptation
- VitalSignStore with tiered temporal compression
## Implementation Notes
### 2026-06 — ESP32 edge vitals: person-count over-count + presence flicker (#998, #996)
Two robustness bugs were fixed in the on-device edge path (`firmware/esp32-csi-node/main/edge_processing.c`, the ADR-039 packet `0xC5110002`). These touch the *boolean/count emission logic*, not the underlying CSI signal-processing math, and do **not** constitute a validated-accuracy claim — true occupancy-count and presence accuracy vs labelled ground truth remain hardware/data-gated (COM9 ESP32-S3 + labelled capture).
- **#998 `n_persons` over-count (reported 4 for one person).** `update_multi_person_vitals()` divided the top-K subcarriers into `top_k_count/2` groups and marked *every* group `active`, so one body's multipath always read the full `EDGE_MAX_PERSONS`. Added an energy gate (`EDGE_PERSON_MIN_ENERGY_RATIO`), spatial dedup (`EDGE_PERSON_MIN_SC_SEP`), and a persistence debounce (`EDGE_PERSON_PERSIST_FRAMES`) via two pure functions `count_distinct_persons()` / `person_count_debounce()`.
- **#996 presence flag flicker at ~50 cm.** Single-threshold compare on a noisy `presence_score` chattered at the boundary. Replaced with a Schmitt trigger + clear-debounce (`presence_flag_update()`, constants `EDGE_PRESENCE_HYST_RATIO` / `EDGE_PRESENCE_CLEAR_FRAMES`); `presence_score` is unchanged and still emitted for consumer-side thresholding.
Both are pinned by host-buildable C99 tests in `firmware/esp32-csi-node/test/test_vitals_count_presence.c` (`make run_vitals`). The exact thresholds are documented constants pending on-device calibration against ground truth.
## References
- Ramsauer et al. (2020). "Hopfield Networks is All You Need." ICLR 2021. (ModernHopfield formulation)
+217 -4
View File
@@ -367,6 +367,7 @@ static float s_heartrate_bpm;
static float s_motion_energy;
static float s_presence_score;
static bool s_presence_detected;
static uint8_t s_presence_below_count; /**< Consecutive frames below low thresh (issue #996). */
static bool s_fall_detected;
static int8_t s_latest_rssi;
static uint32_t s_frame_count;
@@ -398,6 +399,11 @@ static uint16_t s_feature_seq;
/** Multi-person vitals state. */
static edge_person_vitals_t s_persons[EDGE_MAX_PERSONS];
/** Person-count persistence debounce (issue #998). */
static uint8_t s_person_count_candidate; /**< Last raw (gated) candidate count. */
static uint8_t s_person_count_streak; /**< Consecutive frames at the candidate. */
static uint8_t s_person_count_stable; /**< Emitted (debounced) count. */
static edge_biquad_t s_person_bq_br[EDGE_MAX_PERSONS];
static edge_biquad_t s_person_bq_hr[EDGE_MAX_PERSONS];
static float s_person_br_filt[EDGE_MAX_PERSONS][EDGE_PHASE_HISTORY_LEN];
@@ -446,6 +452,61 @@ static void update_top_k(uint16_t n_subcarriers)
s_top_k_count = k;
}
/* ======================================================================
* Presence Flag Hysteresis + Debounce (issue #996)
* ====================================================================== */
/**
* Schmitt-trigger presence decision with a clear-debounce.
*
* Pure function (no globals) so it is host-testable: feed a presence_score
* trace and assert the boolean flag is stable. Replaces the old single-
* threshold `score > threshold` compare that chattered when a noisy score
* dithered around the boundary (observed 2.6-26.7 for one stationary person).
*
* - score > threshold → assert presence (enter immediately)
* - score >= threshold * HYST_RATIO → hold current state (dead band)
* - score < threshold * HYST_RATIO → count toward clearing; only clear
* after CLEAR_FRAMES consecutive frames
*
* @param prev Current presence flag (in/out via return + below_count).
* @param score Latest presence score.
* @param threshold High (enter) threshold.
* @param below_count In/out: consecutive frames the score has been below the
* low threshold. Reset to 0 whenever the score recovers.
* @return New presence flag.
*/
static bool presence_flag_update(bool prev, float score, float threshold,
uint8_t *below_count)
{
float low_thresh = threshold * EDGE_PRESENCE_HYST_RATIO;
if (score > threshold) {
/* Clearly present — assert and reset the clear debounce. */
*below_count = 0;
return true;
}
if (score >= low_thresh) {
/* Dead band: hold whatever we had, no flicker. Recovery above the low
* threshold also resets the clear debounce so a brief dip doesn't
* accumulate toward a false clear. */
*below_count = 0;
return prev;
}
/* Below the low threshold — candidate for clearing. */
if (*below_count < 0xFF) (*below_count)++;
if (!prev) {
return false; /* Already cleared. */
}
if (*below_count >= EDGE_PRESENCE_CLEAR_FRAMES) {
*below_count = 0;
return false; /* Sustained absence — clear. */
}
return true; /* Still within the hold window — keep asserting. */
}
/* ======================================================================
* Adaptive Presence Calibration
* ====================================================================== */
@@ -581,6 +642,112 @@ store_prev:
* Multi-Person Vitals
* ====================================================================== */
/**
* Count distinct persons from per-group energy + representative subcarrier (issue #998).
*
* Pure function (no globals) so it is host-testable. Each of the `n_groups`
* subcarrier groups is a *candidate* person. A candidate is counted only if:
* 1. Energy gate — its energy >= EDGE_PERSON_MIN_ENERGY_RATIO * max energy.
* One body's multipath spreads energy unevenly across the
* groups; weak groups are reflections, not extra people.
* 2. Spatial dedup — its representative subcarrier is at least
* EDGE_PERSON_MIN_SC_SEP away from every already-counted
* person. Adjacent subcarriers see the same reflection, so
* a near-duplicate group is the same body.
*
* The strongest group is always counted (so a present body yields >= 1).
*
* @param energy Per-group energy (e.g. phase variance), length n_groups.
* @param sc_idx Per-group representative subcarrier index, length n_groups.
* @param n_groups Number of candidate groups (<= EDGE_MAX_PERSONS).
* @return Distinct person count in [0, n_groups].
*/
static uint8_t count_distinct_persons(const float *energy, const uint8_t *sc_idx,
uint8_t n_groups)
{
if (n_groups == 0) return 0;
/* Strongest group sets the reference energy. */
float max_energy = 0.0f;
for (uint8_t g = 0; g < n_groups; g++) {
if (energy[g] > max_energy) max_energy = energy[g];
}
/* No real signal anywhere → no persons. */
if (max_energy <= 0.0f) return 0;
float min_energy = max_energy * EDGE_PERSON_MIN_ENERGY_RATIO;
uint8_t counted_sc[EDGE_MAX_PERSONS];
uint8_t count = 0;
/* Greedy by descending energy: take the strongest unclaimed group that is
* spatially separated from everything already counted. */
bool used[EDGE_MAX_PERSONS];
for (uint8_t g = 0; g < n_groups && g < EDGE_MAX_PERSONS; g++) used[g] = false;
for (uint8_t iter = 0; iter < n_groups && iter < EDGE_MAX_PERSONS; iter++) {
/* Find the strongest still-unused group above the energy gate. */
int best = -1;
float best_e = min_energy; /* must beat the gate */
for (uint8_t g = 0; g < n_groups && g < EDGE_MAX_PERSONS; g++) {
if (used[g]) continue;
if (energy[g] >= best_e) { best_e = energy[g]; best = g; }
}
if (best < 0) break; /* nothing left above the gate */
used[best] = true;
/* Spatial dedup against already-counted persons. */
bool duplicate = false;
for (uint8_t c = 0; c < count; c++) {
int sep = (int)sc_idx[best] - (int)counted_sc[c];
if (sep < 0) sep = -sep;
if (sep < EDGE_PERSON_MIN_SC_SEP) { duplicate = true; break; }
}
if (duplicate) continue;
counted_sc[count++] = sc_idx[best];
}
/* The strongest group always represents at least one body. */
if (count == 0) count = 1;
return count;
}
/**
* Debounce a raw person count so a single noisy frame can't change the emitted
* value (issue #998). A new candidate must hold for EDGE_PERSON_PERSIST_FRAMES
* consecutive frames before it replaces the stable count.
*
* Pure function (state passed by pointer) → host-testable.
*
* @param raw Raw (gated) count this frame.
* @param candidate In/out: the candidate being accumulated.
* @param streak In/out: consecutive frames the candidate has held.
* @param stable In/out: the currently emitted count.
* @return The (possibly updated) stable count.
*/
static uint8_t person_count_debounce(uint8_t raw, uint8_t *candidate,
uint8_t *streak, uint8_t *stable)
{
if (raw == *stable) {
/* Agrees with what we emit — reset any pending change. */
*candidate = raw;
*streak = 0;
return *stable;
}
if (raw == *candidate) {
if (*streak < 0xFF) (*streak)++;
} else {
*candidate = raw;
*streak = 1;
}
if (*streak >= EDGE_PERSON_PERSIST_FRAMES) {
*stable = *candidate;
*streak = 0;
}
return *stable;
}
/**
* Update multi-person vitals by assigning top-K subcarriers to person groups.
*
@@ -600,10 +767,25 @@ static void update_multi_person_vitals(const uint8_t *iq_data, uint16_t n_sc,
uint8_t subs_per_person = s_top_k_count / n_persons;
/* Per-group energy + representative subcarrier, for the #998 person gate. */
float group_energy[EDGE_MAX_PERSONS] = {0};
uint8_t group_sc[EDGE_MAX_PERSONS] = {0};
for (uint8_t p = 0; p < n_persons; p++) {
edge_person_vitals_t *pv = &s_persons[p];
pv->active = true;
pv->subcarrier_idx = s_top_k[p * subs_per_person];
group_sc[p] = s_top_k[p * subs_per_person];
/* Group energy = max Welford variance over its subcarriers. This is the
* same variance used for top-K selection, so a multipath group (weak,
* adjacent to the strong one) registers low energy and gets gated out. */
float energy = 0.0f;
for (uint8_t s = 0; s < subs_per_person; s++) {
uint8_t sc = s_top_k[p * subs_per_person + s];
float v = (float)welford_variance(&s_subcarrier_var[sc]);
if (v > energy) energy = v;
}
group_energy[p] = energy;
/* Average phase across this person's subcarrier group. */
float avg_phase = 0.0f;
@@ -662,10 +844,32 @@ static void update_multi_person_vitals(const uint8_t *iq_data, uint16_t n_sc,
}
}
/* Mark remaining persons as inactive. */
for (uint8_t p = n_persons; p < EDGE_MAX_PERSONS; p++) {
/* --- Issue #998: gate phantom persons by energy + spatial dedup,
* then debounce so a single noisy frame can't change the count. --- */
uint8_t raw_count = count_distinct_persons(group_energy, group_sc, n_persons);
uint8_t stable_count = person_count_debounce(raw_count,
&s_person_count_candidate,
&s_person_count_streak,
&s_person_count_stable);
/* Mark the strongest `stable_count` groups active (descending energy); the
* rest — including phantom multipath groups — are inactive. */
bool used[EDGE_MAX_PERSONS];
for (uint8_t p = 0; p < EDGE_MAX_PERSONS; p++) {
used[p] = false;
s_persons[p].active = false;
}
for (uint8_t n = 0; n < stable_count && n < n_persons; n++) {
int best = -1;
float best_e = -1.0f;
for (uint8_t p = 0; p < n_persons; p++) {
if (used[p]) continue;
if (group_energy[p] > best_e) { best_e = group_energy[p]; best = p; }
}
if (best < 0) break;
used[best] = true;
s_persons[best].active = true;
}
}
/* ======================================================================
@@ -960,7 +1164,12 @@ static void process_frame(const edge_ring_slot_t *slot)
} else if (threshold == 0.0f) {
threshold = 0.05f; /* Default until calibrated. */
}
s_presence_detected = (s_presence_score > threshold);
/* Issue #996: hysteresis + clear-debounce instead of a bare threshold
* compare, so a noisy score dithering around the boundary doesn't flicker
* the boolean flag. */
s_presence_detected = presence_flag_update(s_presence_detected,
s_presence_score, threshold,
&s_presence_below_count);
/* --- Step 10: Fall detection (phase acceleration + debounce, issue #263) --- */
if (s_history_len >= 3) {
@@ -1160,6 +1369,7 @@ esp_err_t edge_processing_init(const edge_config_t *cfg)
s_motion_energy = 0.0f;
s_presence_score = 0.0f;
s_presence_detected = false;
s_presence_below_count = 0;
s_fall_detected = false;
s_latest_rssi = 0;
s_frame_count = 0;
@@ -1183,6 +1393,9 @@ esp_err_t edge_processing_init(const edge_config_t *cfg)
for (uint8_t p = 0; p < EDGE_MAX_PERSONS; p++) {
s_persons[p].active = false;
}
s_person_count_candidate = 0;
s_person_count_streak = 0;
s_person_count_stable = 0;
/* Design biquad bandpass filters.
* Sampling rate ~20 Hz (typical ESP32 CSI callback rate). */
@@ -38,6 +38,30 @@
/* ---- Multi-person ---- */
#define EDGE_MAX_PERSONS 4 /**< Max simultaneous persons. */
/* ---- Multi-person counting gates (issue #998) ----
*
* Over-counting root cause: the multi-person path used to split the top-K
* subcarriers into EDGE_MAX_PERSONS groups and mark EVERY group active,
* so one body's multipath always reported the full EDGE_MAX_PERSONS. These
* gates promote a subcarrier group to a real "person" only when it carries
* genuine, distinct, persistent energy:
*
* 1. Energy gate — a group's phase variance must exceed a fraction of the
* strongest group's variance, else it is multipath/noise.
* 2. Spatial dedup — two groups whose representative subcarriers sit within
* EDGE_PERSON_MIN_SC_SEP of each other are the same body
* (adjacent subcarriers see correlated reflections), so
* the weaker one is merged away.
* 3. Persistence — a candidate count must hold for EDGE_PERSON_PERSIST_FRAMES
* consecutive decisions before it is emitted, so a single
* noisy frame cannot promote a phantom person.
*
* These are robustness gates on the existing heuristic, not a calibrated
* occupancy model — true count accuracy vs ground truth remains data-gated. */
#define EDGE_PERSON_MIN_ENERGY_RATIO 0.35f /**< Group var must be >= this * max group var to count. */
#define EDGE_PERSON_MIN_SC_SEP 4 /**< Min subcarrier separation between distinct persons. */
#define EDGE_PERSON_PERSIST_FRAMES 3 /**< Consecutive decisions a count must hold before emit. */
/* ---- Calibration ---- */
#define EDGE_CALIB_FRAMES 1200 /**< Frames for adaptive calibration (~60s at 20 Hz). */
#define EDGE_CALIB_SIGMA_MULT 3.0f /**< Threshold = mean + 3*sigma of ambient. */
@@ -46,6 +70,27 @@
#define EDGE_FALL_COOLDOWN_MS 5000 /**< Minimum ms between fall alerts (debounce). */
#define EDGE_FALL_CONSEC_MIN 3 /**< Consecutive frames above threshold to trigger. */
/* ---- Presence flag hysteresis + debounce (issue #996) ----
*
* Flicker root cause: the presence flag was a single-threshold compare on a
* noisy presence_score (observed 2.6-26.7 frame-to-frame for one stationary
* person), so the boolean chattered at the boundary even while the score
* clearly indicated a person. Fix: Schmitt-trigger hysteresis plus a clear
* debounce.
*
* - Assert presence when score > threshold (enter immediately).
* - Hold presence while score >= threshold * HYST_RATIO (no flicker in the
* gap band).
* - Clear presence only after the score stays below the low threshold for
* EDGE_PRESENCE_CLEAR_FRAMES consecutive frames (genuine departure).
*
* HYST_RATIO < 1.0 sets the low threshold below the high threshold; a wider gap
* (smaller ratio) is more flicker-immune but slower to clear on real exit. The
* exact ratio that best matches a given room's score scale remains an on-device
* tuning parameter — this removes the logic bug (no hysteresis at all). */
#define EDGE_PRESENCE_HYST_RATIO 0.5f /**< Low thresh = HYST_RATIO * high thresh. */
#define EDGE_PRESENCE_CLEAR_FRAMES 5 /**< Frames below low thresh before clearing. */
/* ---- DSP task tuning ---- */
#define EDGE_BATCH_LIMIT 4 /**< Max frames per batch before longer yield. */
+17 -5
View File
@@ -43,9 +43,10 @@ MAIN_DIR = ../main
FUZZ_DURATION ?= 30
FUZZ_JOBS ?= 1
.PHONY: all clean run_serialize run_edge run_nvs run_all test_adr110 run_adr110 host_tests
.PHONY: all clean run_serialize run_edge run_nvs run_all test_adr110 run_adr110 \
test_vitals run_vitals host_tests
all: fuzz_serialize fuzz_edge fuzz_nvs test_adr110
all: fuzz_serialize fuzz_edge fuzz_nvs test_adr110 test_vitals
# --- ADR-110 encoding unit tests ---
# Host-side, no libFuzzer needed — plain C99 deterministic table tests
@@ -57,8 +58,19 @@ test_adr110: test_adr110_encoding.c
run_adr110: test_adr110
./test_adr110
host_tests: run_adr110
@echo "ADR-110 host tests passed"
# --- Vitals count + presence logic unit tests (issue #998 / #996) ---
# Host-side, no libFuzzer. Pins the person-count gate (no over-count for one
# body) and the presence hysteresis (no flicker on a dithering score). Pulls
# the named tuning constants from ../main/edge_processing.h so the test and the
# firmware can never disagree on thresholds.
test_vitals: test_vitals_count_presence.c $(MAIN_DIR)/edge_processing.h
cc -std=c99 -Wall -Wextra -Istubs -I$(MAIN_DIR) -o $@ $< -lm
run_vitals: test_vitals
./test_vitals
host_tests: run_adr110 run_vitals
@echo "Host tests passed (ADR-110 + vitals #998/#996)"
# --- Serialize fuzzer ---
# Tests csi_serialize_frame() with random wifi_csi_info_t inputs.
@@ -94,5 +106,5 @@ run_nvs: fuzz_nvs
run_all: run_serialize run_edge run_nvs
clean:
rm -f fuzz_serialize fuzz_edge fuzz_nvs test_adr110
rm -f fuzz_serialize fuzz_edge fuzz_nvs test_adr110 test_vitals
rm -rf corpus_serialize/ corpus_edge/ corpus_nvs/
@@ -0,0 +1,387 @@
/**
* @file test_vitals_count_presence.c
* @brief Host-side unit tests for the issue #998 / #996 vitals logic fixes.
*
* Covers two pure decision functions extracted from edge_processing.c:
* 1. count_distinct_persons() — issue #998 person over-count gate
* (energy gate + spatial dedup).
* 2. person_count_debounce() — issue #998 count persistence debounce.
* 3. presence_flag_update() — issue #996 presence hysteresis + clear
* debounce (Schmitt trigger).
*
* Build (Linux/macOS/Windows with any C99 compiler):
* cc -std=c99 -Wall -I../main -o test_vitals \
* test_vitals_count_presence.c && ./test_vitals
*
* Exits 0 on all-pass, prints which assertion failed otherwise.
*
* Why a separate host test file: these are deterministic logic checks for the
* exact boundary behaviour the issues describe; libFuzzer adds no signal here.
*
* IMPORTANT — these three functions are copied VERBATIM from
* firmware/esp32-csi-node/main/edge_processing.c. They are pure (no globals,
* no ESP-IDF). If the firmware copy changes, update the copy here and re-run
* this test before the firmware change merges. The named tuning constants are
* pulled from the real header so the test and firmware can never disagree on
* thresholds.
*
* HARDWARE-GATED CAVEAT: these tests pin the *logic* (no flicker / no
* over-count for the synthetic traces). True count accuracy and the exact
* energy/separation/hysteresis thresholds that best match a real room vs
* labelled ground truth remain hardware- and data-gated (COM9 ESP32-S3 +
* labelled occupancy). This is a robustness/logic fix, not a validated
* accuracy claim.
*/
#include <stdint.h>
#include <stdbool.h>
#include <stdio.h>
/* Named tuning constants come from the real firmware header so the test can
* never silently diverge from the constants the firmware compiles with. */
#include "edge_processing.h"
/* ──────────────────────────────────────────────────────────────────────
* System under test — copied VERBATIM from edge_processing.c.
* ────────────────────────────────────────────────────────────────────── */
/* count_distinct_persons() — issue #998 energy gate + spatial dedup. */
static uint8_t count_distinct_persons(const float *energy, const uint8_t *sc_idx,
uint8_t n_groups)
{
if (n_groups == 0) return 0;
float max_energy = 0.0f;
for (uint8_t g = 0; g < n_groups; g++) {
if (energy[g] > max_energy) max_energy = energy[g];
}
if (max_energy <= 0.0f) return 0;
float min_energy = max_energy * EDGE_PERSON_MIN_ENERGY_RATIO;
uint8_t counted_sc[EDGE_MAX_PERSONS];
uint8_t count = 0;
bool used[EDGE_MAX_PERSONS];
for (uint8_t g = 0; g < n_groups && g < EDGE_MAX_PERSONS; g++) used[g] = false;
for (uint8_t iter = 0; iter < n_groups && iter < EDGE_MAX_PERSONS; iter++) {
int best = -1;
float best_e = min_energy;
for (uint8_t g = 0; g < n_groups && g < EDGE_MAX_PERSONS; g++) {
if (used[g]) continue;
if (energy[g] >= best_e) { best_e = energy[g]; best = g; }
}
if (best < 0) break;
used[best] = true;
bool duplicate = false;
for (uint8_t c = 0; c < count; c++) {
int sep = (int)sc_idx[best] - (int)counted_sc[c];
if (sep < 0) sep = -sep;
if (sep < EDGE_PERSON_MIN_SC_SEP) { duplicate = true; break; }
}
if (duplicate) continue;
counted_sc[count++] = sc_idx[best];
}
if (count == 0) count = 1;
return count;
}
/* person_count_debounce() — issue #998 count persistence. */
static uint8_t person_count_debounce(uint8_t raw, uint8_t *candidate,
uint8_t *streak, uint8_t *stable)
{
if (raw == *stable) {
*candidate = raw;
*streak = 0;
return *stable;
}
if (raw == *candidate) {
if (*streak < 0xFF) (*streak)++;
} else {
*candidate = raw;
*streak = 1;
}
if (*streak >= EDGE_PERSON_PERSIST_FRAMES) {
*stable = *candidate;
*streak = 0;
}
return *stable;
}
/* presence_flag_update() — issue #996 hysteresis + clear debounce. */
static bool presence_flag_update(bool prev, float score, float threshold,
uint8_t *below_count)
{
float low_thresh = threshold * EDGE_PRESENCE_HYST_RATIO;
if (score > threshold) {
*below_count = 0;
return true;
}
if (score >= low_thresh) {
*below_count = 0;
return prev;
}
if (*below_count < 0xFF) (*below_count)++;
if (!prev) {
return false;
}
if (*below_count >= EDGE_PRESENCE_CLEAR_FRAMES) {
*below_count = 0;
return false;
}
return true;
}
/* ──────────────────────────────────────────────────────────────────────
* Test harness
* ────────────────────────────────────────────────────────────────────── */
static int g_failed = 0;
static int g_passed = 0;
#define CHECK_EQ_U8(label, got, expected) do { \
if ((uint8_t)(got) == (uint8_t)(expected)) { g_passed++; } \
else { \
g_failed++; \
printf("FAIL: %s — got=%u expected=%u\n", \
(label), (unsigned)(uint8_t)(got), \
(unsigned)(uint8_t)(expected)); \
} \
} while (0)
#define CHECK_TRUE(label, cond) do { \
if (cond) { g_passed++; } \
else { g_failed++; printf("FAIL: %s — expected true\n", (label)); } \
} while (0)
/* ──────────────────────────────────────────────────────────────────────
* #998 — count_distinct_persons: single body must NOT report EDGE_MAX_PERSONS
* ────────────────────────────────────────────────────────────────────── */
/* One strong signature + weak multipath echoes in adjacent subcarrier groups.
* This is exactly the field report: one person ~50 cm → persons=4. The energy
* gate + spatial dedup must collapse this to 1. */
static void test_count_single_strong_signature(void)
{
/* 4 groups: one dominant, three weak multipath (below the energy gate),
* representative subcarriers clustered (adjacent → one body). */
float energy[EDGE_MAX_PERSONS] = {10.0f, 0.6f, 0.4f, 0.3f};
uint8_t sc[EDGE_MAX_PERSONS] = {20, 21, 22, 23};
CHECK_EQ_U8("single strong signature → 1",
count_distinct_persons(energy, sc, EDGE_MAX_PERSONS), 1);
}
/* Even if the weak echoes are spatially spread, they're still below the energy
* gate, so they don't count. */
static void test_count_single_spread_multipath(void)
{
float energy[EDGE_MAX_PERSONS] = {10.0f, 1.0f, 0.8f, 0.5f};
uint8_t sc[EDGE_MAX_PERSONS] = {10, 40, 70, 100};
CHECK_EQ_U8("single body spread multipath → 1",
count_distinct_persons(energy, sc, EDGE_MAX_PERSONS), 1);
}
/* Two genuine, well-separated, comparably-strong signatures → 2. */
static void test_count_two_well_separated(void)
{
float energy[EDGE_MAX_PERSONS] = {10.0f, 9.0f, 0.3f, 0.2f};
uint8_t sc[EDGE_MAX_PERSONS] = {10, 90, 11, 12};
CHECK_EQ_U8("two well-separated strong → 2",
count_distinct_persons(energy, sc, EDGE_MAX_PERSONS), 2);
}
/* Two strong but spatially ADJACENT signatures collapse to 1 (same body):
* spatial dedup prevents double-counting one person's two strong subcarriers. */
static void test_count_two_strong_adjacent_dedup(void)
{
float energy[EDGE_MAX_PERSONS] = {10.0f, 9.0f, 0.3f, 0.2f};
uint8_t sc[EDGE_MAX_PERSONS] = {20, 21, 60, 61}; /* 20 & 21 adjacent */
CHECK_EQ_U8("two strong but adjacent → 1 (dedup)",
count_distinct_persons(energy, sc, EDGE_MAX_PERSONS), 1);
}
/* No signal at all → 0 persons (empty room). */
static void test_count_no_signal(void)
{
float energy[EDGE_MAX_PERSONS] = {0.0f, 0.0f, 0.0f, 0.0f};
uint8_t sc[EDGE_MAX_PERSONS] = {10, 30, 50, 70};
CHECK_EQ_U8("no signal → 0", count_distinct_persons(energy, sc, EDGE_MAX_PERSONS), 0);
}
/* Three genuine well-separated strong signatures → 3 (gate doesn't under-count). */
static void test_count_three_well_separated(void)
{
float energy[EDGE_MAX_PERSONS] = {10.0f, 9.0f, 8.0f, 0.2f};
uint8_t sc[EDGE_MAX_PERSONS] = {10, 50, 90, 11};
CHECK_EQ_U8("three well-separated strong → 3",
count_distinct_persons(energy, sc, EDGE_MAX_PERSONS), 3);
}
/* ──────────────────────────────────────────────────────────────────────
* #998 — person_count_debounce: a single noisy frame can't change the count
* ────────────────────────────────────────────────────────────────────── */
static void test_debounce_rejects_transient_spike(void)
{
uint8_t candidate = 1, streak = 0, stable = 1; /* settled on 1 person */
/* One spurious frame reports 4 — must NOT promote. */
uint8_t out = person_count_debounce(4, &candidate, &streak, &stable);
CHECK_EQ_U8("transient spike held at 1", out, 1);
/* Back to 1 — resets pending change. */
out = person_count_debounce(1, &candidate, &streak, &stable);
CHECK_EQ_U8("recovered to 1", out, 1);
CHECK_EQ_U8("streak reset", streak, 0);
}
static void test_debounce_accepts_sustained_change(void)
{
uint8_t candidate = 1, streak = 0, stable = 1;
uint8_t out = 1;
/* A genuine 2-person arrival must hold EDGE_PERSON_PERSIST_FRAMES frames. */
for (int i = 0; i < EDGE_PERSON_PERSIST_FRAMES; i++) {
out = person_count_debounce(2, &candidate, &streak, &stable);
}
CHECK_EQ_U8("sustained 2 promoted", out, 2);
CHECK_EQ_U8("stable now 2", stable, 2);
}
/* A flapping count (2,1,2,1,...) never accumulates a streak → stays at stable. */
static void test_debounce_flapping_stays_stable(void)
{
uint8_t candidate = 1, streak = 0, stable = 1;
uint8_t out = 1;
for (int i = 0; i < 10; i++) {
out = person_count_debounce((i & 1) ? 1 : 2, &candidate, &streak, &stable);
}
CHECK_EQ_U8("flapping count stays at 1", out, 1);
}
/* ──────────────────────────────────────────────────────────────────────
* #996 — presence_flag_update: dithering score must NOT flicker the flag
* ────────────────────────────────────────────────────────────────────── */
/* Field trace dithers around the OLD single threshold while the person is
* clearly present. With T_high=10, T_low=5, a score sequence that crosses 10
* up and down must produce a STABLE flag (no per-frame flicker). */
static void test_presence_no_flicker_on_dither(void)
{
const float threshold = 10.0f; /* high threshold */
/* Observed-style trace (issue evidence: 2.6-26.7), but here we model the
* realistic "person present" case where the score mostly sits in/above the
* dead band and only briefly dips. */
float trace[] = {5.6f, 23.0f, 6.8f, 12.0f, 8.0f, 26.7f, 7.0f, 11.0f, 9.0f, 24.0f};
int n = (int)(sizeof(trace) / sizeof(trace[0]));
bool flag = false;
uint8_t below = 0;
int flips = 0;
bool prev = flag;
for (int i = 0; i < n; i++) {
flag = presence_flag_update(flag, trace[i], threshold, &below);
if (i > 0 && flag != prev) flips++;
prev = flag;
}
/* First sample (5.6) is below T_low=5? No, 5.6 >= 5 → dead band, holds
* initial false until 23.0 asserts. After that, dips to 6.8/8.0/7.0/9.0 are
* all >= T_low (5), so they HOLD true. The only transition is the initial
* false→true. No flicker. */
CHECK_TRUE("presence asserted by end", flag);
CHECK_TRUE("at most one transition (no flicker)", flips <= 1);
}
/* Hard dither straddling T_low must still not flicker frame-to-frame because of
* the clear debounce: brief sub-T_low dips don't immediately clear. */
static void test_presence_clear_debounce_holds(void)
{
const float threshold = 10.0f; /* T_low = 5.0 */
bool flag = false;
uint8_t below = 0;
/* Assert. */
flag = presence_flag_update(flag, 20.0f, threshold, &below);
CHECK_TRUE("asserted on strong score", flag);
/* A few brief dips below T_low (< CLEAR_FRAMES) must NOT clear. */
for (int i = 0; i < EDGE_PRESENCE_CLEAR_FRAMES - 1; i++) {
flag = presence_flag_update(flag, 1.0f, threshold, &below);
}
CHECK_TRUE("brief dips below T_low still present", flag);
/* Recovery resets the debounce. */
flag = presence_flag_update(flag, 20.0f, threshold, &below);
CHECK_TRUE("recovered", flag);
CHECK_EQ_U8("below_count reset on recovery", below, 0);
}
/* A genuine departure (score drops and STAYS low) clears within the hold window. */
static void test_presence_genuine_departure_clears(void)
{
const float threshold = 10.0f;
bool flag = false;
uint8_t below = 0;
flag = presence_flag_update(flag, 20.0f, threshold, &below);
CHECK_TRUE("asserted", flag);
/* Person leaves: score stays well below T_low for CLEAR_FRAMES frames. */
for (int i = 0; i < EDGE_PRESENCE_CLEAR_FRAMES; i++) {
flag = presence_flag_update(flag, 0.5f, threshold, &below);
}
CHECK_TRUE("cleared after sustained low", !flag);
}
/* Schmitt gap: a score in the dead band (between T_low and T_high) holds state,
* it neither asserts from false nor clears from true. */
static void test_presence_dead_band_holds_state(void)
{
const float threshold = 10.0f; /* dead band 5..10 */
uint8_t below = 0;
/* From false, a dead-band score does not assert. */
bool flag = presence_flag_update(false, 7.0f, threshold, &below);
CHECK_TRUE("dead band does not assert from false", !flag);
/* From true, a dead-band score does not clear. */
below = 0;
flag = presence_flag_update(true, 7.0f, threshold, &below);
CHECK_TRUE("dead band does not clear from true", flag);
}
/* ──────────────────────────────────────────────────────────────────────
* main
* ────────────────────────────────────────────────────────────────────── */
int main(void)
{
/* #998 person count gate */
test_count_single_strong_signature();
test_count_single_spread_multipath();
test_count_two_well_separated();
test_count_two_strong_adjacent_dedup();
test_count_no_signal();
test_count_three_well_separated();
/* #998 count debounce */
test_debounce_rejects_transient_spike();
test_debounce_accepts_sustained_change();
test_debounce_flapping_stays_stable();
/* #996 presence hysteresis */
test_presence_no_flicker_on_dither();
test_presence_clear_debounce_holds();
test_presence_genuine_departure_clears();
test_presence_dead_band_holds_state();
printf("\n%d passed, %d failed\n", g_passed, g_failed);
return g_failed == 0 ? 0 : 1;
}
@@ -0,0 +1,241 @@
//! Field-peak localization for the Observatory 3D view (issue #1050).
//!
//! ## What this is (and is not)
//!
//! The `/ws/sensing` `sensing_update` frame already carries a real `signal_field`
//! — a 20×20 grid built by `generate_signal_field()` from **measured subcarrier
//! variances** weighted by the **measured motion-band power**. The grid's hot
//! cells are the strongest scatterers in that field representation; as the CSI
//! changes (a person moving through the link), the peak cell moves with it.
//!
//! This module reads the **strongest peak(s)** out of that real field and maps
//! the peak cell to the Observatory room's world coordinates. That gives the
//! 3D figure a position + motion magnitude that are **derived from real signal
//! data**, so the figure now tracks where the field energy concentrates.
//!
//! ### Honesty caveat (do not over-claim)
//!
//! The field's subcarrier→angle mapping in `generate_signal_field()` is a
//! *representation*, not calibrated multistatic triangulation in metric room
//! coordinates. A single ESP32 link cannot resolve a true (x, z) room position.
//! So the emitted `position` is **"strongest field peak in the room model"**,
//! not survey-grade localization. It is real (a function of live CSI), it moves
//! with real motion, and it is honest about its source — but it is NOT a
//! calibrated person fix. Per-person skeletal `pose` keypoints in room
//! coordinates remain gated on the pose model + paired ground-truth data
//! (ADR-079), so `pose` here is only ever set from a real aggregate posture
//! estimate when one exists, and is `None` otherwise (never fabricated).
//!
//! ## Coordinate mapping
//!
//! The Observatory builds its field point cloud (see `ui/observatory/js/main.js`
//! `_buildSignalField`) as, for grid cell `(ix, iz)` of a `20×20` grid:
//!
//! ```text
//! world_x = (ix - gridSize/2) * 0.6
//! world_z = (iz - gridSize/2) * 0.5
//! world_y = 0 (floor)
//! ```
//!
//! and indexes the field as `idx = iz * gridSize + ix` — identical to the
//! server's `generate_signal_field()` layout (`values[z * grid + x]`). We map
//! the peak cell with the **same** transform so the figure lands exactly on the
//! field hotspot it is standing on.
/// World-space scale factor for the X (width) axis, matching the Observatory's
/// `_buildSignalField`: `world_x = (ix - nx/2) * X_SCALE`.
pub const X_SCALE: f64 = 0.6;
/// World-space scale factor for the Z (depth) axis, matching the Observatory's
/// `_buildSignalField`: `world_z = (iz - nz/2) * Z_SCALE`.
pub const Z_SCALE: f64 = 0.5;
/// Minimum normalized field value (`signal_field.values` are normalized to
/// `[0, 1]`) for a cell to be considered a real peak rather than background
/// attenuation. Below this we treat the field as having no localizable hotspot.
pub const PEAK_THRESHOLD: f64 = 0.35;
/// A localized field peak in Observatory world coordinates.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct FieldPeak {
/// World position `[x, y, z]` in Observatory scene units (meters). `y` is
/// always `0.0` — the field is a floor-plane grid with no height info.
pub position: [f64; 3],
/// Normalized field intensity at the peak cell, in `[0, 1]`.
pub intensity: f64,
/// Source grid cell `(ix, iz)` the peak was read from (for tests/debug).
pub cell: (usize, usize),
}
/// Map a grid cell `(ix, iz)` of an `nx × nz` field to Observatory world
/// coordinates, matching `ui/observatory/js/main.js::_buildSignalField`.
#[must_use]
pub fn cell_to_world(ix: usize, iz: usize, nx: usize, nz: usize) -> [f64; 3] {
let wx = (ix as f64 - nx as f64 / 2.0) * X_SCALE;
let wz = (iz as f64 - nz as f64 / 2.0) * Z_SCALE;
[wx, 0.0, wz]
}
/// Extract up to `max_peaks` strongest, spatially-separated peaks from a
/// `signal_field` grid.
///
/// * `values` — row-major field grid, `values[iz * nx + ix]`, normalized to
/// `[0, 1]` (as produced by `generate_signal_field`).
/// * `nx`, `nz` — grid dimensions (the field's `grid_size` is `[nx, 1, nz]`).
/// * `max_peaks` — how many person positions to extract (≥ 1).
///
/// Returns peaks sorted strongest-first. Each successive peak is forced to be
/// at least `min_separation_cells` away from all previously selected peaks so
/// two persons don't collapse onto the same hotspot. Returns an **empty**
/// vector when no cell exceeds [`PEAK_THRESHOLD`] — an empty / no-presence
/// field yields no phantom person.
#[must_use]
pub fn extract_peaks(
values: &[f64],
nx: usize,
nz: usize,
max_peaks: usize,
min_separation_cells: f64,
) -> Vec<FieldPeak> {
if nx == 0 || nz == 0 || values.len() < nx * nz || max_peaks == 0 {
return Vec::new();
}
// Collect all cells above threshold, strongest first.
let mut candidates: Vec<(usize, usize, f64)> = Vec::new();
for iz in 0..nz {
for ix in 0..nx {
let v = values[iz * nx + ix];
if v >= PEAK_THRESHOLD {
candidates.push((ix, iz, v));
}
}
}
candidates.sort_by(|a, b| b.2.total_cmp(&a.2));
let mut peaks: Vec<FieldPeak> = Vec::new();
for (ix, iz, v) in candidates {
if peaks.len() >= max_peaks {
break;
}
// Enforce spatial separation from already-chosen peaks (in cell units).
let too_close = peaks.iter().any(|p| {
let dx = p.cell.0 as f64 - ix as f64;
let dz = p.cell.1 as f64 - iz as f64;
(dx * dx + dz * dz).sqrt() < min_separation_cells
});
if too_close {
continue;
}
peaks.push(FieldPeak {
position: cell_to_world(ix, iz, nx, nz),
intensity: v,
cell: (ix, iz),
});
}
peaks
}
/// Convert measured `motion_band_power` to the `motion_score` scale the
/// Observatory UI expects.
///
/// The UI compares `motion_score > 50` to switch between calm and energetic
/// emission (see `_updateDotMatrixMist` / `_updateParticleTrail`). The raw
/// `motion_band_power` is already in roughly that band for live ESP32 data
/// (the issue reports `motion_band_power: 63.3` while moving), so we pass it
/// through directly, clamped to a sane `[0, 100]` display range. This keeps the
/// emitted value a **direct, real** function of measured motion energy rather
/// than a re-scaled invention.
#[must_use]
pub fn motion_score_from_power(motion_band_power: f64) -> f64 {
motion_band_power.clamp(0.0, 100.0)
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn cell_to_world_matches_observatory_layout() {
// Center cell of a 20×20 grid maps near origin.
let c = cell_to_world(10, 10, 20, 20);
assert!((c[0] - 0.0).abs() < 1e-9);
assert_eq!(c[1], 0.0);
assert!((c[2] - 0.0).abs() < 1e-9);
// Corner cell (0,0) maps to the room's near-left corner.
let corner = cell_to_world(0, 0, 20, 20);
assert!((corner[0] - (-6.0)).abs() < 1e-9); // (0-10)*0.6
assert!((corner[2] - (-5.0)).abs() < 1e-9); // (0-10)*0.5
}
#[test]
fn extract_peaks_finds_known_hotspot() {
// 20×20 field, all background, single strong peak at cell (15, 4).
let nx = 20;
let nz = 20;
let mut values = vec![0.05; nx * nz];
let peak_ix = 15;
let peak_iz = 4;
values[peak_iz * nx + peak_ix] = 1.0;
let peaks = extract_peaks(&values, nx, nz, 1, 3.0);
assert_eq!(peaks.len(), 1);
assert_eq!(peaks[0].cell, (peak_ix, peak_iz));
// Position must match the Observatory cell→world transform within tol.
let expected = cell_to_world(peak_ix, peak_iz, nx, nz);
assert!((peaks[0].position[0] - expected[0]).abs() < 1e-9);
assert!((peaks[0].position[2] - expected[2]).abs() < 1e-9);
// Sanity: (15-10)*0.6 = 3.0, (4-10)*0.5 = -3.0
assert!((peaks[0].position[0] - 3.0).abs() < 1e-9);
assert!((peaks[0].position[2] - (-3.0)).abs() < 1e-9);
}
#[test]
fn empty_field_yields_no_peaks() {
let nx = 20;
let nz = 20;
// All cells below PEAK_THRESHOLD — no presence.
let values = vec![0.10; nx * nz];
let peaks = extract_peaks(&values, nx, nz, 3, 3.0);
assert!(
peaks.is_empty(),
"below-threshold field must not produce a phantom peak"
);
}
#[test]
fn two_separated_peaks_do_not_collapse() {
let nx = 20;
let nz = 20;
let mut values = vec![0.05; nx * nz];
values[2 * nx + 3] = 0.95; // peak A at (3, 2)
values[15 * nx + 17] = 0.90; // peak B at (17, 15)
let peaks = extract_peaks(&values, nx, nz, 2, 3.0);
assert_eq!(peaks.len(), 2);
// Strongest first.
assert_eq!(peaks[0].cell, (3, 2));
assert_eq!(peaks[1].cell, (17, 15));
}
#[test]
fn nearby_secondary_peak_is_suppressed() {
let nx = 20;
let nz = 20;
let mut values = vec![0.05; nx * nz];
values[10 * nx + 10] = 1.00; // primary
values[10 * nx + 11] = 0.99; // adjacent — should be suppressed (sep 3.0)
let peaks = extract_peaks(&values, nx, nz, 2, 3.0);
assert_eq!(peaks.len(), 1, "adjacent cell must not become a 2nd person");
assert_eq!(peaks[0].cell, (10, 10));
}
#[test]
fn motion_score_passthrough_and_clamp() {
assert!((motion_score_from_power(63.3) - 63.3).abs() < 1e-9);
assert_eq!(motion_score_from_power(-5.0), 0.0);
assert_eq!(motion_score_from_power(250.0), 100.0);
}
}
@@ -14,6 +14,7 @@ pub mod cli;
pub mod csi;
mod engine_bridge;
mod field_bridge;
mod field_localize;
mod model_format;
mod multistatic_bridge;
pub mod pose;
@@ -406,6 +407,24 @@ struct PersonDetection {
keypoints: Vec<PoseKeypoint>,
bbox: BoundingBox,
zone: String,
/// Room-world position `[x, y, z]` (Observatory scene units / meters),
/// derived from the strongest `signal_field` peak this person sits on
/// (issue #1050). `y` is `0.0` — the field is a floor-plane grid. This is
/// a real field-peak readout, not calibrated triangulation; see
/// `field_localize` for the honesty caveat. Defaults to `[0,0,0]` until
/// field positions are attached by `attach_field_positions`.
#[serde(default)]
position: [f64; 3],
/// Motion magnitude on the Observatory's `0..100` scale, passed through
/// from the measured `motion_band_power` (issue #1050).
#[serde(default)]
motion_score: f64,
/// Coarse posture label (`"standing"`/`"lying"`/…) when a **real** aggregate
/// posture estimate exists, else `None`. Never fabricated — per-person
/// skeletal pose in room coordinates remains gated on the pose model
/// (ADR-079). The Observatory defaults to `'standing'` when this is absent.
#[serde(skip_serializing_if = "Option::is_none")]
pose: Option<String>,
}
#[derive(Debug, Clone, Serialize, Deserialize)]
@@ -2572,6 +2591,8 @@ async fn windows_wifi_task(state: SharedState, tick_ms: u64) {
if !tracked.is_empty() {
update.persons = Some(tracked);
}
// #1050: attach real signal_field-peak positions to each person.
attach_field_positions(&mut update);
if let Ok(json) = serde_json::to_string(&update) {
let _ = s.tx.send(json);
@@ -2725,6 +2746,8 @@ async fn windows_wifi_fallback_tick(state: &SharedState, seq: u32) {
if !tracked.is_empty() {
update.persons = Some(tracked);
}
// #1050: attach real signal_field-peak positions to each person.
attach_field_positions(&mut update);
if let Ok(json) = serde_json::to_string(&update) {
let _ = s.tx.send(json);
@@ -3163,12 +3186,21 @@ async fn handle_ws_pose_client(mut socket: WebSocket, state: SharedState) {
x: kp[0], y: kp[1], z: kp[2], confidence: kp[3],
})
.collect();
let [nx, _ny, nz] = sensing.signal_field.grid_size;
let peak = field_localize::extract_peaks(
&sensing.signal_field.values, nx, nz, 1, 3.0,
).into_iter().next();
vec![PersonDetection {
id: 1,
confidence: sensing.classification.confidence,
bbox: BoundingBox { x: 260.0, y: 150.0, width: 120.0, height: 220.0 },
keypoints,
zone: "zone_1".into(),
position: peak.map_or([0.0, 0.0, 0.0], |p| p.position),
motion_score: field_localize::motion_score_from_power(
sensing.features.motion_band_power,
),
pose: sensing.posture.clone(),
}]
}).unwrap_or_else(|| {
// Prefer tracked persons from broadcast if available
@@ -3947,6 +3979,53 @@ fn derive_single_person_pose(
height: (max_y - min_y).max(160.0),
},
zone: format!("zone_{}", person_idx + 1),
// Position/motion_score/pose are attached from the real signal_field
// peaks by `attach_field_positions` after the tracker step (#1050);
// default here so the synthetic-skeleton geometry stays unchanged.
position: [0.0, 0.0, 0.0],
motion_score: 0.0,
pose: None,
}
}
/// Attach real, field-derived per-person world positions to a `SensingUpdate`'s
/// `persons` (issue #1050).
///
/// For each detected person we read a strongest-peak position out of the frame's
/// real `signal_field` (the same grid the Observatory already renders) and map
/// it to room-world coordinates via `field_localize::cell_to_world`. `motion_score`
/// is passed through from the measured `motion_band_power`; `pose` is taken from
/// the real aggregate `posture` estimate when present, else left `None` (never
/// fabricated). Persons beyond the number of resolvable field peaks fall back to
/// the strongest peak so they remain co-located with real energy rather than at
/// a fake origin; if the field has no peak above threshold the position stays at
/// `[0,0,0]` and `motion_score` still reflects real motion power.
fn attach_field_positions(update: &mut SensingUpdate) {
let Some(persons) = update.persons.as_mut() else {
return;
};
if persons.is_empty() {
return;
}
let [nx, _ny, nz] = update.signal_field.grid_size;
let peaks = field_localize::extract_peaks(
&update.signal_field.values,
nx,
nz,
persons.len().max(1),
3.0,
);
let motion_score = field_localize::motion_score_from_power(update.features.motion_band_power);
let pose_label = update.posture.clone();
for (i, person) in persons.iter_mut().enumerate() {
if let Some(peak) = peaks.get(i).or_else(|| peaks.first()) {
person.position = peak.position;
}
person.motion_score = motion_score;
person.pose = pose_label.clone();
}
}
@@ -5473,6 +5552,8 @@ async fn udp_receiver_task(state: SharedState, udp_port: u16) {
if !tracked.is_empty() {
update.persons = Some(tracked);
}
// #1050: attach real signal_field-peak positions to each person.
attach_field_positions(&mut update);
if let Ok(json) = serde_json::to_string(&update) {
let _ = s.tx.send(json);
@@ -5903,6 +5984,8 @@ async fn udp_receiver_task(state: SharedState, udp_port: u16) {
if !tracked.is_empty() {
update.persons = Some(tracked);
}
// #1050: attach real signal_field-peak positions to each person.
attach_field_positions(&mut update);
if let Ok(json) = serde_json::to_string(&update) {
let _ = s.tx.send(json);
@@ -6076,6 +6159,8 @@ async fn simulated_data_task(state: SharedState, tick_ms: u64) {
if !tracked.is_empty() {
update.persons = Some(tracked);
}
// #1050: attach real signal_field-peak positions to each person.
attach_field_positions(&mut update);
if update.classification.presence {
s.total_detections += 1;
@@ -8220,3 +8305,171 @@ mod export_rvf_mode_tests {
assert!(!export_emits_placeholder_demo(false, true, false));
}
}
#[cfg(test)]
mod observatory_persons_field_position_tests {
//! Issue #1050 — the Observatory 3D figure animates from per-person
//! `position` / `motion_score` / `pose` carried on `sensing_update.persons`.
//!
//! These tests pin the public WS contract: a frame that detects a person on
//! a known signal_field peak must emit a `persons` array whose first entry
//! carries a `position` derived from that peak (matching the Observatory's
//! cell→world transform), a real `motion_score`, and a serialized frame
//! that round-trips. An empty / no-presence field must emit `persons: []`
//! (or no person), never a phantom person at a fabricated origin.
use super::*;
/// Build a 20×20 signal_field that is background everywhere except a single
/// strong normalized peak at grid cell `(ix, iz)`.
fn field_with_peak(ix: usize, iz: usize) -> SignalField {
let nx = 20usize;
let nz = 20usize;
let mut values = vec![0.05f64; nx * nz];
values[iz * nx + ix] = 1.0;
SignalField {
grid_size: [nx, 1, nz],
values,
}
}
/// Build an all-background (below-threshold) 20×20 field — no localizable
/// hotspot, modelling an empty / no-presence room.
fn empty_field() -> SignalField {
SignalField {
grid_size: [20, 1, 20],
values: vec![0.05f64; 20 * 20],
}
}
fn base_update(signal_field: SignalField, presence: bool, motion_band_power: f64) -> SensingUpdate {
SensingUpdate {
msg_type: "sensing_update".to_string(),
timestamp: 1.0,
source: "test".to_string(),
tick: 1,
nodes: vec![],
features: FeatureInfo {
mean_rssi: -60.0,
variance: 48.6,
motion_band_power,
breathing_band_power: 0.0,
dominant_freq_hz: 1.0,
change_points: 0,
spectral_power: 0.0,
},
classification: ClassificationInfo {
motion_level: if presence { "present_moving".to_string() } else { "absent".to_string() },
presence,
confidence: 0.8,
},
signal_field,
vital_signs: None,
enhanced_motion: None,
enhanced_breathing: None,
posture: None,
signal_quality_score: None,
quality_verdict: None,
bssid_count: None,
pose_keypoints: None,
model_status: None,
persons: None,
estimated_persons: Some(1),
node_features: None,
}
}
#[test]
fn sensing_update_emits_persons_with_field_derived_position() {
// Person present, motion energy 63.3, a hotspot at cell (15, 4).
let peak_ix = 15;
let peak_iz = 4;
let mut update = base_update(field_with_peak(peak_ix, peak_iz), true, 63.3);
// Pipeline order: derive raw skeleton, then attach real field positions.
update.persons = Some(derive_pose_from_sensing(&update));
attach_field_positions(&mut update);
let persons = update.persons.as_ref().expect("persons should be Some");
assert!(!persons.is_empty(), "a present person must be emitted");
// Position must match the Observatory cell→world transform for (15, 4):
// x = (15-10)*0.6 = 3.0 ; z = (4-10)*0.5 = -3.0 ; y = 0.
let p0 = &persons[0];
assert!((p0.position[0] - 3.0).abs() < 1e-6, "x={}", p0.position[0]);
assert!((p0.position[1] - 0.0).abs() < 1e-9);
assert!((p0.position[2] - (-3.0)).abs() < 1e-6, "z={}", p0.position[2]);
// motion_score is the measured motion_band_power passed through (≤100).
assert!((p0.motion_score - 63.3).abs() < 1e-6, "motion_score={}", p0.motion_score);
// The serialized WS frame must carry the new fields by their exact
// contract names the Observatory UI reads.
let v = serde_json::to_value(&update).unwrap();
let arr = v["persons"].as_array().expect("persons must be a JSON array");
assert_eq!(arr.len(), persons.len());
let pj = &arr[0];
assert!(pj.get("position").is_some(), "person.position missing from WS frame");
assert!(pj.get("motion_score").is_some(), "person.motion_score missing from WS frame");
assert!((pj["position"][0].as_f64().unwrap() - 3.0).abs() < 1e-6);
assert!((pj["position"][2].as_f64().unwrap() - (-3.0)).abs() < 1e-6);
assert!((pj["motion_score"].as_f64().unwrap() - 63.3).abs() < 1e-6);
}
#[test]
fn pose_is_real_when_posture_present_and_absent_otherwise() {
// No aggregate posture estimate → pose is None (never fabricated).
let mut no_posture = base_update(field_with_peak(10, 10), true, 40.0);
no_posture.persons = Some(derive_pose_from_sensing(&no_posture));
attach_field_positions(&mut no_posture);
let p = &no_posture.persons.as_ref().unwrap()[0];
assert!(p.pose.is_none(), "pose must stay None when no real posture exists");
// skip_serializing_if drops the key entirely (UI defaults to 'standing').
let v = serde_json::to_value(&no_posture).unwrap();
assert!(v["persons"][0].get("pose").is_none());
// Real aggregate posture present → pose is carried through verbatim.
let mut with_posture = base_update(field_with_peak(10, 10), true, 40.0);
with_posture.posture = Some("lying".to_string());
with_posture.persons = Some(derive_pose_from_sensing(&with_posture));
attach_field_positions(&mut with_posture);
let p2 = &with_posture.persons.as_ref().unwrap()[0];
assert_eq!(p2.pose.as_deref(), Some("lying"));
let v2 = serde_json::to_value(&with_posture).unwrap();
assert_eq!(v2["persons"][0]["pose"], "lying");
}
#[test]
fn empty_room_yields_no_phantom_person() {
// No presence → derive_pose_from_sensing returns no persons at all.
let mut update = base_update(empty_field(), false, 2.0);
update.persons = Some(derive_pose_from_sensing(&update));
attach_field_positions(&mut update);
let persons = update.persons.as_ref().unwrap();
assert!(
persons.is_empty(),
"no-presence frame must not emit a phantom person, got {} persons",
persons.len()
);
// And in the serialized frame the array is empty (no fake origin person).
let v = serde_json::to_value(&update).unwrap();
assert_eq!(v["persons"].as_array().unwrap().len(), 0);
}
#[test]
fn present_but_below_threshold_field_keeps_position_at_origin_not_fabricated() {
// Presence is true but the field has no peak above PEAK_THRESHOLD — we
// must NOT invent a position; it stays at the [0,0,0] default while
// motion_score still reflects the real measured motion power. This is
// the honest degenerate case (no localizable hotspot to report).
let mut update = base_update(empty_field(), true, 55.0);
update.persons = Some(derive_pose_from_sensing(&update));
attach_field_positions(&mut update);
let p = &update.persons.as_ref().unwrap()[0];
assert_eq!(p.position, [0.0, 0.0, 0.0], "no peak → default origin, not fabricated coords");
assert!((p.motion_score - 55.0).abs() < 1e-6, "motion_score stays real");
}
}
@@ -192,6 +192,11 @@ pub fn derive_single_person_pose(
height: (max_y - min_y).max(160.0),
},
zone: format!("zone_{}", person_idx + 1),
// Field-derived fields (#1050) — defaulted here; the live `/ws/sensing`
// path attaches real positions via `attach_field_positions`.
position: [0.0, 0.0, 0.0],
motion_score: 0.0,
pose: None,
}
}
@@ -176,6 +176,13 @@ pub fn tracker_to_person_detections(tracker: &PoseTracker) -> Vec<PersonDetectio
keypoints,
bbox,
zone: "tracked".to_string(),
// Field-derived position/motion_score/pose are (re)attached from
// the live signal_field by `attach_field_positions` after this
// tracker step (#1050); the Kalman tracker smooths keypoints only,
// so we default here and let the field readout fill them in.
position: [0.0, 0.0, 0.0],
motion_score: 0.0,
pose: None,
}
})
.collect()
@@ -329,6 +336,9 @@ mod tests {
height: 1.0,
},
zone: "test".to_string(),
position: [0.0, 0.0, 0.0],
motion_score: 0.0,
pose: None,
}
}
@@ -203,6 +203,21 @@ pub struct PersonDetection {
pub keypoints: Vec<PoseKeypoint>,
pub bbox: BoundingBox,
pub zone: String,
/// Room-world position `[x, y, z]` (Observatory scene units / meters),
/// derived from the strongest `signal_field` peak (issue #1050). `y` is
/// `0.0` — the field is a floor-plane grid. Real field-peak readout, not
/// calibrated triangulation. Defaults to `[0,0,0]`.
#[serde(default)]
pub position: [f64; 3],
/// Motion magnitude on the Observatory's `0..100` scale, passed through
/// from the measured `motion_band_power` (issue #1050).
#[serde(default)]
pub motion_score: f64,
/// Coarse posture label when a real aggregate posture estimate exists,
/// else `None`. Never fabricated; per-person skeletal pose remains gated
/// on the pose model (ADR-079).
#[serde(skip_serializing_if = "Option::is_none")]
pub pose: Option<String>,
}
#[derive(Debug, Clone, Serialize, Deserialize)]