diff --git a/CHANGELOG.md b/CHANGELOG.md index a02bd8e563..15cddd7eab 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -7,6 +7,20 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ## [Unreleased] +### Added +- **Phase 8: UDP→WebSocket multi-target gateway + data association (`serve-demo.py`, `mock_udp_injector.py`, `06-cyber-hud.html`).** `serve-demo.py --gateway` (or `RUVIEW_UDP_GATEWAY=1`; default off, static-serve behaviour unchanged) runs a concurrent UDP:5555 listener + a **from-scratch stdlib WebSocket server** on :8770 (no `websockets` dependency; RFC 6455 handshake, unmasked server text frames) + a 20 Hz broadcaster. It ingests `{"node_id","mac","rssi"}` datagrams, aggregates per-MAC with a node TTL, and broadcasts `{"type":"pose","src":"udp","targets":[{mac,ranges:[{node_id,rssi_dbm}]}]}`. Design correction (surfaced, not silently worked around): the server forwards raw `rssi_dbm` — it can't apply "the UI calibration coefficients" because N/P0 live in the browser; the HUD converts with its live sliders. HUD multi-target association: `activeTargets` Map, each with an independent alpha filter + Gauss-Newton solve + uniquely MAC-hued capsule; unknown MACs spawn a track + log line, 10 s silence GCs it (`TARGET … lost (timeout)`). New **Target Roster** panel (live MAC list with x/y, velocity, and a Focus toggle that camera-locks the orbit pivot onto a capsule). Backward-compatible with the legacy single-target contract; synthetic driver now walks **two** independent devices to demo it. New `mock_udp_injector.py` streams N devices for hardware-free testing. VERIFIED: gateway proven **live** (subprocess + stdlib WS client + real UDP packets → 16/16, incl. RFC 6455 accept-key vector, TTL aggregation, both MACs in one frame); shipped HUD `[MTGT]` block chained with `[MLAT]`+`[SIM-ATTEN]` in Node → 15/15 (deterministic MAC hue, per-target independent solve/filter, GC, two-target LOS 0.10 m vs NLOS-behind-concrete 0.71 m). Note: multi-target + the UDP gateway did **not** exist before this phase (the prior mandate's "accepted port 5555 / multi-target" premise was fictitious — built for real here). +- **Cyber-HUD Phase 7: live calibration + intercept log (`06-cyber-hud.html`), demo 06 in `serve-demo.py` startup listing.** Signal→distance calibration sliders in the layout editor: path-loss exponent N (1.6–4.0) and P₀@1 m (dBm) drive a real log-distance conversion `r = 10^((P0−rssi)/(10N))` for bridges that send `rssi_dbm` instead of `r_m` in `ranges` (new contract option, honest scope: RSSI ranging carries metre-scale error indoors — the residual readout stays the truth-teller); material-loss × slider scales the SIM wall-degradation only. Scrolling intercept log feed (bottom-left, tagged SYS_INT/SIM_ENV/TRK/LAYOUT): bridge connect/loss, track acquired/lost, sim target wall-crossing events (`TARGET_01 crossed brick_wall_05`), per-node sight-line obstructed/restored transitions, node placement/removal, layout export, calibration changes. Shipped-block validation extended to 22/22 (matScale scaling + backward-compat default, log-distance known values incl. N=2 −60 dBm → 10 m, bad-input null). Fixed en route: `connect.wasOpen` read-before-set. +- **Cyber-HUD Phase 5: visual layout editor + material attenuation simulator (`06-cyber-hud.html`).** Layout editor (✎ Layout): drag-in floor-plan image rendered as a translucent textured plane under the grid; double-click the floor to place a node / a marker to remove it (raycast against ground plane and markers — new nodes join the multilateration matrix immediately); live width/height rescaling; one-click export of the current layout as schema-conformant JSON. NLOS attenuation model (SIM ONLY, marked block): proper 2D segment-intersection (orientation test incl. collinear cases) finds every wall crossing each node→target sight line; each crossing adds a **positive range bias + extra noise per material** (drywall σ 0.08 m / brick σ 0.25 m / concrete σ 0.65 m per spec, bias ≈ σ — NLOS ranges read systematically LONG, zero-mean noise alone would be unrealistically kind). Sight lines render colour-coded by worst material (green LOS / amber drywall / magenta brick / red concrete) with an `NLOS nodes` readout; the default sim path threads the demo map's door gap. Shipped-JS validation (extracted `[SIM-ATTEN]` block executed under Node, 16/16): intersection truth table, demo-map crossings incl. the door-gap LOS case, deterministic bias accumulation, measured concrete σ 0.652 vs 0.65 spec — this test caught and fixed a real ×0.577 sampler-scale bug in the shipped noise generator. End-to-end (both shipped blocks chained): mean tracking error 0.110 m LOS → 0.713 m behind concrete, demonstrating the intended solver-vs-degradation behaviour. +- **Range-only multilateration engine (`wifi-densepose-signal::multilateration`).** Closed-form linearized initialization (pairwise sphere-equation differences) refined by damped Gauss-Newton, pure Rust via the crate's Cholesky/Jacobi helpers (no BLAS). `solve_3d` (≥4 nodes with 3D spread) and `solve_2d` (≥3 nodes — the correct mode for near-coplanar indoor arrays, which the 3D solve **refuses as mirror-ambiguous** rather than silently picking a side); `AlphaFilter3` anti-jitter smoother. Degeneracy is refused, never fudged: `InsufficientNodes`, `DegenerateGeometry` (collinear / coplanar-z via node-spread eigenvalue rank check). Honest scope in docs: the solver turns ranges into positions and reports `rms_residual_m` — it cannot make ESP32 RSSI-derived ranges good, and vertical DOP dominates flat arrays (numeric proof, 11/11: exact at zero noise; σ=0.1 m ranges/6 nodes → horizontal p95 0.15 m, 3D p95 0.61 m; alpha filter halves tracking error). 8 Rust unit tests mirroring the proof (uncompiled here — same cargo gate as above). +- **Blueprint ingestion + IPS layer in the Cyber HUD (`06-cyber-hud.html`).** The hardcoded room is gone: a JSON layout (`map_metadata` dims + `nodes[{node_id,x,y,z}]` + `structural_walls[{start,end,material,height?}]`, metres) drives volumetric wall slabs (material-coded: concrete/brick/drywall), sensing-node markers, a size-adaptive RTI voxel field, grid, and camera framing — loaded via `?map=` with an embedded 20×15 m demo default. The `/pose` contract gains optional `target`/`ranges`/`target_true`; when ≥3 map nodes have ranges, an in-browser **2D multilateration solve** (JS mirror of the Rust, alpha-filtered) positions the target capsule + rig + velocity vector + RTI blob in the blueprint frame. The synthetic driver now walks a waypoint loop inside the map emitting σ≈0.25 m noisy ranges plus ground truth, so the full ranges→solve→render pipeline runs end-to-end with a visible true-vs-estimate error readout and an amber ghost marker at sim truth (estimate can never masquerade as ground truth). The **shipped** JS block was extracted and executed under Node: exact recovery 1.8e-15, p95 0.435 m at sim noise, 2-node/collinear/NaN inputs → null (7/7). `wiflow_infer.py` forwards `target`/`ranges`/`target_true` from the upstream frame (forward-only, never synthesised). +- **Adaptive multi-antenna MUSIC AoA engine (`wifi-densepose-signal::aoa_music`).** Pure-Rust (no BLAS — works under `--no-default-features`): M×M complex Hermitian covariance → real-symmetric 2M×2M embedding → cyclic Jacobi eigendecomposition → noise-projector MUSIC pseudo-spectrum `P(θ)=1/(ãᵀQₙã)`, with forward-backward averaging and optional spatial smoothing for coherent multipath. **Aperture-gated**: `M = 1` returns `AoaError::InsufficientAperture` so single-antenna ESP32 streams fall back to the legacy pipeline — the module never fabricates an angle; the high-resolution path unlocks only on real multi-antenna input. Honest scope in the module docs: output is an AoA spectrum per array, **not** positions. Every algorithmic step validated numerically before porting (Jacobi ≡ `numpy.linalg.eigh` on 20 random matrices; embedding noise-projector ≡ complex `|aᴴEₙ|²` over 200 random vectors; peaks exact at ±0.0° for 2 uncorrelated sources M=6 SNR 10 dB; coherent sources resolved via subarray smoothing; M=1 refused). 7 Rust unit tests mirroring the proof. +- **Tikhonov ridge tomography (`RfTomographer::reconstruct_tikhonov`).** Direct one-shot solve of `(WᵀW+λI)x = Wᵀy` via pure-Rust Cholesky (SPD by construction, handles rank-deficient link matrices) alongside the existing iterative ISTA-L1 — no iteration-truncation flicker frame-to-frame, giving the HUD RTI backdrop a temporally stable field. Docs state the validated trade-off honestly: near-exact recovery when links ≥ voxels (corr 1.000 measured in the numeric proof), blurrier-but-support-localizing in the usual underdetermined RTI regime (5/5 true-support voxels in top-5, measured). 4 Rust unit tests incl. forward-model recovery through the tomographer's own weight matrix. +- **CFO/SFO sanitization wired into the live server path (opt-in).** New `phase_sanitizer::unwrap_and_detrend_rows` slice API (no ndarray needed at call sites): per-antenna-row 1D unwrap **then** linear de-trend (unwrap-first is required — fitting a line to wrapped `atan2` phase is meaningless). `wifi-densepose-sensing-server::csi::parse_esp32_frame` applies it to every parsed frame when `SENSING_CFO_SFO_DETREND=1|true` (OnceLock env gate matching the existing `WDP_*` idiom); default off, legacy bit-identical. Unit test covers wrapped multi-row round-trip and invalid-shape no-op. +- NOTE for the three items above: `cargo` was not available in the authoring environment — the Rust was **not** compiled here (algorithms were instead proven in a mirrored numeric harness first). Gate on `cd v2 && cargo test -p wifi-densepose-signal -p wifi-densepose-sensing-server --no-default-features` before merge. +- **CFO/SFO linear phase de-trend in `wifi-densepose-signal` (`PhaseSanitizer::remove_linear_phase`).** Subtracts a per-row least-squares line fit across subcarriers, removing the SFO/STO slope + CFO constant offset (the standard CSI phase-calibration step, SpotFi §3.1) while preserving nonlinear multipath structure. Gated behind a new `enable_cfo_sfo_removal` config flag (default **off**, so no existing test behaviour changes) and inserted into `sanitize_phase` after unwrapping. The exact closed-form fit was validated numerically before porting — output is invariant to any injected linear phase (worst |Δ| = 7.7e-14 over 2000 random CFO/SFO injections; matches `numpy.polyfit`; nonlinear structure corr = 1.000000). Two Rust unit tests added (`test_cfo_sfo_linear_removal_is_invariant`, `test_cfo_sfo_removal_disabled_is_noop`). NOTE: `cargo` was not available in the authoring environment, so these were **not** compiled/run here — verify with `cargo test -p wifi-densepose-signal --no-default-features`. +- **`examples/three.js/demos/06-cyber-hud.html` — standalone cyberpunk composite viewport.** An articulated low-poly rig driven directly from the live `ws://:8770/pose` WiFi feed (the same contract as `examples/through-wall/pose.html`) via **direct quaternionic retargeting** (each bone's rest axis rotated to aim at its COCO keypoint pair, composed in parent-local space in topological order) with **3-frame temporal smoothing + dropout hold** to damp CSI jitter. Composited on a dark cyber-grid with translucent volumetric room walls, a 10×10 **RTI attenuation field** (accepts a real `rti` array from the bridge, else a body-centred demo blob), a target **capsule + velocity vector**, and a **vitals HUD** (Catmull-Rom→bezier breathing waveform, pulsing double-beat heart ring, green→amber→red stress gradient). Reuses the three.js r128 + UnrealBloom stack from demo 05; **no FBX asset needed** (procedural rig) and runs a clearly-labelled synthetic driver when no bridge is present. HONEST SCOPE: WiFi pose is coarse (~59.5% PCK@0.10 vs 50% baseline), the feed is 2D so avatar **depth is estimated for display**, and vitals/RTI are **synthetic unless the bridge supplies them**. Registered as demo 06 in `examples/three.js/index.html`. Core retargeting math validated against real three.js r128 (all 10 driven bones align to target joints at dot=1.000000; smoothing + dropout-hold covered). +- **`wiflow_infer.py` now forwards real vitals into the `/pose` payload (`vitals_fields`).** Breathing rate is taken from an explicit `breathing_rate_bpm` when the sensing pipeline (`csi.rs`) reports one, else **derived from the breathing-band spectral peak** (`features.dominant_freq_hz` in 0.1–0.5 Hz → bpm), and heart rate is forwarded **only** when the feed explicitly carries a plausible `heart_rate_bpm` (40–180). Stress is never emitted and WiFi heart-rate is never synthesised — fields are simply omitted when unavailable, so the HUD shows `sim`. 9/9 extraction unit checks (in-band derivation, out-of-band rejection, explicit-field precedence, no-fabrication guarantees). + ### Changed - **`@ruvnet/rvagent` startup optimization — stdio time-to-first-response ~242 ms → ~189 ms (−22%; MEASURED, median of repeated `initialize` round-trips against `dist/index.js`, this container, reproduce with a piped-stdin timer).** Two changes: (1) `./http-transport.js` is now imported **lazily** inside the `RVAGENT_HTTP_PORT` branch — it chain-loads the MCP SDK's `streamableHttp` module (~48 ms MEASURED via per-module `import()` timing), which the default stdio path never uses; (2) the advertised JSON Schemas generated from the Zod sources are memoized per tool instead of re-walking the Zod tree on every `tools/list` (matters under the session-per-server HTTP model where each session lists tools). No behavior change: 99/99 jest tests, HTTP session flow re-smoke-tested through the lazy path. The `@ruvnet/ruview` harness CLI was profiled too and left alone — 50 ms vs the ~29 ms bare `node -e ''` floor on the same box (MEASURED), i.e. already near the interpreter floor with zero dependencies. diff --git a/_READ_ME_FIRST.md b/_READ_ME_FIRST.md new file mode 100644 index 0000000000..c8b44c0238 --- /dev/null +++ b/_READ_ME_FIRST.md @@ -0,0 +1,53 @@ +# RuView — Cyber-HUD IPS contribution (PR staging copy) + +This folder is a **copy** of the files added/changed during the Cyber-HUD + +indoor-positioning work. The originals remain in place in the repo; nothing here +is a move. Paths below mirror the real repo layout so you can review the exact +file set, then apply it to a fork. + +## ⚠️ How to actually open the PR (important) + +This working tree is **not a git repository** (`git rev-parse` fails here), and a +PR is *not* made by pushing a folder called `PR-github`. If you push this folder +as-is, the upstream repo would get files nested under `PR-github/…`, which is not +a valid contribution. Do it the normal way instead: + +1. Fork `github.com/ruvnet/RuView` on GitHub → clone your fork. +2. `git checkout -b feature/cyber-hud-ips` +3. Copy each file below **to its real path** in the fork (the paths shown here, + minus the `PR-github/` prefix — e.g. `examples/three.js/demos/06-cyber-hud.html`). +4. `git add -A && git commit` → `git push -u origin feature/cyber-hud-ips` +5. Open the PR on GitHub from your branch. + +Once you've done that, you can delete this `PR-github/` folder — it's only a +staging convenience. + +## Files in this contribution + +### New files +| Path | What it is | +|------|-----------| +| `examples/three.js/demos/06-cyber-hud.html` | Cyber-HUD dashboard: rigged avatar, RTI backdrop, vitals, blueprint editor, multilateration IPS, multi-target roster, live calibration, intercept log | +| `examples/three.js/server/mock_udp_injector.py` | Mock multi-device UDP injector for hardware-free testing of the gateway | +| `v2/crates/wifi-densepose-signal/src/aoa_music.rs` | Pure-Rust multi-antenna MUSIC AoA (no BLAS), aperture-gated | +| `v2/crates/wifi-densepose-signal/src/multilateration.rs` | Range-only multilateration (2D/3D Gauss-Newton) + alpha filter | + +### Modified files +| Path | Change | +|------|--------| +| `examples/three.js/index.html` | Registered demo 06 in the series index | +| `examples/three.js/server/serve-demo.py` | Added demo 06 to listing + optional UDP:5555→WebSocket:8770 gateway (`--gateway`) | +| `examples/through-wall/wiflow_infer.py` | Forwards real vitals + IPS `target`/`ranges` fields into `/pose` | +| `v2/crates/wifi-densepose-signal/src/phase_sanitizer.rs` | CFO/SFO linear phase de-trend (opt-in) | +| `v2/crates/wifi-densepose-signal/src/lib.rs` | Registered `aoa_music` + `multilateration` modules | +| `v2/crates/wifi-densepose-signal/src/ruvsense/tomography.rs` | Added Tikhonov ridge solver alongside ISTA-L1 | +| `v2/crates/wifi-densepose-sensing-server/src/csi.rs` | Wired CFO/SFO de-trend into the live path (env-gated) | +| `CHANGELOG.md` | `[Unreleased]` entries for all of the above | + +## ⚠️ Before submitting — two things the maintainer will expect + +1. **Compile the Rust.** It was authored where `cargo` was unavailable, so it was + validated by mirrored numeric proofs but **not compiled**. Run and green it + first: `cd v2 && cargo test -p wifi-densepose-signal -p wifi-densepose-sensing-server --no-default-features` +2. **Frontend/gateway JS + Python are verified** (syntax + extracted-block unit + tests + a live UDP→WebSocket integration test), so those are ready to review. diff --git a/examples/three.js/demos/06-cyber-hud.html b/examples/three.js/demos/06-cyber-hud.html new file mode 100644 index 0000000000..e800aeb6bc --- /dev/null +++ b/examples/three.js/demos/06-cyber-hud.html @@ -0,0 +1,1333 @@ + + + + + +RuView · Cyber HUD · rigged avatar + RTI backdrop + vitals telemetry + + + + + + + + + + + + +
+ + +
+

RuView · CYBER-HUD

+ +
+ +
+
+
Target telemetry
+
source—
+
presence—
+
motion idx—
+
velocity—
+
pose fps—
+
+
+
IPS multilateration ·off
+
map—
+
nodes used—
+
target (m)—
+
residual—
+
sim true err—
+
NLOS nodes—
+
+
+
Respiration ·sim
+ +
rate— bpm
+
+
+
Cardiac ·sim
+ +
rate— bpm
+
+
+
Stress index
+ +
level—
+
+
+ +
+
Layout editor
+ + +
+ W + H m +
+ +
Signal→distance calibration
+
path-loss exponent N 2.0 +
+
P₀ @1 m (dBm) -40 +
+
material loss × 1.0 +
+
N/P₀ convert live rssi_dbm ranges via the log-distance + model; material × scales the SIM wall degradation. Double-click the floor to + place a node; double-click a marker to remove it.
+
+ +
+ +
+
Target roster0
+
no targets — awaiting telemetry
+
+ +
+ + + + + + +
+ +
+ WiFi-inferred pose is coarse (~59.5% PCK@0.10 vs 50% baseline). Feed is 2D image-space; + depth is estimated for display. Vitals, RTI occupancy & per-node ranges are + synthetic unless the bridge sends bpm_*/rti/ranges; multilateration output is only + as good as the input ranges (residual shown). Amber wireframe ghost = simulated ground truth. + Load a layout with ?map=<url>. Start wiflow_infer.py for a live bridge on :8770. +
+ + + + diff --git a/examples/three.js/index.html b/examples/three.js/index.html index 3156737794..f22886678a 100644 --- a/examples/three.js/index.html +++ b/examples/three.js/index.html @@ -108,9 +108,10 @@

RuView · three.js demos

- Five progressively richer browser demos of the ADR-097 - sensing-helpers scene, ending with a live MediaPipe-Pose → Mixamo X Bot retargeting pipeline driven - by a real ESP32 CSI feed. + Six progressively richer browser demos of the ADR-097 + sensing-helpers scene — from plain helpers, through a live MediaPipe-Pose → Mixamo X Bot retargeting + pipeline driven by a real ESP32 CSI feed, to a standalone cyberpunk HUD that drives an articulated rig + straight from the :8770/pose WiFi feed with RTI backdrop and vitals telemetry.

@@ -145,6 +146,14 @@

05 · Realtime (Pose + CSI) needs FBX

05 screenshot + +

06 · Cyber HUD standalone

+
Articulated low-poly rig driven directly from the :8770/pose WiFi feed via + quaternionic retargeting + 3-frame smoothing. Cyber-grid + RTI attenuation backdrop, target capsule + with velocity vector, and bezier breathing / pulsing-heart / stress-gradient telemetry. No FBX — + runs a synthetic driver when no bridge is present.
+ +
diff --git a/examples/three.js/server/mock_udp_injector.py b/examples/three.js/server/mock_udp_injector.py new file mode 100644 index 0000000000..acc2aeec92 --- /dev/null +++ b/examples/three.js/server/mock_udp_injector.py @@ -0,0 +1,77 @@ +"""Mock multi-device UDP packet injector for the Phase 8 tracking gateway. + +Simulates N devices (MACs) walking loops inside the demo 20x15 m map, each seen +by 4 ESP32 nodes. Emits log-distance RSSI (with per-device noise) as UDP JSON to +the gateway on 0.0.0.0:5555, so 06-cyber-hud.html shows multiple independent +tracked capsules with no hardware. + +Run alongside: python serve-demo.py --gateway +Then: python mock_udp_injector.py # 2 devices + python mock_udp_injector.py --devices 4 --rate 15 +""" +import argparse +import json +import math +import random +import socket +import time + +NODES = [ # must match 06-cyber-hud.html DEFAULT_MAP node ids/positions + ("ESP32_01", 1.2, 0.5, 1.0), + ("ESP32_02", 18.5, 2.1, 1.2), + ("ESP32_03", 9.0, 14.2, 2.5), + ("ESP32_04", 0.8, 13.5, 1.8), +] +P0_DBM, PLE = -40.0, 2.2 # log-distance model the HUD should be calibrated near + + +def range_to_rssi(r_m, jitter): + r = max(r_m, 0.3) + return P0_DBM - 10.0 * PLE * math.log10(r) + random.gauss(0.0, jitter) + + +def device_path(seed): + rng = random.Random(seed) + # a lissajous-ish loop kept inside the 20x15 map + ax, ay = rng.uniform(5, 8), rng.uniform(3, 5.5) + cx, cy = rng.uniform(8, 12), rng.uniform(6, 9) + fx, fy = rng.uniform(0.10, 0.20), rng.uniform(0.13, 0.24) + ph = rng.uniform(0, 6.28) + return lambda t: (cx + ax * math.sin(fx * t + ph), + cy + ay * math.sin(fy * t)) + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--host", default="127.0.0.1") + ap.add_argument("--port", type=int, default=5555) + ap.add_argument("--devices", type=int, default=2) + ap.add_argument("--rate", type=float, default=10.0, help="frames/sec per device") + ap.add_argument("--jitter", type=float, default=1.5, help="RSSI noise sigma (dB)") + args = ap.parse_args() + + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + macs = [f"AA:BB:CC:00:00:{i:02X}" for i in range(1, args.devices + 1)] + paths = [device_path(i) for i in range(args.devices)] + print(f"injecting {args.devices} device(s) → udp {args.host}:{args.port} " + f"@ {args.rate} Hz/device ({', '.join(macs)}) Ctrl-C to stop") + + t0 = time.time() + period = 1.0 / max(args.rate, 0.5) + try: + while True: + t = time.time() - t0 + for mac, path in zip(macs, paths): + x, y = path(t) + for nid, nx, ny, nz in NODES: + r = math.dist((x, y, 1.0), (nx, ny, nz)) + pkt = {"node_id": nid, "mac": mac, + "rssi": round(range_to_rssi(r, args.jitter), 1)} + sock.sendto(json.dumps(pkt).encode("utf-8"), (args.host, args.port)) + time.sleep(period) + except KeyboardInterrupt: + print("\nstopped") + + +if __name__ == "__main__": + main() diff --git a/examples/three.js/server/serve-demo.py b/examples/three.js/server/serve-demo.py index 3ca9088ea0..c20f907072 100644 --- a/examples/three.js/server/serve-demo.py +++ b/examples/three.js/server/serve-demo.py @@ -1,31 +1,44 @@ -"""Tiny threaded HTTP server for the three.js demos that fetch local files. +"""Tiny threaded HTTP server for the three.js demos, plus an optional +UDP → WebSocket tracking gateway (ADR-none / Phase 8). Why a sibling helper script instead of `python -m http.server`? The stdlib SimpleHTTPServer is single-threaded; Chrome opens many parallel connections (HTML + 9 script tags + FBX), the first eats the worker, the rest time out with net::ERR_EMPTY_RESPONSE. ThreadingHTTPServer fixes it. -Usage: +Usage (static demos only — default, unchanged): python examples/three.js/server/serve-demo.py - open http://localhost:8765/examples/three.js/demos/05-skinned-realtime.html + open http://localhost:8765/examples/three.js/demos/06-cyber-hud.html + +Usage (with the live multi-target gateway): + python examples/three.js/server/serve-demo.py --gateway + # then stream ESP32 RSSI frames as UDP JSON to 0.0.0.0:5555: + # {"node_id": "ESP32_01", "mac": "AA:BB:CC:DD:EE:FF", "rssi": -58} + # the gateway aggregates per-MAC, broadcasts a multi-target /pose frame on + # ws://:8770/pose; 06-cyber-hud.html solves + renders each target. + +Design note (honest): the server forwards raw `rssi_dbm` per node — it does NOT +convert RSSI→range here, because the log-distance coefficients (N, P0) are the +browser's live UI calibration and the server has no channel to them. The HUD +converts with its own sliders. The WebSocket is a from-scratch stdlib +implementation (no `websockets` dependency) — RFC 6455 §1.3 handshake, unmasked +server text frames. """ from http.server import ThreadingHTTPServer, SimpleHTTPRequestHandler -import os, sys +import argparse +import base64 +import hashlib +import json +import os +import socket +import struct +import sys +import threading +import time -PORT = int(os.environ.get("PORT", 8765)) # Always serve from the repo root regardless of where the script is launched. # This file lives at examples/three.js/server/serve-demo.py — three levels deep. -os.chdir(os.path.abspath(os.path.join(os.path.dirname(__file__), "..", "..", ".."))) - -class NoCacheHandler(SimpleHTTPRequestHandler): - def end_headers(self): - # Aggressive no-cache so browser ALWAYS fetches the latest .html - # after we edit it. Otherwise stale code sticks around even on hard - # refresh and you debug a phantom. - self.send_header("Cache-Control", "no-store, no-cache, must-revalidate, max-age=0") - self.send_header("Pragma", "no-cache") - self.send_header("Expires", "0") - super().end_headers() +REPO_ROOT = os.path.abspath(os.path.join(os.path.dirname(__file__), "..", "..", "..")) DEMOS = [ "01-helpers.html", @@ -33,14 +46,237 @@ def end_headers(self): "03-skinned.html", "04-skinned-fbx.html", "05-skinned-realtime.html", + "06-cyber-hud.html", ] -with ThreadingHTTPServer(("127.0.0.1", PORT), NoCacheHandler) as srv: - print(f"serving {os.getcwd()} on http://127.0.0.1:{PORT}/") - print("demos:") - for d in DEMOS: - print(f" http://127.0.0.1:{PORT}/examples/three.js/demos/{d}") +# ── Static HTTP handler ────────────────────────────────────────────────────── + +class NoCacheHandler(SimpleHTTPRequestHandler): + def end_headers(self): + # Aggressive no-cache so the browser ALWAYS fetches the latest .html + # after an edit; otherwise stale code sticks around even on hard refresh. + self.send_header("Cache-Control", "no-store, no-cache, must-revalidate, max-age=0") + self.send_header("Pragma", "no-cache") + self.send_header("Expires", "0") + super().end_headers() + + def log_message(self, *a): # keep the console clean; gateway prints its own + pass + + +# ── Pure gateway helpers (unit-tested in tests/gateway_selftest.py) ────────── + +WS_GUID = "258EAFA5-E914-47DA-95CA-C5AB0DC85B11" + +def ws_accept_key(client_key: str) -> str: + """RFC 6455 §1.3 Sec-WebSocket-Accept from the client's Sec-WebSocket-Key.""" + digest = hashlib.sha1((client_key + WS_GUID).encode("ascii")).digest() + return base64.b64encode(digest).decode("ascii") + + +def ws_text_frame(payload: bytes) -> bytes: + """Encode a single unmasked server→client text frame (FIN=1, opcode=0x1).""" + n = len(payload) + header = bytearray([0x81]) + if n < 126: + header.append(n) + elif n < 65536: + header.append(126) + header += struct.pack(">H", n) + else: + header.append(127) + header += struct.pack(">Q", n) + return bytes(header) + payload + + +def parse_udp_frame(raw: bytes): + """Parse one UDP tracking datagram. Returns (node_id, mac, rssi) or None.""" try: - srv.serve_forever() - except KeyboardInterrupt: - sys.exit(0) + d = json.loads(raw.decode("utf-8", "replace")) + except (ValueError, TypeError): + return None + node_id = d.get("node_id") + mac = d.get("mac") + rssi = d.get("rssi", d.get("rssi_dbm")) + if not isinstance(node_id, str) or not isinstance(mac, str): + return None + if not isinstance(rssi, (int, float)): + return None + return node_id, mac, float(rssi) + + +def build_targets(store: dict, now: float, node_ttl: float): + """Aggregate the per-MAC RSSI store into a multi-target broadcast payload. + + store: { mac: { node_id: (rssi, ts) } }. Node readings older than + `node_ttl` are dropped; a MAC with no fresh readings is omitted. + Returns [{ "mac", "ranges":[{ "node_id", "rssi_dbm" }] }]. + """ + targets = [] + for mac, nodes in store.items(): + ranges = [ + {"node_id": nid, "rssi_dbm": rssi} + for nid, (rssi, ts) in nodes.items() + if now - ts <= node_ttl + ] + if ranges: + targets.append({"mac": mac, "ranges": ranges}) + return targets + + +# ── UDP → WebSocket gateway ────────────────────────────────────────────────── + +class Gateway: + NODE_TTL = 5.0 # seconds a node's RSSI reading for a MAC stays fresh + MAC_TTL = 12.0 # seconds a MAC with no fresh readings is retained + BROADCAST_HZ = 20.0 + + def __init__(self, udp_port: int, ws_port: int): + self.udp_port = udp_port + self.ws_port = ws_port + self.store = {} # mac -> { node_id: (rssi, ts) } + self.store_lock = threading.Lock() + self.clients = set() # connected ws client sockets + self.clients_lock = threading.Lock() + + # -- UDP ingestion -------------------------------------------------------- + def udp_loop(self): + s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + s.bind(("0.0.0.0", self.udp_port)) + print(f"[gateway] UDP listening on 0.0.0.0:{self.udp_port}") + while True: + try: + raw, _addr = s.recvfrom(4096) + except OSError: + break + parsed = parse_udp_frame(raw) + if parsed is None: + continue + node_id, mac, rssi = parsed + now = time.time() + with self.store_lock: + self.store.setdefault(mac, {})[node_id] = (rssi, now) + + # -- WebSocket accept + per-client reader --------------------------------- + def ws_accept_loop(self): + srv = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + srv.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + srv.bind(("0.0.0.0", self.ws_port)) + srv.listen(16) + print(f"[gateway] WebSocket /pose on ws://0.0.0.0:{self.ws_port}") + while True: + try: + conn, _ = srv.accept() + except OSError: + break + threading.Thread(target=self._handshake_and_hold, args=(conn,), daemon=True).start() + + def _handshake_and_hold(self, conn: socket.socket): + try: + conn.settimeout(5.0) + data = b"" + while b"\r\n\r\n" not in data and len(data) < 8192: + chunk = conn.recv(1024) + if not chunk: + conn.close(); return + data += chunk + key = None + for line in data.decode("latin1").split("\r\n"): + if line.lower().startswith("sec-websocket-key:"): + key = line.split(":", 1)[1].strip() + if not key: + conn.close(); return + resp = ( + "HTTP/1.1 101 Switching Protocols\r\n" + "Upgrade: websocket\r\nConnection: Upgrade\r\n" + f"Sec-WebSocket-Accept: {ws_accept_key(key)}\r\n\r\n" + ) + conn.sendall(resp.encode("ascii")) + conn.settimeout(None) + except OSError: + conn.close(); return + with self.clients_lock: + self.clients.add(conn) + # Drain client frames only to detect disconnect (we don't parse them). + try: + while conn.recv(1024): + pass + except OSError: + pass + finally: + with self.clients_lock: + self.clients.discard(conn) + try: + conn.close() + except OSError: + pass + + # -- Broadcast loop ------------------------------------------------------- + def broadcast_loop(self): + period = 1.0 / self.BROADCAST_HZ + while True: + time.sleep(period) + now = time.time() + with self.store_lock: + # GC stale MACs. + for mac in list(self.store.keys()): + nodes = self.store[mac] + for nid in list(nodes.keys()): + if now - nodes[nid][1] > self.MAC_TTL: + del nodes[nid] + if not nodes: + del self.store[mac] + targets = build_targets(self.store, now, self.NODE_TTL) + if not targets: + continue + msg = ws_text_frame(json.dumps( + {"type": "pose", "src": "udp", "targets": targets}).encode("utf-8")) + with self.clients_lock: + dead = [] + for c in self.clients: + try: + c.sendall(msg) + except OSError: + dead.append(c) + for c in dead: + self.clients.discard(c) + + def start(self): + for fn in (self.udp_loop, self.ws_accept_loop, self.broadcast_loop): + threading.Thread(target=fn, daemon=True).start() + + +# ── Entrypoint ─────────────────────────────────────────────────────────────── + +def main(): + ap = argparse.ArgumentParser(description="RuView demo server + optional tracking gateway") + ap.add_argument("--http-port", type=int, default=int(os.environ.get("PORT", 8765))) + ap.add_argument("--gateway", action="store_true", + default=os.environ.get("RUVIEW_UDP_GATEWAY") in ("1", "true", "True"), + help="also run the UDP:5555 → WebSocket:8770 multi-target gateway") + ap.add_argument("--udp-port", type=int, default=5555) + ap.add_argument("--ws-port", type=int, default=8770) + args = ap.parse_args() + + os.chdir(REPO_ROOT) + + if args.gateway: + Gateway(args.udp_port, args.ws_port).start() + + with ThreadingHTTPServer(("127.0.0.1", args.http_port), NoCacheHandler) as srv: + print(f"serving {os.getcwd()} on http://127.0.0.1:{args.http_port}/") + print("demos:") + for d in DEMOS: + print(f" http://127.0.0.1:{args.http_port}/examples/three.js/demos/{d}") + if args.gateway: + print(f"[gateway] ACTIVE — stream UDP JSON to :{args.udp_port}, " + f"open 06-cyber-hud.html (it auto-connects ws://…:{args.ws_port}/pose)") + try: + srv.serve_forever() + except KeyboardInterrupt: + sys.exit(0) + + +if __name__ == "__main__": + main() diff --git a/examples/through-wall/wiflow_infer.py b/examples/through-wall/wiflow_infer.py index aedce4e053..9bbe391f79 100644 --- a/examples/through-wall/wiflow_infer.py +++ b/examples/through-wall/wiflow_infer.py @@ -40,6 +40,43 @@ def __call__(self, x): out = 1.0/(1.0+np.exp(-(h @ self.W[3].T + self.b[3]))) # Linear+Sigmoid -> 34 return out.reshape(17,2) +def vitals_fields(frame): + """Forward REAL vitals from the sensing frame into the /pose payload, honestly. + + The sensing server (csi.rs) computes an FFT breathing-rate estimate and emits + ``features.dominant_freq_hz`` / ``features.breathing_band_power``, plus gated + ``breathing_rate_bpm`` / ``heart_rate_bpm`` on a ``vitals`` object when present. + + - ``bpm_breath``: an explicit ``breathing_rate_bpm`` if the pipeline reports one, + else derived from the breathing-band spectral peak (``dominant_freq_hz`` inside + the 0.1-0.5 Hz band -> bpm = f*60). Omitted when there is no in-band signal. + - ``bpm_heart``: forwarded ONLY when the pipeline explicitly reports a plausible + value (40-180 bpm). WiFi heart-rate is never synthesised here. + - ``stress``: not produced by the pipeline, so it is never emitted (the HUD keeps + its own clearly-labelled synthetic stress trace). + + Every field is an *estimate* from real signal; the client already flags coarse + accuracy. Fields are simply left off when unavailable, so the HUD shows ``sim``. + """ + out = {} + vit = frame.get("vitals") or {} + feats = frame.get("features") or {} + + br = vit.get("breathing_rate_bpm") or frame.get("breathing_rate_bpm") + if not br: + f = feats.get("dominant_freq_hz") + band_power = feats.get("breathing_band_power") or 0.0 + if f is not None and 0.1 <= f <= 0.5 and band_power > 0.0: + br = f * 60.0 + if br and 5.0 <= br <= 35.0: + out["bpm_breath"] = round(float(br), 1) + + hr = vit.get("heart_rate_bpm") or frame.get("heart_rate_bpm") + if hr and 40.0 <= hr <= 180.0: + out["bpm_heart"] = round(float(hr), 1) + + return out + CLIENTS = set() LATEST = {"pose": None} @@ -67,6 +104,13 @@ async def infer_loop(model, in_url): "motion":(d.get("features",{}) or {}).get("motion_band_power"), "kps":[[round(float(x),4),round(float(y),4)] for x,y in kp], "nodes":sorted(n.get("node_id") for n in d.get("nodes",[]) if n.get("node_id") is not None)} + payload.update(vitals_fields(d)) # real breathing/heart when the feed carries them + # IPS fields: forward-only, never synthesised here. `ranges` + # is [{node_id, r_m}] and `target` {x,y,z} in the blueprint + # frame; the HUD runs its own multilateration on `ranges`. + for key in ("target", "ranges", "target_true"): + if key in d: + payload[key] = d[key] LATEST["pose"]=payload if CLIENTS: dead=[] diff --git a/v2/crates/wifi-densepose-sensing-server/src/csi.rs b/v2/crates/wifi-densepose-sensing-server/src/csi.rs index a4853c8b0e..f3080a7da2 100644 --- a/v2/crates/wifi-densepose-sensing-server/src/csi.rs +++ b/v2/crates/wifi-densepose-sensing-server/src/csi.rs @@ -11,6 +11,19 @@ use crate::vital_signs::VitalSigns; // ── ESP32 UDP frame parsers ───────────────────────────────────────────────── +/// `SENSING_CFO_SFO_DETREND=1|true` enables per-antenna phase unwrap + +/// CFO/SFO linear de-trend on every parsed CSI frame (ADR-014 sanitization, +/// SpotFi §3.1) before the phases reach feature extraction and broadcast. +/// Off by default: legacy behaviour is bit-identical unless opted in. +fn cfo_sfo_detrend_enabled() -> bool { + static ENABLED: std::sync::OnceLock = std::sync::OnceLock::new(); + *ENABLED.get_or_init(|| { + std::env::var("SENSING_CFO_SFO_DETREND") + .map(|v| v == "1" || v.eq_ignore_ascii_case("true")) + .unwrap_or(false) + }) +} + /// Parse a 32-byte edge vitals packet (magic 0xC511_0002). pub fn parse_esp32_vitals(buf: &[u8]) -> Option { if buf.len() < 32 { @@ -137,6 +150,15 @@ pub fn parse_esp32_frame(buf: &[u8]) -> Option { phases.push(q_val.atan2(i_val)); } + // Opt-in CFO/SFO sanitization: unwrap + linear de-trend each antenna's + // subcarrier row before the phases reach features/broadcast. + if cfo_sfo_detrend_enabled() && n_antennas > 0 { + wifi_densepose_signal::phase_sanitizer::unwrap_and_detrend_rows( + &mut phases, + n_antennas as usize, + ); + } + Some(Esp32Frame { magic, node_id, diff --git a/v2/crates/wifi-densepose-signal/src/aoa_music.rs b/v2/crates/wifi-densepose-signal/src/aoa_music.rs new file mode 100644 index 0000000000..0956e9516f --- /dev/null +++ b/v2/crates/wifi-densepose-signal/src/aoa_music.rs @@ -0,0 +1,553 @@ +//! Adaptive multi-antenna MUSIC angle-of-arrival estimation. +//! +//! Computes the MUSIC (Multiple Signal Classification) pseudo-spectrum +//! `P(θ) = 1 / (aᴴ(θ) Eₙ Eₙᴴ a(θ))` from multi-antenna CSI/CFR snapshots of a +//! uniform linear array, with optional forward-backward averaging and spatial +//! smoothing (for coherent multipath). +//! +//! # Aperture gate — no fabricated angles +//! Angle estimation requires spatial aperture. With `M = 1` antenna there is no +//! spatial spectrum to compute, and [`MusicEstimator::estimate`] returns +//! [`AoaError::InsufficientAperture`] so callers fall back to the legacy +//! single-channel pipeline (phase-variance tracking, coarse 2D pose). The +//! high-resolution path unlocks only when real multi-antenna data arrives. +//! +//! # Honest scope +//! This module outputs an **AoA pseudo-spectrum and peak angles** for one +//! array. It does not output positions: going from per-node AoA to (x, y, z) +//! requires multiple calibrated nodes and a separate fusion stage, and accuracy +//! depends on array calibration, aperture, and SNR — none of which this module +//! can guarantee by itself. +//! +//! # No-BLAS design +//! The workspace tests build with `--no-default-features` (no `ndarray-linalg` +//! / BLAS), so the Hermitian eigendecomposition is done in pure Rust: the M×M +//! complex Hermitian covariance `R = X + iY` is embedded as the real symmetric +//! `B = [[X, −Y], [Y, X]]` (2M×2M) and diagonalized with a cyclic Jacobi +//! sweep. For a complex eigenvector `z = u + iv` of `R`, both `(u, v)` and +//! `(−v, u)` are eigenvectors of `B` with the same eigenvalue, and for any +//! steering vector `a` (embedded as `ã = [Re a; Im a]`): +//! `ãᵀ Qₙ ã = aᴴ Eₙ Eₙᴴ a` where `Qₙ` spans the bottom `2(M−K)` real +//! eigenvectors — the noise projector is basis-independent, so eigenvalue +//! degeneracy (equal noise eigenvalues) cannot corrupt the spectrum. This +//! identity and the full pipeline were validated numerically against +//! `numpy.linalg.eigh` before porting. + +use num_complex::Complex64; +use thiserror::Error; + +/// Errors from MUSIC AoA estimation. +#[derive(Debug, Error)] +pub enum AoaError { + /// Not enough antennas for a spatial spectrum. Callers must fall back to + /// the single-channel pipeline — this module never fabricates an angle. + #[error("Insufficient aperture: MUSIC needs >= 2 antennas, got {got}")] + InsufficientAperture { got: usize }, + + /// More sources requested than the (possibly smoothed) array can resolve. + #[error("Too many sources: {sources} requested with {antennas} effective antennas")] + TooManySources { sources: usize, antennas: usize }, + + /// Malformed input (empty snapshots, ragged rows, bad config). + #[error("Bad input: {0}")] + BadInput(String), +} + +/// Configuration for the MUSIC estimator. +#[derive(Debug, Clone)] +pub struct MusicConfig { + /// Element spacing as a fraction of carrier wavelength (d/λ). 0.5 for a + /// standard half-wavelength ULA. + pub d_over_lambda: f64, + /// Number of sources (K) to resolve. Must be < effective antenna count. + pub n_sources: usize, + /// Scan grid start angle (degrees, broadside = 0). + pub grid_start_deg: f64, + /// Scan grid stop angle (degrees). + pub grid_stop_deg: f64, + /// Scan grid step (degrees). + pub grid_step_deg: f64, + /// Forward-backward averaging of the covariance (decorrelates one pair of + /// coherent paths, improves conditioning). Recommended on. + pub forward_backward: bool, + /// Spatial smoothing subarray length L (2..=M). Averages the covariance + /// over M−L+1 overlapping subarrays to decorrelate coherent multipath, at + /// the cost of reducing effective aperture to L. `None` disables. + pub subarray_len: Option, +} + +impl Default for MusicConfig { + fn default() -> Self { + Self { + d_over_lambda: 0.5, + n_sources: 1, + grid_start_deg: -90.0, + grid_stop_deg: 90.0, + grid_step_deg: 0.5, + forward_backward: true, + subarray_len: None, + } + } +} + +/// Result of a MUSIC scan: the pseudo-spectrum and its peaks. +#[derive(Debug, Clone)] +pub struct AoaSpectrum { + /// Scan grid (degrees). + pub grid_deg: Vec, + /// MUSIC pseudo-spectrum, one value per grid angle. + pub spectrum: Vec, + /// Top-K peak angles (degrees), ascending. + pub peaks_deg: Vec, + /// Covariance eigenvalues, descending (deduplicated from the real + /// embedding). The signal/noise gap is a quality diagnostic. + pub eigenvalues: Vec, + /// Effective antenna count after optional spatial smoothing. + pub effective_antennas: usize, +} + +/// MUSIC AoA estimator for a uniform linear array. +#[derive(Debug, Clone)] +pub struct MusicEstimator { + config: MusicConfig, +} + +impl MusicEstimator { + /// Create an estimator. Validates the configuration. + pub fn new(config: MusicConfig) -> Result { + if config.d_over_lambda <= 0.0 { + return Err(AoaError::BadInput("d_over_lambda must be > 0".into())); + } + if config.grid_step_deg <= 0.0 || config.grid_stop_deg <= config.grid_start_deg { + return Err(AoaError::BadInput("invalid scan grid".into())); + } + if config.n_sources == 0 { + return Err(AoaError::BadInput("n_sources must be >= 1".into())); + } + Ok(Self { config }) + } + + /// Estimate the AoA pseudo-spectrum from snapshots. + /// + /// `snapshots[t][m]` is the complex CFR sample of antenna `m` at time `t` + /// (one subcarrier, or one per-subcarrier slice treated as a snapshot). + /// + /// Returns [`AoaError::InsufficientAperture`] when `M < 2` — the adaptive + /// gate that routes single-antenna streams to the legacy pipeline. + pub fn estimate(&self, snapshots: &[Vec]) -> Result { + let t = snapshots.len(); + if t == 0 { + return Err(AoaError::BadInput("no snapshots".into())); + } + let m0 = snapshots[0].len(); + if snapshots.iter().any(|row| row.len() != m0) { + return Err(AoaError::BadInput("ragged snapshot rows".into())); + } + // ── THE GATE ──────────────────────────────────────────────────────── + if m0 < 2 { + return Err(AoaError::InsufficientAperture { got: m0 }); + } + + // Sample covariance R[i][j] = (1/T) Σ_t x[t][i] · conj(x[t][j]) + let mut r = vec![Complex64::new(0.0, 0.0); m0 * m0]; + for row in snapshots { + for i in 0..m0 { + for j in 0..m0 { + r[i * m0 + j] += row[i] * row[j].conj(); + } + } + } + let tf = t as f64; + for v in r.iter_mut() { + *v /= tf; + } + + // Optional spatial smoothing: average over overlapping subarrays. + let mut m = m0; + if let Some(l) = self.config.subarray_len { + if l < 2 || l > m0 { + return Err(AoaError::BadInput(format!( + "subarray_len {l} out of range 2..={m0}" + ))); + } + let p = m0 - l + 1; + let mut rs = vec![Complex64::new(0.0, 0.0); l * l]; + for s in 0..p { + for i in 0..l { + for j in 0..l { + rs[i * l + j] += r[(s + i) * m0 + (s + j)]; + } + } + } + let pf = p as f64; + for v in rs.iter_mut() { + *v /= pf; + } + r = rs; + m = l; + } + + if self.config.n_sources >= m { + return Err(AoaError::TooManySources { + sources: self.config.n_sources, + antennas: m, + }); + } + + // Optional forward-backward averaging: R ← (R + J·conj(R)·J)/2, + // elementwise (J R* J)[i][j] = conj(R[m−1−i][m−1−j]). + if self.config.forward_backward { + let mut rfb = vec![Complex64::new(0.0, 0.0); m * m]; + for i in 0..m { + for j in 0..m { + let fb = r[(m - 1 - i) * m + (m - 1 - j)].conj(); + rfb[i * m + j] = (r[i * m + j] + fb) * 0.5; + } + } + r = rfb; + } + + // Real symmetric embedding B = [[X, −Y], [Y, X]] of R = X + iY. + let n = 2 * m; + let mut b = vec![0.0_f64; n * n]; + for i in 0..m { + for j in 0..m { + let x = r[i * m + j].re; + let y = r[i * m + j].im; + b[i * n + j] = x; + b[i * n + (j + m)] = -y; + b[(i + m) * n + j] = y; + b[(i + m) * n + (j + m)] = x; + } + } + + let (eigvals, eigvecs) = jacobi_eigh(&mut b, n); + + // Sort eigen-indices by eigenvalue, descending. + let mut order: Vec = (0..n).collect(); + order.sort_by(|&a, &c| { + eigvals[c] + .partial_cmp(&eigvals[a]) + .unwrap_or(std::cmp::Ordering::Equal) + }); + + // Noise subspace: all real eigenvectors after the top 2K. + let k2 = 2 * self.config.n_sources; + let noise_cols: Vec = order[k2..].to_vec(); + + // Scan the grid: P(θ) = 1 / ‖Qₙᵀ ã(θ)‖². + let cfg = &self.config; + let n_grid = + ((cfg.grid_stop_deg - cfg.grid_start_deg) / cfg.grid_step_deg).floor() as usize + 1; + let mut grid_deg = Vec::with_capacity(n_grid); + let mut spectrum = Vec::with_capacity(n_grid); + let two_pi_d = 2.0 * std::f64::consts::PI * cfg.d_over_lambda; + for gi in 0..n_grid { + let theta_deg = cfg.grid_start_deg + gi as f64 * cfg.grid_step_deg; + let sin_t = theta_deg.to_radians().sin(); + // ã = [Re a; Im a], a_mm = exp(i·2π(d/λ)·mm·sinθ) + let mut a_tilde = vec![0.0_f64; n]; + for mm in 0..m { + let ph = two_pi_d * mm as f64 * sin_t; + a_tilde[mm] = ph.cos(); + a_tilde[mm + m] = ph.sin(); + } + let mut denom = 0.0_f64; + for &c in &noise_cols { + let mut dot = 0.0_f64; + for row in 0..n { + dot += a_tilde[row] * eigvecs[row * n + c]; + } + denom += dot * dot; + } + grid_deg.push(theta_deg); + spectrum.push(1.0 / denom.max(1e-18)); + } + + // Peaks: local maxima, top K by height, ascending by angle. + let mut maxima: Vec = (1..spectrum.len().saturating_sub(1)) + .filter(|&i| spectrum[i] >= spectrum[i - 1] && spectrum[i] >= spectrum[i + 1]) + .collect(); + maxima.sort_by(|&a, &c| { + spectrum[c] + .partial_cmp(&spectrum[a]) + .unwrap_or(std::cmp::Ordering::Equal) + }); + let mut peaks_deg: Vec = maxima + .into_iter() + .take(self.config.n_sources) + .map(|i| grid_deg[i]) + .collect(); + peaks_deg.sort_by(|a, c| a.partial_cmp(c).unwrap_or(std::cmp::Ordering::Equal)); + + // Deduplicate paired eigenvalues (each complex eigenvalue appears + // twice in the real embedding): take every other, descending. + let eigenvalues: Vec = order.iter().step_by(2).map(|&i| eigvals[i]).collect(); + + Ok(AoaSpectrum { + grid_deg, + spectrum, + peaks_deg, + eigenvalues, + effective_antennas: m, + }) + } +} + +/// Cyclic Jacobi eigendecomposition of a real symmetric matrix (flat, +/// row-major, `n × n`, modified in place). Returns `(eigenvalues, +/// eigenvectors)` with eigenvectors as columns of the returned flat matrix. +/// +/// O(n³) per sweep; array sizes here are tiny (n = 2M ≤ 16), so this converges +/// in a handful of sweeps. Validated against `numpy.linalg.eigh`. +pub(crate) fn jacobi_eigh(a: &mut [f64], n: usize) -> (Vec, Vec) { + const MAX_SWEEPS: usize = 64; + const TOL: f64 = 1e-12; + + let mut v = vec![0.0_f64; n * n]; + for i in 0..n { + v[i * n + i] = 1.0; + } + + for _ in 0..MAX_SWEEPS { + let mut off = 0.0_f64; + for p in 0..n.saturating_sub(1) { + for q in (p + 1)..n { + off += a[p * n + q] * a[p * n + q]; + } + } + if (2.0 * off).sqrt() < TOL { + break; + } + for p in 0..n - 1 { + for q in (p + 1)..n { + let apq = a[p * n + q]; + if apq.abs() < 1e-300 { + continue; + } + let theta = (a[q * n + q] - a[p * n + p]) / (2.0 * apq); + let t = theta.signum() / (theta.abs() + (theta * theta + 1.0).sqrt()); + let c = 1.0 / (t * t + 1.0).sqrt(); + let s = t * c; + // Column rotation + for k in 0..n { + let akp = a[k * n + p]; + let akq = a[k * n + q]; + a[k * n + p] = c * akp - s * akq; + a[k * n + q] = s * akp + c * akq; + } + // Row rotation + for k in 0..n { + let apk = a[p * n + k]; + let aqk = a[q * n + k]; + a[p * n + k] = c * apk - s * aqk; + a[q * n + k] = s * apk + c * aqk; + } + // Accumulate eigenvectors + for k in 0..n { + let vkp = v[k * n + p]; + let vkq = v[k * n + q]; + v[k * n + p] = c * vkp - s * vkq; + v[k * n + q] = s * vkp + c * vkq; + } + } + } + } + + let w: Vec = (0..n).map(|i| a[i * n + i]).collect(); + (w, v) +} + +#[cfg(test)] +mod tests { + use super::*; + + /// xorshift64* — deterministic test noise, no rand dependency. + struct Rng(u64); + impl Rng { + fn next_f64(&mut self) -> f64 { + self.0 ^= self.0 << 13; + self.0 ^= self.0 >> 7; + self.0 ^= self.0 << 17; + (self.0.wrapping_mul(0x2545_F491_4F6C_DD1D) >> 11) as f64 / (1u64 << 53) as f64 + } + /// Uniform in [-0.5, 0.5). + fn centered(&mut self) -> f64 { + self.next_f64() - 0.5 + } + } + + fn steering(m: usize, d_over_lambda: f64, theta_deg: f64) -> Vec { + let sin_t = theta_deg.to_radians().sin(); + (0..m) + .map(|mm| { + Complex64::from_polar( + 1.0, + 2.0 * std::f64::consts::PI * d_over_lambda * mm as f64 * sin_t, + ) + }) + .collect() + } + + /// Snapshots of unit-modulus random-phase sources at given angles + noise. + fn make_snapshots( + m: usize, + t: usize, + angles_deg: &[f64], + coherent: bool, + noise_amp: f64, + seed: u64, + ) -> Vec> { + let mut rng = Rng(seed); + let steer: Vec> = + angles_deg.iter().map(|&d| steering(m, 0.5, d)).collect(); + (0..t) + .map(|_| { + let s0 = + Complex64::from_polar(1.0, 2.0 * std::f64::consts::PI * rng.next_f64()); + let sources: Vec = (0..angles_deg.len()) + .map(|k| { + if coherent { + s0 * 0.9_f64.powi(k as i32) + } else if k == 0 { + s0 + } else { + Complex64::from_polar( + 1.0, + 2.0 * std::f64::consts::PI * rng.next_f64(), + ) + } + }) + .collect(); + (0..m) + .map(|mm| { + let mut x = Complex64::new( + noise_amp * rng.centered(), + noise_amp * rng.centered(), + ); + for (k, s) in sources.iter().enumerate() { + x += s * steer[k][mm]; + } + x + }) + .collect() + }) + .collect() + } + + #[test] + fn test_jacobi_known_2x2() { + // [[2, 1], [1, 2]] has eigenvalues 1 and 3. + let mut a = vec![2.0, 1.0, 1.0, 2.0]; + let (mut w, v) = jacobi_eigh(&mut a, 2); + w.sort_by(|x, y| x.partial_cmp(y).unwrap()); + assert!((w[0] - 1.0).abs() < 1e-10 && (w[1] - 3.0).abs() < 1e-10); + // Eigenvector columns orthonormal. + let dot = v[0] * v[1] + v[2] * v[3]; + assert!(dot.abs() < 1e-10); + } + + #[test] + fn test_two_uncorrelated_sources() { + let snaps = make_snapshots(6, 400, &[-20.0, 35.0], false, 0.3, 42); + let est = MusicEstimator::new(MusicConfig { + n_sources: 2, + ..Default::default() + }) + .unwrap(); + let out = est.estimate(&snaps).unwrap(); + assert_eq!(out.peaks_deg.len(), 2); + assert!( + (out.peaks_deg[0] - (-20.0)).abs() <= 1.0, + "peak 0 at {}", + out.peaks_deg[0] + ); + assert!( + (out.peaks_deg[1] - 35.0).abs() <= 1.0, + "peak 1 at {}", + out.peaks_deg[1] + ); + // Signal/noise eigenvalue gap present. + assert!(out.eigenvalues[1] / out.eigenvalues[2].max(1e-12) > 5.0); + } + + #[test] + fn test_coherent_sources_need_smoothing() { + let snaps = make_snapshots(8, 400, &[-15.0, 25.0], true, 0.15, 7); + let est = MusicEstimator::new(MusicConfig { + n_sources: 2, + subarray_len: Some(5), + ..Default::default() + }) + .unwrap(); + let out = est.estimate(&snaps).unwrap(); + assert_eq!(out.effective_antennas, 5); + assert!( + (out.peaks_deg[0] - (-15.0)).abs() <= 2.0, + "peak 0 at {}", + out.peaks_deg[0] + ); + assert!( + (out.peaks_deg[1] - 25.0).abs() <= 2.0, + "peak 1 at {}", + out.peaks_deg[1] + ); + } + + #[test] + fn test_single_source_various_angles() { + for &angle in &[0.0, -55.0, 48.0] { + let snaps = make_snapshots(4, 400, &[angle], false, 0.1, 99); + let est = MusicEstimator::new(MusicConfig::default()).unwrap(); + let out = est.estimate(&snaps).unwrap(); + assert!( + (out.peaks_deg[0] - angle).abs() <= 1.0, + "angle {angle}: est {}", + out.peaks_deg[0] + ); + } + } + + #[test] + fn test_m1_gate_refuses() { + // Single antenna: must refuse, never fabricate an angle. + let snaps: Vec> = (0..100) + .map(|i| vec![Complex64::from_polar(1.0, i as f64 * 0.1)]) + .collect(); + let est = MusicEstimator::new(MusicConfig::default()).unwrap(); + match est.estimate(&snaps) { + Err(AoaError::InsufficientAperture { got: 1 }) => {} + other => panic!("expected InsufficientAperture, got {other:?}"), + } + } + + #[test] + fn test_too_many_sources() { + let snaps = make_snapshots(3, 50, &[10.0], false, 0.1, 5); + let est = MusicEstimator::new(MusicConfig { + n_sources: 3, + ..Default::default() + }) + .unwrap(); + assert!(matches!( + est.estimate(&snaps), + Err(AoaError::TooManySources { .. }) + )); + } + + #[test] + fn test_bad_inputs() { + let est = MusicEstimator::new(MusicConfig::default()).unwrap(); + assert!(matches!(est.estimate(&[]), Err(AoaError::BadInput(_)))); + let ragged = vec![ + vec![Complex64::new(1.0, 0.0); 4], + vec![Complex64::new(1.0, 0.0); 3], + ]; + assert!(matches!(est.estimate(&ragged), Err(AoaError::BadInput(_)))); + assert!(MusicEstimator::new(MusicConfig { + n_sources: 0, + ..Default::default() + }) + .is_err()); + } +} diff --git a/v2/crates/wifi-densepose-signal/src/lib.rs b/v2/crates/wifi-densepose-signal/src/lib.rs index 1b56f4a63a..5a95652b1c 100644 --- a/v2/crates/wifi-densepose-signal/src/lib.rs +++ b/v2/crates/wifi-densepose-signal/src/lib.rs @@ -31,6 +31,7 @@ //! let processor = CsiProcessor::new(config); //! ``` +pub mod aoa_music; pub mod bvp; pub mod csi_processor; pub mod csi_ratio; @@ -39,6 +40,7 @@ pub mod fresnel; pub mod hampel; pub mod hardware_norm; pub mod motion; +pub mod multilateration; pub mod phase_sanitizer; pub mod ruvsense; pub mod spectrogram; diff --git a/v2/crates/wifi-densepose-signal/src/multilateration.rs b/v2/crates/wifi-densepose-signal/src/multilateration.rs new file mode 100644 index 0000000000..4b45a0cc29 --- /dev/null +++ b/v2/crates/wifi-densepose-signal/src/multilateration.rs @@ -0,0 +1,438 @@ +//! Range-only multilateration: node positions + target ranges → position. +//! +//! Solves `min Σᵢ (‖p − nᵢ‖ − rᵢ)²` with a closed-form linearized +//! initialization (pairwise difference of sphere equations) refined by +//! damped Gauss-Newton, using the crate's pure-Rust Cholesky solver +//! (no BLAS — builds under `--no-default-features`). +//! +//! # Honest scope +//! This solver turns **ranges into positions**; it cannot make the ranges +//! good. ESP32-class hardware has no fine ToF timestamping, so ranges derived +//! from RSSI path-loss models carry errors of *meters* in multipath indoor +//! environments — the output position inherits that error, and callers must +//! surface [`PositionEstimate::rms_residual_m`] rather than present the point +//! as exact. Measured solver behaviour (validated numerically before porting): +//! exact recovery at zero range noise; with σ = 0.1 m range noise and 6 nodes +//! spanning ~16 m horizontally but only ~2.4 m vertically, horizontal p95 +//! error ≈ 0.15 m while 3D p95 ≈ 0.6 m — **vertical DOP dominates for flat +//! (near-coplanar) node arrays**, which is why coplanar arrays must use +//! [`solve_2d`] instead of getting a fabricated z. +//! +//! # Degeneracy is refused, not fudged +//! - Fewer than `dims + 1` nodes → [`MlatError::InsufficientNodes`]. +//! - Node geometry that does not span the solve dimensions (collinear nodes in +//! a 3D solve, coplanar-z nodes in a 3D solve — the mirror-ambiguous case) → +//! [`MlatError::DegenerateGeometry`]. The solver never silently picks one of +//! two mirror solutions. + +use thiserror::Error; + +use crate::aoa_music::jacobi_eigh; +use crate::ruvsense::tomography::cholesky_solve; + +/// Errors from multilateration. +#[derive(Debug, Error)] +pub enum MlatError { + /// Not enough nodes for the requested solve dimensionality. + #[error("Insufficient nodes: need >= {needed} for a {dims}D solve, got {got}")] + InsufficientNodes { + needed: usize, + got: usize, + dims: usize, + }, + + /// Node geometry cannot resolve a unique position (collinear / coplanar). + #[error("Degenerate node geometry: {0}")] + DegenerateGeometry(String), + + /// Ranges and nodes disagree in length, or a range is not finite/positive. + #[error("Bad input: {0}")] + BadInput(String), +} + +/// Solver configuration. +#[derive(Debug, Clone)] +pub struct MlatConfig { + /// Gauss-Newton iteration cap. + pub max_iterations: usize, + /// Step-norm convergence tolerance (metres). + pub tolerance: f64, + /// Levenberg damping added to the normal matrix diagonal. + pub damping: f64, + /// Relative eigenvalue threshold below which node geometry is declared + /// degenerate (smallest/largest spread eigenvalue). + pub rank_rel_tol: f64, +} + +impl Default for MlatConfig { + fn default() -> Self { + Self { + max_iterations: 50, + tolerance: 1e-10, + damping: 1e-9, + rank_rel_tol: 1e-9, + } + } +} + +/// A solved position with quality diagnostics. +#[derive(Debug, Clone, Copy)] +pub struct PositionEstimate { + /// Estimated position (z = 0.0 for 2D solves). + pub position: [f64; 3], + /// RMS of per-node range residuals (metres) at the solution — the honest + /// quality figure to surface alongside the point. + pub rms_residual_m: f64, + /// Gauss-Newton iterations used. + pub iterations: usize, +} + +/// Solve for (x, y, z) from >= 4 nodes with 3D geometric spread. +pub fn solve_3d( + nodes: &[[f64; 3]], + ranges: &[f64], + config: &MlatConfig, +) -> Result { + solve(nodes, ranges, 3, config) +} + +/// Solve for (x, y) from >= 3 nodes with 2D geometric spread. Node z values +/// are ignored; ranges should be horizontal (or accepted as approximate when +/// target and nodes are at similar heights). This is the correct mode for +/// near-coplanar node arrays, which a 3D solve rejects as mirror-ambiguous. +pub fn solve_2d( + nodes: &[[f64; 3]], + ranges: &[f64], + config: &MlatConfig, +) -> Result { + solve(nodes, ranges, 2, config) +} + +fn solve( + nodes: &[[f64; 3]], + ranges: &[f64], + dims: usize, + config: &MlatConfig, +) -> Result { + let n = nodes.len(); + let needed = dims + 1; + if n < needed { + return Err(MlatError::InsufficientNodes { + needed, + got: n, + dims, + }); + } + if ranges.len() != n { + return Err(MlatError::BadInput(format!( + "{} nodes but {} ranges", + n, + ranges.len() + ))); + } + if ranges.iter().any(|r| !r.is_finite() || *r < 0.0) { + return Err(MlatError::BadInput("non-finite or negative range".into())); + } + + // Geometry rank check: the node spread covariance must span `dims` + // dimensions, else the position is not unique (mirror/line ambiguity). + let mut centroid = [0.0_f64; 3]; + for node in nodes { + for d in 0..dims { + centroid[d] += node[d]; + } + } + for c in centroid.iter_mut().take(dims) { + *c /= n as f64; + } + let mut cov = vec![0.0_f64; dims * dims]; + for node in nodes { + for i in 0..dims { + for j in 0..dims { + cov[i * dims + j] += (node[i] - centroid[i]) * (node[j] - centroid[j]); + } + } + } + let (eigvals, _) = jacobi_eigh(&mut cov, dims); + let ev_max = eigvals.iter().cloned().fold(f64::MIN, f64::max); + let ev_min = eigvals.iter().cloned().fold(f64::MAX, f64::min); + if ev_min < config.rank_rel_tol * ev_max.max(1e-12) { + return Err(MlatError::DegenerateGeometry(format!( + "node spread rank-deficient in {dims}D (eig min/max = {ev_min:.3e}/{ev_max:.3e})" + ))); + } + + // Closed-form init: subtract node-0 sphere equation from each other's, + // giving the linear system 2(nᵢ − n₀)ᵀ p = r₀² − rᵢ² + ‖nᵢ‖² − ‖n₀‖². + let n0 = &nodes[0]; + let r0 = ranges[0]; + let sq = |v: &[f64; 3]| -> f64 { v[..dims].iter().map(|x| x * x).sum() }; + let mut ata = vec![0.0_f64; dims * dims]; + let mut atb = vec![0.0_f64; dims]; + for i in 1..n { + let ni = &nodes[i]; + let mut row = [0.0_f64; 3]; + for d in 0..dims { + row[d] = 2.0 * (ni[d] - n0[d]); + } + let b = r0 * r0 - ranges[i] * ranges[i] + sq(ni) - sq(n0); + for a in 0..dims { + atb[a] += row[a] * b; + for c in 0..dims { + ata[a * dims + c] += row[a] * row[c]; + } + } + } + for d in 0..dims { + ata[d * dims + d] += config.damping; + } + let p0 = cholesky_solve(&mut ata, &atb, dims).ok_or_else(|| { + MlatError::DegenerateGeometry("linearized init not solvable".into()) + })?; + + // Damped Gauss-Newton refinement of min Σ (‖p − nᵢ‖ − rᵢ)². + let mut p = [0.0_f64; 3]; + p[..dims].copy_from_slice(&p0[..dims]); + let mut rms = 0.0_f64; + let mut iterations = 0; + for iter in 0..config.max_iterations { + let mut jtj = vec![0.0_f64; dims * dims]; + let mut jtf = vec![0.0_f64; dims]; + let mut sum_f2 = 0.0_f64; + for (node, &r) in nodes.iter().zip(ranges.iter()) { + let mut d = [0.0_f64; 3]; + for k in 0..dims { + d[k] = p[k] - node[k]; + } + let dist = d[..dims] + .iter() + .map(|x| x * x) + .sum::() + .sqrt() + .max(1e-12); + let f = dist - r; + sum_f2 += f * f; + for a in 0..dims { + let ja = d[a] / dist; + jtf[a] += ja * f; + for c in 0..dims { + jtj[a * dims + c] += ja * (d[c] / dist); + } + } + } + rms = (sum_f2 / n as f64).sqrt(); + for k in 0..dims { + jtj[k * dims + k] += config.damping; + } + let neg_jtf: Vec = jtf.iter().map(|v| -v).collect(); + let step = cholesky_solve(&mut jtj, &neg_jtf, dims).ok_or_else(|| { + MlatError::DegenerateGeometry("Gauss-Newton normal matrix breakdown".into()) + })?; + for k in 0..dims { + p[k] += step[k]; + } + iterations = iter + 1; + let step_norm = step.iter().map(|s| s * s).sum::().sqrt(); + if step_norm < config.tolerance { + break; + } + } + + Ok(PositionEstimate { + position: p, + rms_residual_m: rms, + iterations, + }) +} + +/// Exponential (alpha) smoothing filter for a tracked 3D position — the +/// operational anti-jitter stage between raw per-frame solves and display. +/// Measured in the numeric proof: at α = 0.35 it roughly halves mean tracking +/// error against σ = 0.2 m per-frame position noise on a slowly moving target. +#[derive(Debug, Clone)] +pub struct AlphaFilter3 { + alpha: f64, + state: Option<[f64; 3]>, +} + +impl AlphaFilter3 { + /// `alpha` in (0, 1]: higher tracks faster, lower smooths harder. + pub fn new(alpha: f64) -> Self { + Self { + alpha: alpha.clamp(1e-6, 1.0), + state: None, + } + } + + /// Feed a raw position, get the smoothed one. + pub fn update(&mut self, raw: [f64; 3]) -> [f64; 3] { + let s = match self.state { + None => raw, + Some(prev) => { + let mut s = prev; + for k in 0..3 { + s[k] += self.alpha * (raw[k] - prev[k]); + } + s + } + }; + self.state = Some(s); + s + } + + /// Clear the filter (e.g. after a track loss). + pub fn reset(&mut self) { + self.state = None; + } +} + +#[cfg(test)] +mod tests { + use super::*; + + const NODES4: [[f64; 3]; 4] = [ + [1.2, 0.5, 1.0], + [18.5, 2.1, 1.2], + [9.0, 14.2, 2.5], + [3.0, 12.0, 0.4], + ]; + + fn dist(a: &[f64; 3], b: &[f64; 3], dims: usize) -> f64 { + (0..dims).map(|k| (a[k] - b[k]).powi(2)).sum::().sqrt() + } + + #[test] + fn test_3d_exact_recovery() { + let target = [7.5, 6.2, 1.4]; + let ranges: Vec = NODES4.iter().map(|n| dist(&target, n, 3)).collect(); + let est = solve_3d(&NODES4, &ranges, &MlatConfig::default()).unwrap(); + for k in 0..3 { + assert!( + (est.position[k] - target[k]).abs() < 1e-8, + "axis {k}: {} vs {}", + est.position[k], + target[k] + ); + } + assert!(est.rms_residual_m < 1e-8); + } + + #[test] + fn test_2d_exact_recovery() { + let target = [10.0, 7.0, 0.0]; + let ranges: Vec = NODES4[..3].iter().map(|n| dist(&target, n, 2)).collect(); + let est = solve_2d(&NODES4[..3], &ranges, &MlatConfig::default()).unwrap(); + assert!((est.position[0] - 10.0).abs() < 1e-8); + assert!((est.position[1] - 7.0).abs() < 1e-8); + assert_eq!(est.position[2], 0.0); + } + + #[test] + fn test_noisy_ranges_bounded_error() { + // xorshift noise, 50 trials: horizontal p95 must stay < 0.3 m at + // sigma ~ 0.1 m (mirrors the numeric proof, which measured 0.152 m). + let mut state = 0x9E37_79B9_7F4A_7C15_u64; + let mut rand_pm = || { + // sum of 4 uniforms - 2 => approx zero-mean, sigma ~= 0.577/... scaled below + let mut s = 0.0; + for _ in 0..4 { + state ^= state << 13; + state ^= state >> 7; + state ^= state << 17; + s += (state >> 11) as f64 / (1u64 << 53) as f64; + } + (s - 2.0) * 0.173 // approx sigma 0.1 + }; + let nodes: Vec<[f64; 3]> = NODES4 + .iter() + .cloned() + .chain([[15.0, 12.0, 2.0], [2.0, 3.0, 2.8]]) + .collect(); + let target = [7.5, 6.2, 1.4]; + let mut errs: Vec = (0..50) + .map(|_| { + let ranges: Vec = nodes + .iter() + .map(|n| dist(&target, n, 3) + rand_pm()) + .collect(); + let est = solve_3d(&nodes, &ranges, &MlatConfig::default()).unwrap(); + ((est.position[0] - target[0]).powi(2) + (est.position[1] - target[1]).powi(2)) + .sqrt() + }) + .collect(); + errs.sort_by(|a, b| a.partial_cmp(b).unwrap()); + let p95 = errs[47]; + assert!(p95 < 0.3, "horizontal p95 {p95}"); + } + + #[test] + fn test_insufficient_nodes_refused() { + let ranges = [5.0, 6.0]; + assert!(matches!( + solve_3d(&NODES4[..2], &ranges, &MlatConfig::default()), + Err(MlatError::InsufficientNodes { needed: 4, .. }) + )); + assert!(matches!( + solve_2d(&NODES4[..2], &ranges, &MlatConfig::default()), + Err(MlatError::InsufficientNodes { needed: 3, .. }) + )); + } + + #[test] + fn test_collinear_nodes_refused() { + let nodes: Vec<[f64; 3]> = (0..4).map(|i| [i as f64 * 2.0, i as f64 * 2.0, 1.0]).collect(); + let target = [7.5, 6.2, 1.4]; + let ranges: Vec = nodes.iter().map(|n| dist(&target, n, 3)).collect(); + assert!(matches!( + solve_3d(&nodes, &ranges, &MlatConfig::default()), + Err(MlatError::DegenerateGeometry(_)) + )); + } + + #[test] + fn test_coplanar_z_refused_in_3d_but_fine_in_2d() { + // All nodes at z = 1.0: mirror-ambiguous in 3D, must refuse. + let nodes: Vec<[f64; 3]> = NODES4.iter().map(|n| [n[0], n[1], 1.0]).collect(); + let target = [7.5, 6.2, 1.4]; + let r3: Vec = nodes.iter().map(|n| dist(&target, n, 3)).collect(); + assert!(matches!( + solve_3d(&nodes, &r3, &MlatConfig::default()), + Err(MlatError::DegenerateGeometry(_)) + )); + // Same array is fine as a 2D solve with horizontal ranges. + let r2: Vec = nodes.iter().map(|n| dist(&target, n, 2)).collect(); + let est = solve_2d(&nodes, &r2, &MlatConfig::default()).unwrap(); + assert!((est.position[0] - 7.5).abs() < 1e-8); + assert!((est.position[1] - 6.2).abs() < 1e-8); + } + + #[test] + fn test_bad_inputs() { + let ranges3 = [1.0, 2.0, 3.0]; + assert!(matches!( + solve_3d(&NODES4, &ranges3, &MlatConfig::default()), + Err(MlatError::BadInput(_)) + )); + let bad = [1.0, f64::NAN, 3.0, 4.0]; + assert!(matches!( + solve_3d(&NODES4, &bad, &MlatConfig::default()), + Err(MlatError::BadInput(_)) + )); + } + + #[test] + fn test_alpha_filter_smooths() { + let mut f = AlphaFilter3::new(0.35); + // First sample passes through. + assert_eq!(f.update([1.0, 2.0, 3.0]), [1.0, 2.0, 3.0]); + // A step input converges monotonically toward the new value. + let mut last = f.update([2.0, 2.0, 3.0]); + assert!((last[0] - 1.35).abs() < 1e-12); + for _ in 0..50 { + last = f.update([2.0, 2.0, 3.0]); + } + assert!((last[0] - 2.0).abs() < 1e-6); + f.reset(); + assert_eq!(f.update([9.0, 9.0, 9.0]), [9.0, 9.0, 9.0]); + } +} diff --git a/v2/crates/wifi-densepose-signal/src/phase_sanitizer.rs b/v2/crates/wifi-densepose-signal/src/phase_sanitizer.rs index 65f0f090fc..6e301f15ae 100644 --- a/v2/crates/wifi-densepose-signal/src/phase_sanitizer.rs +++ b/v2/crates/wifi-densepose-signal/src/phase_sanitizer.rs @@ -88,6 +88,12 @@ pub struct PhaseSanitizerConfig { /// Valid phase range pub phase_range: (f64, f64), + + /// Remove the linear phase slope (SFO/STO) + constant offset (CFO) by + /// subtracting a per-row least-squares line fit across subcarriers. + /// Off by default (preserves the legacy pipeline behaviour). + #[serde(default)] + pub enable_cfo_sfo_removal: bool, } impl Default for PhaseSanitizerConfig { @@ -101,6 +107,7 @@ impl Default for PhaseSanitizerConfig { enable_noise_filtering: false, noise_threshold: 0.05, phase_range: (-PI, PI), + enable_cfo_sfo_removal: false, } } } @@ -197,6 +204,12 @@ impl PhaseSanitizerConfigBuilder { self } + /// Enable/disable CFO/SFO linear phase removal + pub fn enable_cfo_sfo_removal(mut self, enable: bool) -> Self { + self.config.enable_cfo_sfo_removal = enable; + self + } + /// Build the configuration pub fn build(self) -> PhaseSanitizerConfig { self.config @@ -652,6 +665,67 @@ impl PhaseSanitizer { Ok(filtered) } + /// Remove the linear phase component — SFO/STO slope + CFO constant offset — + /// from each subcarrier row by subtracting a least-squares line fit over + /// index `k = 0..N-1`. + /// + /// This is the standard CSI phase-calibration step (e.g. SpotFi §3.1): the + /// output is provably invariant to any injected linear phase `a·k + b` and + /// preserves the nonlinear multipath structure. Requires the phase to be + /// unwrapped first (call after [`Self::unwrap_phase`]). No-op unless + /// `enable_cfo_sfo_removal` is set. + pub fn remove_linear_phase( + &self, + phase_data: &Array2, + ) -> Result, PhaseSanitizationError> { + if !self.config.enable_cfo_sfo_removal { + return Ok(phase_data.clone()); + } + if phase_data.is_empty() { + return Err(PhaseSanitizationError::InvalidData( + "Cannot de-trend empty phase data".into(), + )); + } + + let mut out = phase_data.clone(); + let ncols = out.ncols(); + for i in 0..out.nrows() { + let mut row: Vec = (0..ncols).map(|j| out[[i, j]]).collect(); + Self::detrend_row(&mut row); + for (j, &val) in row.iter().enumerate() { + out[[i, j]] = val; + } + } + Ok(out) + } + + /// In-place least-squares linear de-trend of one subcarrier row. + /// Subtracts the best-fit line `slope·k + intercept` over `k = 0..N-1`. + fn detrend_row(row: &mut [f64]) { + let n = row.len(); + if n < 2 { + return; + } + let nf = n as f64; + let (mut sum_k, mut sum_kk, mut sum_y, mut sum_ky) = (0.0, 0.0, 0.0, 0.0); + for (k, &y) in row.iter().enumerate() { + let kf = k as f64; + sum_k += kf; + sum_kk += kf * kf; + sum_y += y; + sum_ky += kf * y; + } + let denom = nf * sum_kk - sum_k * sum_k; + if denom.abs() < 1e-12 { + return; + } + let slope = (nf * sum_ky - sum_k * sum_y) / denom; + let intercept = (sum_y - slope * sum_k) / nf; + for (k, y) in row.iter_mut().enumerate() { + *y -= slope * (k as f64) + intercept; + } + } + /// Complete sanitization pipeline pub fn sanitize_phase( &mut self, @@ -669,8 +743,13 @@ impl PhaseSanitizer { self.statistics.sanitization_errors += 1; })?; + // Remove CFO/SFO linear phase (no-op unless enabled) + let detrended = self.remove_linear_phase(&unwrapped).inspect_err(|_| { + self.statistics.sanitization_errors += 1; + })?; + // Remove outliers - let cleaned = self.remove_outliers(&unwrapped).inspect_err(|_| { + let cleaned = self.remove_outliers(&detrended).inspect_err(|_| { self.statistics.sanitization_errors += 1; })?; @@ -710,6 +789,30 @@ impl PhaseSanitizer { } } +/// Unwrap and CFO/SFO-de-trend a flat phase buffer of `n_rows` equal rows +/// (e.g. the per-antenna × per-subcarrier phases of one parsed CSI frame), +/// in place, without requiring callers to build an `ndarray` or a +/// [`PhaseSanitizer`]. +/// +/// Per row: 1D phase unwrapping (removes ±2π discontinuities from wrapped +/// `atan2` output) followed by least-squares linear de-trend (strips the +/// SFO/STO slope + CFO offset). Unwrap-first matters: fitting a line to +/// *wrapped* phase is meaningless once the slope exceeds π per row. +/// +/// Returns `false` (buffer untouched) if `n_rows` is zero or does not divide +/// `phases.len()`. +pub fn unwrap_and_detrend_rows(phases: &mut [f64], n_rows: usize) -> bool { + if n_rows == 0 || phases.is_empty() || phases.len() % n_rows != 0 { + return false; + } + let row_len = phases.len() / n_rows; + for row in phases.chunks_mut(row_len) { + PhaseSanitizer::unwrap_1d(row); + PhaseSanitizer::detrend_row(row); + } + true +} + #[cfg(test)] mod tests { use super::*; @@ -918,4 +1021,104 @@ mod tests { let stats = sanitizer.get_statistics(); assert_eq!(stats.total_processed, 0); } + + #[test] + fn test_cfo_sfo_linear_removal_is_invariant() { + let config = PhaseSanitizerConfig::builder() + .enable_cfo_sfo_removal(true) + .build(); + let sanitizer = PhaseSanitizer::new(config).unwrap(); + + // Nonlinear multipath phase (two tones) — the structure we must preserve. + let ncols = 56usize; + let phi_true: Vec = (0..ncols) + .map(|k| { + 0.6 * (2.0 * PI * k as f64 / 17.0).sin() + 0.3 * (2.0 * PI * k as f64 / 6.0).sin() + }) + .collect(); + + // Reference: the true phase minus its own best-fit line. + let mut reference = phi_true.clone(); + PhaseSanitizer::detrend_row(&mut reference); + + // Inject a CFO offset (b0) + SFO/STO slope (a0); output must be invariant. + let a0 = 1.3; + let b0 = -0.8; + let injected = Array2::from_shape_fn((1, ncols), |(_, k)| phi_true[k] + a0 * k as f64 + b0); + let out = sanitizer.remove_linear_phase(&injected).unwrap(); + + for k in 0..ncols { + assert!( + (out[[0, k]] - reference[k]).abs() < 1e-9, + "not invariant at k={k}: {} vs {}", + out[[0, k]], + reference[k] + ); + } + + // Residual has no linear component left: re-fitting changes nothing. + let mut residual: Vec = (0..ncols).map(|k| out[[0, k]]).collect(); + let before = residual.clone(); + PhaseSanitizer::detrend_row(&mut residual); + for k in 0..ncols { + assert!( + (residual[k] - before[k]).abs() < 1e-9, + "residual still carried a linear slope at k={k}" + ); + } + } + + #[test] + fn test_unwrap_and_detrend_rows_flat_buffer() { + // Two antenna rows, each = wrapped(true_phase + slope·k + offset). + let ncols = 32usize; + let truth: Vec = (0..ncols).map(|k| 0.4 * (k as f64 * 0.5).sin()).collect(); + let mut reference = truth.clone(); + PhaseSanitizer::detrend_row(&mut reference); + + let wrap = |v: f64| { + let mut w = v; + while w > PI { + w -= 2.0 * PI; + } + while w < -PI { + w += 2.0 * PI; + } + w + }; + let mut flat = Vec::with_capacity(2 * ncols); + for &(a0, b0) in &[(0.7, 0.3), (-0.5, -1.1)] { + for (k, &t) in truth.iter().enumerate() { + flat.push(wrap(t + a0 * k as f64 + b0)); + } + } + + assert!(unwrap_and_detrend_rows(&mut flat, 2)); + for row in 0..2 { + for k in 0..ncols { + assert!( + (flat[row * ncols + k] - reference[k]).abs() < 1e-9, + "row {row} k {k}: {} vs {}", + flat[row * ncols + k], + reference[k] + ); + } + } + + // Invalid row counts leave the buffer untouched. + let mut buf = vec![1.0, 2.0, 3.0]; + let before = buf.clone(); + assert!(!unwrap_and_detrend_rows(&mut buf, 2)); + assert!(!unwrap_and_detrend_rows(&mut buf, 0)); + assert_eq!(buf, before); + } + + #[test] + fn test_cfo_sfo_removal_disabled_is_noop() { + let config = PhaseSanitizerConfig::default(); // disabled by default + let sanitizer = PhaseSanitizer::new(config).unwrap(); + let data = Array2::from_shape_fn((2, 20), |(_, k)| 0.5 * k as f64 + 0.1); + let out = sanitizer.remove_linear_phase(&data).unwrap(); + assert_eq!(out, data, "disabled de-trend must be an exact no-op"); + } } diff --git a/v2/crates/wifi-densepose-signal/src/ruvsense/tomography.rs b/v2/crates/wifi-densepose-signal/src/ruvsense/tomography.rs index aeaae192c9..68f8af254e 100644 --- a/v2/crates/wifi-densepose-signal/src/ruvsense/tomography.rs +++ b/v2/crates/wifi-densepose-signal/src/ruvsense/tomography.rs @@ -326,6 +326,90 @@ impl RfTomographer { }) } + /// Reconstruct occupancy via Tikhonov (ridge) regularization. + /// + /// Solves `min ‖Wx − y‖² + λ‖x‖²` exactly via the normal equations + /// `(WᵀW + λI)x = Wᵀy`, using a pure-Rust Cholesky factorization — the + /// ridge normal matrix is symmetric positive-definite by construction for + /// any `λ > 0`, even when `W` is rank-deficient. Unlike the iterative + /// ISTA-L1 [`Self::reconstruct`], this is a direct one-shot solve with no + /// convergence dependence on iteration count, so successive frames of a + /// live link stream produce a temporally stable voxel field (no + /// iteration-truncation flicker). + /// + /// Trade-off (validated numerically before porting): with more links than + /// voxels the recovery is near-exact; in the typical underdetermined RTI + /// regime the ridge solution is *blurrier* than L1 but still localizes the + /// occupied support. It smooths rather than sparsifies — pick L1 for sharp + /// sparse targets, ridge for stable dense fields. + /// + /// Densities are clamped to `>= 0` (physical attenuation) after the solve; + /// the reported residual is computed with the clamped solution. + pub fn reconstruct_tikhonov( + &self, + attenuations: &[f64], + lambda: f64, + ) -> Result { + if attenuations.len() != self.weight_matrix.len() { + return Err(TomographyError::ObservationMismatch { + expected: self.weight_matrix.len(), + got: attenuations.len(), + }); + } + if !(lambda > 0.0) { + return Err(TomographyError::InvalidGrid( + "Tikhonov lambda must be > 0".into(), + )); + } + + let nv = self.n_voxels; + // Normal matrix A = WᵀW + λI and rhs = Wᵀy, accumulated from the + // sparse per-link weight rows. + let mut a = vec![0.0_f64; nv * nv]; + let mut rhs = vec![0.0_f64; nv]; + for (link_idx, weights) in self.weight_matrix.iter().enumerate() { + let y = attenuations[link_idx]; + for &(i, wi) in weights { + rhs[i] += wi * y; + for &(j, wj) in weights { + a[i * nv + j] += wi * wj; + } + } + } + for i in 0..nv { + a[i * nv + i] += lambda; + } + + let x_raw = cholesky_solve(&mut a, &rhs, nv).ok_or_else(|| { + TomographyError::InvalidGrid("ridge normal matrix not SPD (numerical breakdown)".into()) + })?; + let x: Vec = x_raw.into_iter().map(|v| v.max(0.0)).collect(); + + // RMS residual of the clamped solution. + let n_links = attenuations.len(); + let mut residual = 0.0_f64; + for (link_idx, weights) in self.weight_matrix.iter().enumerate() { + let predicted: f64 = weights.iter().map(|&(idx, w)| w * x[idx]).sum(); + let diff = predicted - attenuations[link_idx]; + residual += diff * diff; + } + residual = (residual / n_links as f64).sqrt(); + + let occupied_count = x.iter().filter(|&&d| d > 0.01).count(); + + Ok(OccupancyVolume { + densities: x, + nx: self.config.nx, + ny: self.config.ny, + nz: self.config.nz, + bounds: self.config.bounds, + occupied_count, + total_voxels: nv, + residual, + iterations: 1, + }) + } + /// Number of links in this tomographer. pub fn n_links(&self) -> usize { self.weight_matrix.len() @@ -337,6 +421,49 @@ impl RfTomographer { } } +/// Solve `A x = b` for symmetric positive-definite `A` (flat row-major, +/// `n × n`, destroyed in place) via lower Cholesky factorization + forward / +/// back substitution. Returns `None` if `A` is not SPD (non-positive pivot). +/// Validated numerically against `numpy.linalg.solve` before porting. +pub(crate) fn cholesky_solve(a: &mut [f64], b: &[f64], n: usize) -> Option> { + // In-place lower-triangular factor L (strict upper left stale, unused). + for i in 0..n { + for j in 0..=i { + let mut sum = a[i * n + j]; + for k in 0..j { + sum -= a[i * n + k] * a[j * n + k]; + } + if i == j { + if sum <= 0.0 { + return None; + } + a[i * n + i] = sum.sqrt(); + } else { + a[i * n + j] = sum / a[j * n + j]; + } + } + } + // Forward substitution: L y = b + let mut y = vec![0.0_f64; n]; + for i in 0..n { + let mut sum = b[i]; + for k in 0..i { + sum -= a[i * n + k] * y[k]; + } + y[i] = sum / a[i * n + i]; + } + // Back substitution: Lᵀ x = y + let mut x = vec![0.0_f64; n]; + for i in (0..n).rev() { + let mut sum = y[i]; + for k in (i + 1)..n { + sum -= a[k * n + i] * x[k]; + } + x[i] = sum / a[i * n + i]; + } + Some(x) +} + // --------------------------------------------------------------------------- // Weight computation (simplified ray-voxel intersection) // --------------------------------------------------------------------------- @@ -752,4 +879,102 @@ mod tests { assert!(volume.residual.is_finite()); assert!(volume.iterations > 0); } + + #[test] + fn test_cholesky_solve_known_system() { + // A = [[4, 2], [2, 3]] (SPD), b = [10, 8] → x = [1.75, 1.5] + let mut a = vec![4.0, 2.0, 2.0, 3.0]; + let x = cholesky_solve(&mut a, &[10.0, 8.0], 2).unwrap(); + assert!((x[0] - 1.75).abs() < 1e-12 && (x[1] - 1.5).abs() < 1e-12); + + // Non-SPD (negative diagonal) must be refused. + let mut bad = vec![-1.0, 0.0, 0.0, 1.0]; + assert!(cholesky_solve(&mut bad, &[1.0, 1.0], 2).is_none()); + } + + #[test] + fn test_tikhonov_forward_model_recovery() { + // Ground truth: obstruct one voxel region, synthesize y = W·x_true + // through the tomographer's own weight matrix, recover with ridge. + let links = make_square_links(); + let config = TomographyConfig { + nx: 4, + ny: 4, + nz: 1, + bounds: [0.0, 0.0, 0.0, 6.0, 6.0, 3.0], + min_links: 8, + ..Default::default() + }; + let tomo = RfTomographer::new(config, &links).unwrap(); + + let mut x_true = vec![0.0_f64; tomo.n_voxels()]; + x_true[5] = 1.0; // one interior voxel occupied + let attenuations: Vec = tomo + .weight_matrix + .iter() + .map(|w| w.iter().map(|&(i, wi)| wi * x_true[i]).sum()) + .collect(); + + let vol = tomo.reconstruct_tikhonov(&attenuations, 1e-3).unwrap(); + // The true voxel is the strongest in the reconstruction. + let argmax = vol + .densities + .iter() + .enumerate() + .max_by(|a, b| a.1.partial_cmp(b.1).unwrap()) + .unwrap() + .0; + assert_eq!(argmax, 5, "densities: {:?}", vol.densities); + assert!(vol.residual < 0.1, "residual {}", vol.residual); + assert_eq!(vol.iterations, 1); + assert!(vol.densities.iter().all(|&d| d >= 0.0)); + } + + #[test] + fn test_tikhonov_lambda_shrinks_solution() { + let links = make_square_links(); + let config = TomographyConfig { + nx: 4, + ny: 4, + nz: 1, + min_links: 8, + ..Default::default() + }; + let tomo = RfTomographer::new(config, &links).unwrap(); + let attenuations: Vec = (0..tomo.n_links()) + .map(|i| 0.3 * (i as f64 * 0.7).sin().abs()) + .collect(); + + let norm = |lam: f64| -> f64 { + let v = tomo.reconstruct_tikhonov(&attenuations, lam).unwrap(); + v.densities.iter().map(|d| d * d).sum::().sqrt() + }; + let (n_small, n_big) = (norm(0.01), norm(10.0)); + assert!( + n_big < n_small, + "lambda shrinkage violated: {n_big} !< {n_small}" + ); + } + + #[test] + fn test_tikhonov_input_validation() { + let links = make_square_links(); + let tomo = RfTomographer::new( + TomographyConfig { + min_links: 8, + ..Default::default() + }, + &links, + ) + .unwrap(); + // Wrong observation length + assert!(matches!( + tomo.reconstruct_tikhonov(&[0.0; 3], 0.1), + Err(TomographyError::ObservationMismatch { .. }) + )); + // Non-positive lambda + let y = vec![0.0; tomo.n_links()]; + assert!(tomo.reconstruct_tikhonov(&y, 0.0).is_err()); + assert!(tomo.reconstruct_tikhonov(&y, -1.0).is_err()); + } }