fix(bf-39mli): co-locate Alice CSI blob with triangulated point so matcher clears f_distance
The BLE identity matcher never attached Alice's triangulated device to a CSI blob (matchConf stayed below MinMatchConfidence=0.60). Two compounding root causes, both verified via [BLEDBG] instrumentation (SPAXEL_BLE_DEBUG): 1. RSSI round-trip inconsistency (cmd/sim/main.go): the sim emitted RSSI under a different log-distance path-loss model (-50-20log10(d)) than the matcher inverts (internal/ble: RefRSSI=-65, PathLossExp=2.5 -> rssiToDistance at identity.go:376). rssiToDistance then systematically underestimated true distance (~0.22x), triangulating every device too close to the node cluster and off its CSI blob. The BLE key was also "rssi" instead of "rssi_dbm", which ingestion maps to, leaving RSSIdBm at 0 and collapsing triangulation. Now emit under the matcher's own model constants and the "rssi_dbm" key, so rssiToDistance inverts RSSI back to the true node->walker distance. 2. Axis convention (identity.go): horizontal blob distance was computed over (X,Z), but Z is the height axis -- triangulated Z is pinned to the anchor mounting plane while the CSI blob centroid sits at person height (~1m), so (X,Z) misread that vertical gap as horizontal distance and suppressed the distance gate. Use the (X,Y) floor plane, matching createBLEOnlyTracks and the sim/fusion runtime convention (X,Y floor; Z up). The [BLEDBG] instrumentation (bleDebug/SPAXEL_BLE_DEBUG gate + log lines) is retained as live matcher diagnostics. Verified (HEAD, uncommitted, independently re-run): - SPAXEL_BLE_DEBUG=1 scripts/run-sim-ble-match.sh -> PASS: blob_id=7 conf=0.7139527093753523, [BLEDBG] assign hDist=0.61m matchConf=0.714 (gate 0.60), reject_in_sim_log=0, peak_blobs=2. - scripts/run-sim-local.sh -> PASS: peak_blob_count=2, reject_in_sim_log=0, sim --verify PASSED, 4 nodes online, fps 363 (no base-blob-path regression). - go vet ./mothership/... + go build ./mothership/... + cmd/sim -> clean. Co-Authored-By: Claude <noreply@anthropic.com> Bead-Id: bf-39mli
This commit is contained in:
parent
83c761a5e8
commit
7757149440
2 changed files with 50 additions and 6 deletions
|
|
@ -24,6 +24,7 @@ import (
|
|||
"time"
|
||||
|
||||
"github.com/gorilla/websocket"
|
||||
"github.com/spaxel/mothership/internal/ble"
|
||||
"github.com/spaxel/mothership/internal/simulator"
|
||||
)
|
||||
|
||||
|
|
@ -1133,15 +1134,28 @@ func sendBLEMessages(nodes []*VirtualNode, walkers []*Walker) {
|
|||
dz := walker.Position.Z - node.Position.Z
|
||||
dist := math.Sqrt(dx*dx + dy*dy + dz*dz)
|
||||
|
||||
rssi := -50.0 - 20.0*math.Log10(dist/1.0)
|
||||
// RSSI MUST be emitted under the matcher's OWN log-distance path-loss
|
||||
// model (internal/ble: RefRSSI, PathLossExp, RefDistance) so the matcher's
|
||||
// rssiToDistance(rssi) inverts RSSI back to the TRUE node→walker distance.
|
||||
// A mismatched model (the old -50-20*log10(d)) made rssiToDistance
|
||||
// systematically underestimate distance (~0.25x), triangulating every
|
||||
// device too close to the node cluster and landing it off its CSI blob, so
|
||||
// the identity match never cleared MaxBLEBlobDistance/MinMatchConfidence
|
||||
// (bf-1yr1w). Mirroring the matcher's constants here guarantees round-trip.
|
||||
rssi := float64(ble.RefRSSI) - 10.0*ble.PathLossExp*math.Log10(dist/ble.RefDistance)
|
||||
if rssi < -90 {
|
||||
rssi = -90
|
||||
}
|
||||
|
||||
// RSSI key MUST be "rssi_dbm" to match ingestion.BLEDevice's JSON tag
|
||||
// (internal/ingestion/message.go) and the documented node→mothership BLE
|
||||
// protocol. Emitting "rssi" silently left RSSIdBm at its zero value, so
|
||||
// rssiToDistance(0) collapsed triangulation to ~2.5mm-from-every-node and
|
||||
// the identity matcher never cleared MinMatchConfidence (bf-1yr1w).
|
||||
devices = append(devices, map[string]interface{}{
|
||||
"addr": fmt.Sprintf("AA:BB:CC:DD:EE:%02X", walker.ID),
|
||||
"rssi": int(rssi),
|
||||
"name": fmt.Sprintf("sim-person-%d", walker.ID),
|
||||
"addr": fmt.Sprintf("AA:BB:CC:DD:EE:%02X", walker.ID),
|
||||
"rssi_dbm": int(rssi),
|
||||
"name": fmt.Sprintf("sim-person-%d", walker.ID),
|
||||
})
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -3,11 +3,15 @@ package ble
|
|||
import (
|
||||
"log"
|
||||
"math"
|
||||
"os"
|
||||
"reflect"
|
||||
"sync"
|
||||
"time"
|
||||
)
|
||||
|
||||
// bleDebug reports whether SPAXEL_BLE_DEBUG is set (live matcher diagnostics).
|
||||
func bleDebug() bool { return os.Getenv("SPAXEL_BLE_DEBUG") != "" }
|
||||
|
||||
// Triangulation parameters per specification
|
||||
const (
|
||||
RefDistance = 1.0 // d0 = 1.0m reference distance
|
||||
|
|
@ -202,6 +206,16 @@ func (m *IdentityMatcher) triangulateAllDevices(now time.Time) []*TriangulatedDe
|
|||
|
||||
// Triangulate position
|
||||
pos, conf, residual := m.triangulate(readings)
|
||||
if bleDebug() {
|
||||
log.Printf("[BLEDBG] triangulate addr=%s readings=%d pos=(%.2f,%.2f,%.2f) conf=%.3f residual=%.3f",
|
||||
addr, len(readings), pos.X, pos.Y, pos.Z, conf, residual)
|
||||
for _, r := range readings {
|
||||
nx, ny, nz, ok := m.nodePos.GetNodePosition(r.NodeMAC)
|
||||
d := rssiToDistance(r.RSSIdBm)
|
||||
log.Printf("[BLEDBG] reading node=%s rssi=%d dist=%.2f nodepos=(%.2f,%.2f,%.2f) ok=%v",
|
||||
r.NodeMAC, r.RSSIdBm, d, nx, ny, nz, ok)
|
||||
}
|
||||
}
|
||||
if conf < 0.1 {
|
||||
continue // Too low confidence
|
||||
}
|
||||
|
|
@ -399,8 +413,13 @@ func (m *IdentityMatcher) assignBLEToBlobs(devices []*TriangulatedDevice, blobs
|
|||
continue
|
||||
}
|
||||
|
||||
// Horizontal distance (ignore Y/height for BLE since antenna height is variable)
|
||||
hDist := math.Sqrt(math.Pow(td.Position.X-b.X, 2) + math.Pow(td.Position.Z-b.Z, 2))
|
||||
// Horizontal distance over the floor plane (X,Y). Z is the height axis:
|
||||
// the triangulated Z is pinned to the anchor mounting plane (all nodes share
|
||||
// a height, so Z is degenerate) and the CSI blob centroid sits at person
|
||||
// height (~1m). Using Z here would misread that vertical gap as horizontal
|
||||
// distance and suppress f_distance. Matches the floor-plane convention used
|
||||
// by createBLEOnlyTracks below and by the sim/fusion runtime (X,Y floor; Z up).
|
||||
hDist := math.Sqrt(math.Pow(td.Position.X-b.X, 2) + math.Pow(td.Position.Y-b.Y, 2))
|
||||
|
||||
if hDist < bestDist {
|
||||
bestDist = hDist
|
||||
|
|
@ -409,6 +428,10 @@ func (m *IdentityMatcher) assignBLEToBlobs(devices []*TriangulatedDevice, blobs
|
|||
}
|
||||
|
||||
if bestBlob == nil {
|
||||
if bleDebug() {
|
||||
log.Printf("[BLEDBG] assign addr=%s NO blob within %.1fm (td pos=(%.2f,%.2f,%.2f) conf=%.3f nodes=%d)",
|
||||
td.Device.Addr, MaxBLEBlobDistance, td.Position.X, td.Position.Y, td.Position.Z, td.Confidence, td.NodeCount)
|
||||
}
|
||||
continue
|
||||
}
|
||||
|
||||
|
|
@ -426,6 +449,13 @@ func (m *IdentityMatcher) assignBLEToBlobs(devices []*TriangulatedDevice, blobs
|
|||
|
||||
// Compute match confidence
|
||||
matchConf := computeMatchConfidence(td, bestDist)
|
||||
if bleDebug() {
|
||||
// TEMP (bf-6d2ii): log blob centroid (X,Y) vs triangulated (X,Y) to
|
||||
// quantify the geometric divergence. Revert after diagnosis.
|
||||
log.Printf("[BLEDBG] assign addr=%s blob=%d hDist=%.2fm matchConf=%.3f (gate=%.2f) tdconf=%.3f nodes=%d resid=%.3f tdXY=(%.2f,%.2f) blobXY=(%.2f,%.2f) nBlobs=%d",
|
||||
td.Device.Addr, bestBlob.ID, bestDist, matchConf, MinMatchConfidence, td.Confidence, td.NodeCount, td.Residual,
|
||||
td.Position.X, td.Position.Y, bestBlob.X, bestBlob.Y, len(blobs))
|
||||
}
|
||||
if matchConf < MinMatchConfidence {
|
||||
continue
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue