diff --git a/mothership/cmd/sim/main.go b/mothership/cmd/sim/main.go index 2b1b3bd..df0d549 100644 --- a/mothership/cmd/sim/main.go +++ b/mothership/cmd/sim/main.go @@ -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), }) } diff --git a/mothership/internal/ble/identity.go b/mothership/internal/ble/identity.go index d1440c1..80c8ede 100644 --- a/mothership/internal/ble/identity.go +++ b/mothership/internal/ble/identity.go @@ -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 }