feat(fusion): 3D Fresnel zone weighted multi-link localization
New package mothership/internal/fusion implementing spatial localization via Fresnel zone inverse-distance weighting across multiple TX-RX links: - Grid3D: voxel grid (default 0.2m cells) with AddLinkInfluence using weight = activation / (1 + normalised_excess), where normalised_excess = excess_path_length / first_Fresnel_zone_radius - Engine: fuses LinkMotion slices into a 3D activation map, normalises, and extracts peak blobs with confidence scores - FresnelZoneRadius helper for callers choosing grid resolution - 15 tests covering edge cases, ±1m position accuracy with 4+ links, off-centre target, and performance (<50ms for 20 links) Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
jedarden
parent
bcfd1e3f16
commit
9c56a3795b
3 changed files with 764 additions and 0 deletions
192
mothership/internal/fusion/fusion.go
Normal file
192
mothership/internal/fusion/fusion.go
Normal file
|
|
@ -0,0 +1,192 @@
|
|||
package fusion
|
||||
|
||||
import (
|
||||
"math"
|
||||
"sync"
|
||||
"time"
|
||||
)
|
||||
|
||||
// LinkMotion describes one link's current motion state for 3D fusion.
|
||||
type LinkMotion struct {
|
||||
// NodeMAC is the transmitting node's MAC address.
|
||||
NodeMAC string
|
||||
// PeerMAC is the receiving node's MAC address.
|
||||
PeerMAC string
|
||||
// DeltaRMS is the motion feature amplitude (from signal.MotionFeatures).
|
||||
DeltaRMS float64
|
||||
// Motion is true when the link reports motion above threshold.
|
||||
Motion bool
|
||||
}
|
||||
|
||||
// NodePosition holds a node's 3D position in world coordinates (metres).
|
||||
type NodePosition struct {
|
||||
MAC string
|
||||
X float64
|
||||
Y float64 // height above floor
|
||||
Z float64
|
||||
}
|
||||
|
||||
// Blob is a detected presence position with a confidence score.
|
||||
type Blob struct {
|
||||
X, Y, Z float64 // world-space position (metres)
|
||||
Confidence float64 // normalised [0..1]
|
||||
}
|
||||
|
||||
// Result is returned after each fusion cycle.
|
||||
type Result struct {
|
||||
Blobs []Blob
|
||||
Timestamp time.Time
|
||||
// ActiveLinks is the number of links that contributed to this fusion.
|
||||
ActiveLinks int
|
||||
}
|
||||
|
||||
// Engine runs the multi-link 3D Fresnel zone fusion.
|
||||
type Engine struct {
|
||||
mu sync.RWMutex
|
||||
grid *Grid3D
|
||||
nodePos map[string]NodePosition
|
||||
minDelta float64 // minimum deltaRMS to use a link
|
||||
maxBlobs int
|
||||
blobThresh float64 // normalised activation threshold for peak detection
|
||||
lastResult *Result
|
||||
}
|
||||
|
||||
// Config holds optional configuration for NewEngine.
|
||||
type Config struct {
|
||||
// Room dimensions in metres.
|
||||
Width, Height, Depth float64
|
||||
// Origin of the grid's minimum corner.
|
||||
OriginX, OriginY, OriginZ float64
|
||||
// CellSize in metres (default 0.2).
|
||||
CellSize float64
|
||||
// MinDeltaRMS is the minimum link deltaRMS to include (default 0.01).
|
||||
MinDeltaRMS float64
|
||||
// MaxBlobs is the maximum number of blobs to return (default 6).
|
||||
MaxBlobs int
|
||||
// BlobThreshold is the normalised activation floor for peak detection (default 0.3).
|
||||
BlobThreshold float64
|
||||
}
|
||||
|
||||
// NewEngine creates a 3D fusion engine.
|
||||
// If cfg is nil, sensible defaults for a 10×3×10 m room are used.
|
||||
func NewEngine(cfg *Config) *Engine {
|
||||
if cfg == nil {
|
||||
cfg = &Config{
|
||||
Width: 10, Height: 3, Depth: 10,
|
||||
}
|
||||
}
|
||||
cellSize := cfg.CellSize
|
||||
if cellSize <= 0 {
|
||||
cellSize = defaultCellSize
|
||||
}
|
||||
minDelta := cfg.MinDeltaRMS
|
||||
if minDelta <= 0 {
|
||||
minDelta = 0.01
|
||||
}
|
||||
maxBlobs := cfg.MaxBlobs
|
||||
if maxBlobs <= 0 {
|
||||
maxBlobs = 6
|
||||
}
|
||||
blobThresh := cfg.BlobThreshold
|
||||
if blobThresh <= 0 {
|
||||
blobThresh = 0.3
|
||||
}
|
||||
g := NewGrid3D(cfg.Width, cfg.Height, cfg.Depth, cellSize,
|
||||
cfg.OriginX, cfg.OriginY, cfg.OriginZ)
|
||||
return &Engine{
|
||||
grid: g,
|
||||
nodePos: make(map[string]NodePosition),
|
||||
minDelta: minDelta,
|
||||
maxBlobs: maxBlobs,
|
||||
blobThresh: blobThresh,
|
||||
}
|
||||
}
|
||||
|
||||
// SetNodePosition updates a node's 3D world-space position.
|
||||
func (e *Engine) SetNodePosition(mac string, x, y, z float64) {
|
||||
e.mu.Lock()
|
||||
e.nodePos[mac] = NodePosition{MAC: mac, X: x, Y: y, Z: z}
|
||||
e.mu.Unlock()
|
||||
}
|
||||
|
||||
// RemoveNode removes a node from the position registry.
|
||||
func (e *Engine) RemoveNode(mac string) {
|
||||
e.mu.Lock()
|
||||
delete(e.nodePos, mac)
|
||||
e.mu.Unlock()
|
||||
}
|
||||
|
||||
// Fuse performs a single fusion step over the provided link motion states.
|
||||
// It returns a Result containing detected blob positions and confidence scores.
|
||||
func (e *Engine) Fuse(links []LinkMotion) *Result {
|
||||
// Snapshot node positions under read lock.
|
||||
e.mu.RLock()
|
||||
nodePos := make(map[string]NodePosition, len(e.nodePos))
|
||||
for k, v := range e.nodePos {
|
||||
nodePos[k] = v
|
||||
}
|
||||
minDelta := e.minDelta
|
||||
e.mu.RUnlock()
|
||||
|
||||
e.grid.Reset()
|
||||
|
||||
activeLinks := 0
|
||||
for _, lm := range links {
|
||||
if !lm.Motion || lm.DeltaRMS < minDelta {
|
||||
continue
|
||||
}
|
||||
posA, okA := nodePos[lm.NodeMAC]
|
||||
posB, okB := nodePos[lm.PeerMAC]
|
||||
if !okA || !okB {
|
||||
continue
|
||||
}
|
||||
e.grid.AddLinkInfluence(
|
||||
posA.X, posA.Y, posA.Z,
|
||||
posB.X, posB.Y, posB.Z,
|
||||
lm.DeltaRMS,
|
||||
)
|
||||
activeLinks++
|
||||
}
|
||||
|
||||
result := &Result{
|
||||
Timestamp: time.Now(),
|
||||
ActiveLinks: activeLinks,
|
||||
}
|
||||
|
||||
if activeLinks == 0 {
|
||||
e.mu.Lock()
|
||||
e.lastResult = result
|
||||
e.mu.Unlock()
|
||||
return result
|
||||
}
|
||||
|
||||
e.grid.Normalize()
|
||||
|
||||
rawPeaks := e.grid.Peaks(e.maxBlobs, e.blobThresh)
|
||||
blobs := make([]Blob, len(rawPeaks))
|
||||
for i, p := range rawPeaks {
|
||||
blobs[i] = Blob{X: p[0], Y: p[1], Z: p[2], Confidence: p[3]}
|
||||
}
|
||||
result.Blobs = blobs
|
||||
|
||||
e.mu.Lock()
|
||||
e.lastResult = result
|
||||
e.mu.Unlock()
|
||||
|
||||
return result
|
||||
}
|
||||
|
||||
// LastResult returns the most recent fusion result, or nil.
|
||||
func (e *Engine) LastResult() *Result {
|
||||
e.mu.RLock()
|
||||
defer e.mu.RUnlock()
|
||||
return e.lastResult
|
||||
}
|
||||
|
||||
// FresnelZoneRadius returns the maximum first-Fresnel-zone radius at the
|
||||
// midpoint of a link of length d (metres) at 2.4 GHz.
|
||||
// Useful for callers choosing grid resolution.
|
||||
func FresnelZoneRadius(linkLength float64) float64 {
|
||||
const lambda = 0.125
|
||||
return math.Sqrt(lambda * linkLength / 4.0)
|
||||
}
|
||||
352
mothership/internal/fusion/fusion_test.go
Normal file
352
mothership/internal/fusion/fusion_test.go
Normal file
|
|
@ -0,0 +1,352 @@
|
|||
package fusion
|
||||
|
||||
import (
|
||||
"math"
|
||||
"testing"
|
||||
"time"
|
||||
)
|
||||
|
||||
// ---- Grid3D unit tests ----
|
||||
|
||||
func TestGrid3D_Reset(t *testing.T) {
|
||||
g := NewGrid3D(4, 3, 4, 0.5, 0, 0, 0)
|
||||
g.AddLinkInfluence(0, 1, 0, 4, 1, 4, 0.5)
|
||||
g.Reset()
|
||||
for i, v := range g.Snapshot() {
|
||||
if v != 0 {
|
||||
t.Fatalf("cell %d not zero after Reset: %f", i, v)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func TestGrid3D_AddLinkInfluence_Degenerate(t *testing.T) {
|
||||
g := NewGrid3D(4, 3, 4, 0.5, 0, 0, 0)
|
||||
// Same TX/RX position — should be a no-op.
|
||||
g.AddLinkInfluence(2, 1, 2, 2, 1, 2, 1.0)
|
||||
for i, v := range g.Snapshot() {
|
||||
if v != 0 {
|
||||
t.Fatalf("expected zero for degenerate link, cell %d = %f", i, v)
|
||||
}
|
||||
}
|
||||
// Zero activation — no-op.
|
||||
g.AddLinkInfluence(0, 1, 0, 4, 1, 4, 0)
|
||||
for i, v := range g.Snapshot() {
|
||||
if v != 0 {
|
||||
t.Fatalf("expected zero for zero activation, cell %d = %f", i, v)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func TestGrid3D_Normalize(t *testing.T) {
|
||||
g := NewGrid3D(4, 3, 4, 1.0, 0, 0, 0)
|
||||
g.AddLinkInfluence(0, 1, 0, 4, 1, 4, 1.0)
|
||||
ok := g.Normalize()
|
||||
if !ok {
|
||||
t.Fatal("Normalize returned false on non-empty grid")
|
||||
}
|
||||
maxVal := 0.0
|
||||
for _, v := range g.Snapshot() {
|
||||
if v > maxVal {
|
||||
maxVal = v
|
||||
}
|
||||
}
|
||||
if math.Abs(maxVal-1.0) > 1e-9 {
|
||||
t.Fatalf("max after Normalize = %f, want 1.0", maxVal)
|
||||
}
|
||||
}
|
||||
|
||||
func TestGrid3D_NormalizeEmpty(t *testing.T) {
|
||||
g := NewGrid3D(4, 3, 4, 1.0, 0, 0, 0)
|
||||
if g.Normalize() {
|
||||
t.Fatal("Normalize should return false on empty grid")
|
||||
}
|
||||
}
|
||||
|
||||
// ---- Fresnel zone geometry ----
|
||||
|
||||
func TestFresnelZoneRadius(t *testing.T) {
|
||||
// For a 5 m link at 2.4 GHz: r1 = sqrt(0.125 * 5 / 4) ≈ 0.395 m
|
||||
r := FresnelZoneRadius(5.0)
|
||||
expected := math.Sqrt(0.125 * 5.0 / 4.0)
|
||||
if math.Abs(r-expected) > 1e-9 {
|
||||
t.Fatalf("FresnelZoneRadius(5) = %f, want %f", r, expected)
|
||||
}
|
||||
}
|
||||
|
||||
// ---- Fusion engine tests ----
|
||||
|
||||
func makeEngine(w, h, d float64) *Engine {
|
||||
return NewEngine(&Config{
|
||||
Width: w, Height: h, Depth: d,
|
||||
CellSize: 0.2,
|
||||
MinDeltaRMS: 0.01,
|
||||
MaxBlobs: 6,
|
||||
BlobThreshold: 0.3,
|
||||
})
|
||||
}
|
||||
|
||||
func TestEngine_NoLinks(t *testing.T) {
|
||||
e := makeEngine(10, 3, 10)
|
||||
r := e.Fuse(nil)
|
||||
if r == nil {
|
||||
t.Fatal("nil result")
|
||||
}
|
||||
if len(r.Blobs) != 0 {
|
||||
t.Fatalf("expected 0 blobs with no links, got %d", len(r.Blobs))
|
||||
}
|
||||
if r.ActiveLinks != 0 {
|
||||
t.Fatalf("expected 0 active links, got %d", r.ActiveLinks)
|
||||
}
|
||||
}
|
||||
|
||||
func TestEngine_NoMotionLinks(t *testing.T) {
|
||||
e := makeEngine(10, 3, 10)
|
||||
e.SetNodePosition("A", 0, 1, 0)
|
||||
e.SetNodePosition("B", 5, 1, 5)
|
||||
links := []LinkMotion{
|
||||
{NodeMAC: "A", PeerMAC: "B", DeltaRMS: 0.5, Motion: false},
|
||||
}
|
||||
r := e.Fuse(links)
|
||||
if len(r.Blobs) != 0 {
|
||||
t.Fatalf("expected 0 blobs when Motion=false, got %d", len(r.Blobs))
|
||||
}
|
||||
}
|
||||
|
||||
func TestEngine_BelowThresholdLink(t *testing.T) {
|
||||
e := makeEngine(10, 3, 10)
|
||||
e.SetNodePosition("A", 0, 1, 0)
|
||||
e.SetNodePosition("B", 5, 1, 5)
|
||||
links := []LinkMotion{
|
||||
{NodeMAC: "A", PeerMAC: "B", DeltaRMS: 0.005, Motion: true},
|
||||
}
|
||||
r := e.Fuse(links)
|
||||
if r.ActiveLinks != 0 {
|
||||
t.Fatalf("expected link filtered by minDelta, got %d active", r.ActiveLinks)
|
||||
}
|
||||
}
|
||||
|
||||
func TestEngine_MissingNodePosition(t *testing.T) {
|
||||
e := makeEngine(10, 3, 10)
|
||||
// Only register one of the two nodes.
|
||||
e.SetNodePosition("A", 0, 1, 0)
|
||||
links := []LinkMotion{
|
||||
{NodeMAC: "A", PeerMAC: "B", DeltaRMS: 0.5, Motion: true},
|
||||
}
|
||||
r := e.Fuse(links)
|
||||
if r.ActiveLinks != 0 {
|
||||
t.Fatalf("expected link skipped for missing position, got %d active", r.ActiveLinks)
|
||||
}
|
||||
}
|
||||
|
||||
// TestEngine_SingleLink_MidpointPeak checks that the peak lies near the
|
||||
// midpoint of a single crossing link.
|
||||
func TestEngine_SingleLink_MidpointPeak(t *testing.T) {
|
||||
e := NewEngine(&Config{
|
||||
Width: 10, Height: 3, Depth: 10,
|
||||
CellSize: 0.2, MinDeltaRMS: 0.01, MaxBlobs: 6, BlobThreshold: 0.1,
|
||||
})
|
||||
// Horizontal link at height 1 m.
|
||||
e.SetNodePosition("TX", 0, 1, 5)
|
||||
e.SetNodePosition("RX", 10, 1, 5)
|
||||
|
||||
links := []LinkMotion{
|
||||
{NodeMAC: "TX", PeerMAC: "RX", DeltaRMS: 1.0, Motion: true},
|
||||
}
|
||||
r := e.Fuse(links)
|
||||
|
||||
if len(r.Blobs) == 0 {
|
||||
t.Fatal("expected at least one blob from active link")
|
||||
}
|
||||
top := r.Blobs[0]
|
||||
if math.Abs(top.X-5.0) > 1.5 {
|
||||
t.Errorf("top blob X = %.2f, expected near 5.0", top.X)
|
||||
}
|
||||
if math.Abs(top.Z-5.0) > 1.5 {
|
||||
t.Errorf("top blob Z = %.2f, expected near 5.0", top.Z)
|
||||
}
|
||||
}
|
||||
|
||||
// TestEngine_FourLinks_PositionAccuracy is the acceptance-criterion test:
|
||||
// with 4 links whose intersection is the target, the top blob must be within
|
||||
// ±1 m of the true target position.
|
||||
func TestEngine_FourLinks_PositionAccuracy(t *testing.T) {
|
||||
const (
|
||||
targetX = 5.0
|
||||
targetZ = 5.0
|
||||
tol = 1.0
|
||||
)
|
||||
|
||||
e := NewEngine(&Config{
|
||||
Width: 10, Height: 3, Depth: 10,
|
||||
CellSize: 0.2, MinDeltaRMS: 0.01, MaxBlobs: 6, BlobThreshold: 0.2,
|
||||
})
|
||||
|
||||
// Four nodes at corners, height 1 m.
|
||||
e.SetNodePosition("N1", 0, 1, 0)
|
||||
e.SetNodePosition("N2", 10, 1, 0)
|
||||
e.SetNodePosition("N3", 10, 1, 10)
|
||||
e.SetNodePosition("N4", 0, 1, 10)
|
||||
|
||||
links := []LinkMotion{
|
||||
{NodeMAC: "N1", PeerMAC: "N3", DeltaRMS: 0.9, Motion: true}, // diagonal through centre
|
||||
{NodeMAC: "N2", PeerMAC: "N4", DeltaRMS: 0.9, Motion: true}, // diagonal through centre
|
||||
{NodeMAC: "N1", PeerMAC: "N2", DeltaRMS: 0.5, Motion: true}, // near edge
|
||||
{NodeMAC: "N3", PeerMAC: "N4", DeltaRMS: 0.5, Motion: true}, // near edge
|
||||
}
|
||||
|
||||
r := e.Fuse(links)
|
||||
|
||||
if r.ActiveLinks != 4 {
|
||||
t.Fatalf("expected 4 active links, got %d", r.ActiveLinks)
|
||||
}
|
||||
if len(r.Blobs) == 0 {
|
||||
t.Fatal("expected at least one blob from 4 crossing links")
|
||||
}
|
||||
|
||||
top := r.Blobs[0]
|
||||
dx := top.X - targetX
|
||||
dz := top.Z - targetZ
|
||||
dist2D := math.Sqrt(dx*dx + dz*dz)
|
||||
|
||||
if dist2D > tol {
|
||||
t.Errorf("top blob at (%.2f, %.2f), target (%.1f, %.1f): 2D dist=%.2f > %.1f m",
|
||||
top.X, top.Z, targetX, targetZ, dist2D, tol)
|
||||
}
|
||||
}
|
||||
|
||||
// TestEngine_FourLinks_OffCentre verifies accuracy when the target is not at
|
||||
// the geometric centre of the node layout.
|
||||
func TestEngine_FourLinks_OffCentre(t *testing.T) {
|
||||
const (
|
||||
targetX = 3.0
|
||||
targetZ = 7.0
|
||||
tol = 1.0
|
||||
)
|
||||
|
||||
e := NewEngine(&Config{
|
||||
Width: 10, Height: 3, Depth: 10,
|
||||
CellSize: 0.2, MinDeltaRMS: 0.01, MaxBlobs: 6, BlobThreshold: 0.2,
|
||||
})
|
||||
|
||||
nodes := []NodePosition{
|
||||
{"N1", 0, 1, 0}, {"N2", 10, 1, 0},
|
||||
{"N3", 10, 1, 10}, {"N4", 0, 1, 10},
|
||||
{"N5", 0, 1, 5}, {"N6", 5, 1, 10},
|
||||
}
|
||||
for _, n := range nodes {
|
||||
e.SetNodePosition(n.MAC, n.X, n.Y, n.Z)
|
||||
}
|
||||
|
||||
links := buildSyntheticLinks(nodes, targetX, 1.0, targetZ)
|
||||
|
||||
r := e.Fuse(links)
|
||||
|
||||
if len(r.Blobs) == 0 {
|
||||
t.Fatal("expected at least one blob")
|
||||
}
|
||||
top := r.Blobs[0]
|
||||
dx := top.X - targetX
|
||||
dz := top.Z - targetZ
|
||||
dist2D := math.Sqrt(dx*dx + dz*dz)
|
||||
|
||||
if dist2D > tol {
|
||||
t.Errorf("off-centre: blob at (%.2f, %.2f), target (%.1f, %.1f): dist=%.2f > %.1f m",
|
||||
top.X, top.Z, targetX, targetZ, dist2D, tol)
|
||||
}
|
||||
}
|
||||
|
||||
// buildSyntheticLinks creates link activations for all node pairs, weighted by
|
||||
// how close the target point is to each link's Fresnel zone.
|
||||
func buildSyntheticLinks(nodes []NodePosition, tx, ty, tz float64) []LinkMotion {
|
||||
var links []LinkMotion
|
||||
for i := 0; i < len(nodes); i++ {
|
||||
for j := i + 1; j < len(nodes); j++ {
|
||||
a, b := nodes[i], nodes[j]
|
||||
dAT := math.Sqrt((tx-a.X)*(tx-a.X) + (ty-a.Y)*(ty-a.Y) + (tz-a.Z)*(tz-a.Z))
|
||||
dTB := math.Sqrt((tx-b.X)*(tx-b.X) + (ty-b.Y)*(ty-b.Y) + (tz-b.Z)*(tz-b.Z))
|
||||
dAB := math.Sqrt((b.X-a.X)*(b.X-a.X) + (b.Y-a.Y)*(b.Y-a.Y) + (b.Z-a.Z)*(b.Z-a.Z))
|
||||
excess := dAT + dTB - dAB
|
||||
|
||||
const lambda = 0.125
|
||||
r1 := math.Sqrt(lambda * dAB / 4.0)
|
||||
if r1 < 0.1 {
|
||||
r1 = 0.1
|
||||
}
|
||||
ne := excess / r1
|
||||
activation := 1.0 / (1.0 + ne)
|
||||
if activation < 0.05 {
|
||||
continue
|
||||
}
|
||||
links = append(links, LinkMotion{
|
||||
NodeMAC: a.MAC,
|
||||
PeerMAC: b.MAC,
|
||||
DeltaRMS: activation,
|
||||
Motion: true,
|
||||
})
|
||||
}
|
||||
}
|
||||
return links
|
||||
}
|
||||
|
||||
// TestEngine_LastResult checks that LastResult is updated after each Fuse call.
|
||||
func TestEngine_LastResult(t *testing.T) {
|
||||
e := makeEngine(10, 3, 10)
|
||||
if e.LastResult() != nil {
|
||||
t.Fatal("expected nil before first Fuse")
|
||||
}
|
||||
e.SetNodePosition("A", 0, 1, 0)
|
||||
e.SetNodePosition("B", 10, 1, 10)
|
||||
r := e.Fuse([]LinkMotion{{NodeMAC: "A", PeerMAC: "B", DeltaRMS: 0.5, Motion: true}})
|
||||
if e.LastResult() != r {
|
||||
t.Fatal("LastResult should return the most recent result")
|
||||
}
|
||||
}
|
||||
|
||||
// TestEngine_RemoveNode ensures removed nodes are excluded from fusion.
|
||||
func TestEngine_RemoveNode(t *testing.T) {
|
||||
e := makeEngine(10, 3, 10)
|
||||
e.SetNodePosition("A", 0, 1, 0)
|
||||
e.SetNodePosition("B", 10, 1, 10)
|
||||
e.RemoveNode("B")
|
||||
r := e.Fuse([]LinkMotion{{NodeMAC: "A", PeerMAC: "B", DeltaRMS: 0.5, Motion: true}})
|
||||
if r.ActiveLinks != 0 {
|
||||
t.Fatalf("expected 0 active links after RemoveNode, got %d", r.ActiveLinks)
|
||||
}
|
||||
}
|
||||
|
||||
// TestEngine_PerformanceTwentyLinks checks that fusion over 20 links completes
|
||||
// within the 50 ms acceptance criterion.
|
||||
func TestEngine_PerformanceTwentyLinks(t *testing.T) {
|
||||
e := NewEngine(&Config{
|
||||
Width: 10, Height: 3, Depth: 10,
|
||||
CellSize: 0.2, MinDeltaRMS: 0.01, MaxBlobs: 6, BlobThreshold: 0.3,
|
||||
})
|
||||
|
||||
macs := []string{"N0", "N1", "N2", "N3", "N4", "N5", "N6", "N7", "N8", "N9"}
|
||||
xs := []float64{0, 5, 10, 0, 5, 10, 0, 5, 10, 5}
|
||||
zs := []float64{0, 0, 0, 5, 5, 5, 10, 10, 10, 5}
|
||||
for i, m := range macs {
|
||||
e.SetNodePosition(m, xs[i], 1.0, zs[i])
|
||||
}
|
||||
|
||||
var links []LinkMotion
|
||||
for i := 0; i < len(macs) && len(links) < 20; i++ {
|
||||
for j := i + 1; j < len(macs) && len(links) < 20; j++ {
|
||||
links = append(links, LinkMotion{
|
||||
NodeMAC: macs[i], PeerMAC: macs[j],
|
||||
DeltaRMS: 0.5, Motion: true,
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
const iterations = 10
|
||||
start := time.Now()
|
||||
for k := 0; k < iterations; k++ {
|
||||
e.Fuse(links)
|
||||
}
|
||||
perFuse := time.Since(start) / iterations
|
||||
const limit = 50 * time.Millisecond
|
||||
if perFuse > limit {
|
||||
t.Errorf("fusion took %v per call (limit %v)", perFuse, limit)
|
||||
}
|
||||
}
|
||||
220
mothership/internal/fusion/grid3d.go
Normal file
220
mothership/internal/fusion/grid3d.go
Normal file
|
|
@ -0,0 +1,220 @@
|
|||
// Package fusion provides 3D Fresnel zone weighted multi-link spatial localization.
|
||||
package fusion
|
||||
|
||||
import (
|
||||
"math"
|
||||
"sync"
|
||||
)
|
||||
|
||||
const defaultCellSize = 0.2 // metres
|
||||
|
||||
// Grid3D is a 3D voxel grid for accumulating link activation weights.
|
||||
// Axes: X (width), Y (height), Z (depth).
|
||||
type Grid3D struct {
|
||||
mu sync.RWMutex
|
||||
cells []float64
|
||||
nx, ny, nz int
|
||||
cellSize float64
|
||||
ox, oy, oz float64 // origin (min corner)
|
||||
}
|
||||
|
||||
// NewGrid3D creates a voxel grid covering the given room dimensions.
|
||||
// width/height/depth in metres; origin is the minimum-corner in world space.
|
||||
func NewGrid3D(width, height, depth, cellSize, ox, oy, oz float64) *Grid3D {
|
||||
if cellSize <= 0 {
|
||||
cellSize = defaultCellSize
|
||||
}
|
||||
nx := max1(int(math.Ceil(width / cellSize)))
|
||||
ny := max1(int(math.Ceil(height / cellSize)))
|
||||
nz := max1(int(math.Ceil(depth / cellSize)))
|
||||
return &Grid3D{
|
||||
cells: make([]float64, nx*ny*nz),
|
||||
nx: nx,
|
||||
ny: ny,
|
||||
nz: nz,
|
||||
cellSize: cellSize,
|
||||
ox: ox,
|
||||
oy: oy,
|
||||
oz: oz,
|
||||
}
|
||||
}
|
||||
|
||||
func max1(v int) int {
|
||||
if v < 1 {
|
||||
return 1
|
||||
}
|
||||
return v
|
||||
}
|
||||
|
||||
// Reset zeroes all voxels.
|
||||
func (g *Grid3D) Reset() {
|
||||
g.mu.Lock()
|
||||
defer g.mu.Unlock()
|
||||
for i := range g.cells {
|
||||
g.cells[i] = 0
|
||||
}
|
||||
}
|
||||
|
||||
// idx returns the flat index for voxel (ix, iy, iz).
|
||||
func (g *Grid3D) idx(ix, iy, iz int) int {
|
||||
return iz*g.ny*g.nx + iy*g.nx + ix
|
||||
}
|
||||
|
||||
// AddLinkInfluence paints the Fresnel-zone influence of a single TX-RX link.
|
||||
//
|
||||
// TX is at (ax, ay, az), RX at (bx, by, bz), all in metres.
|
||||
// activation is the deltaRMS value for this link (must be > 0).
|
||||
//
|
||||
// For each voxel centre P the contribution is:
|
||||
//
|
||||
// excess = dist(A,P) + dist(P,B) - dist(A,B) (excess path length)
|
||||
//
|
||||
// The first Fresnel zone radius at the midpoint of the link is:
|
||||
//
|
||||
// r1 = sqrt(λ·d/4) where d = dist(A,B), λ = 0.125 m (2.4 GHz)
|
||||
//
|
||||
// We define a "normalised excess" ne = excess / r1.
|
||||
// When ne <= 1 the voxel is inside the first Fresnel zone.
|
||||
//
|
||||
// Weight = activation / (1 + ne) — inverse distance to the ellipsoid surface.
|
||||
// This peaks sharply at ne=0 (voxel on the direct path) and falls off as
|
||||
// the voxel moves away from the ellipsoid.
|
||||
func (g *Grid3D) AddLinkInfluence(ax, ay, az, bx, by, bz, activation float64) {
|
||||
if activation <= 0 {
|
||||
return
|
||||
}
|
||||
|
||||
dx := bx - ax
|
||||
dy := by - ay
|
||||
dz := bz - az
|
||||
ab := math.Sqrt(dx*dx + dy*dy + dz*dz)
|
||||
if ab < 0.1 {
|
||||
return // degenerate link
|
||||
}
|
||||
|
||||
// First Fresnel zone semi-axis (λ = 0.125 m at 2.4 GHz).
|
||||
const lambda = 0.125
|
||||
r1 := math.Sqrt(lambda * ab / 4.0)
|
||||
if r1 < 0.1 {
|
||||
r1 = 0.1
|
||||
}
|
||||
|
||||
g.mu.Lock()
|
||||
defer g.mu.Unlock()
|
||||
|
||||
for iz := 0; iz < g.nz; iz++ {
|
||||
pz := g.oz + (float64(iz)+0.5)*g.cellSize
|
||||
for iy := 0; iy < g.ny; iy++ {
|
||||
py := g.oy + (float64(iy)+0.5)*g.cellSize
|
||||
for ix := 0; ix < g.nx; ix++ {
|
||||
px := g.ox + (float64(ix)+0.5)*g.cellSize
|
||||
|
||||
dAP := math.Sqrt((px-ax)*(px-ax) + (py-ay)*(py-ay) + (pz-az)*(pz-az))
|
||||
dPB := math.Sqrt((px-bx)*(px-bx) + (py-by)*(py-by) + (pz-bz)*(pz-bz))
|
||||
excess := dAP + dPB - ab
|
||||
if excess < 0 {
|
||||
excess = 0
|
||||
}
|
||||
ne := excess / r1
|
||||
weight := activation / (1.0 + ne)
|
||||
g.cells[g.idx(ix, iy, iz)] += weight
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Normalize scales the grid so the maximum voxel value is 1.0.
|
||||
// Returns false if all voxels are zero.
|
||||
func (g *Grid3D) Normalize() bool {
|
||||
g.mu.Lock()
|
||||
defer g.mu.Unlock()
|
||||
|
||||
maxVal := 0.0
|
||||
for _, v := range g.cells {
|
||||
if v > maxVal {
|
||||
maxVal = v
|
||||
}
|
||||
}
|
||||
if maxVal == 0 {
|
||||
return false
|
||||
}
|
||||
inv := 1.0 / maxVal
|
||||
for i := range g.cells {
|
||||
g.cells[i] *= inv
|
||||
}
|
||||
return true
|
||||
}
|
||||
|
||||
// Peaks returns the top-N local maxima as (x, y, z, weight) tuples.
|
||||
// A voxel is a local maximum if it exceeds threshold and is strictly greater
|
||||
// than all 26-connected neighbours.
|
||||
func (g *Grid3D) Peaks(n int, threshold float64) [][4]float64 {
|
||||
g.mu.RLock()
|
||||
defer g.mu.RUnlock()
|
||||
|
||||
type peak struct{ x, y, z, w float64 }
|
||||
var candidates []peak
|
||||
|
||||
for iz := 1; iz < g.nz-1; iz++ {
|
||||
for iy := 1; iy < g.ny-1; iy++ {
|
||||
for ix := 1; ix < g.nx-1; ix++ {
|
||||
v := g.cells[g.idx(ix, iy, iz)]
|
||||
if v < threshold {
|
||||
continue
|
||||
}
|
||||
isMax := true
|
||||
outer:
|
||||
for dz := -1; dz <= 1; dz++ {
|
||||
for dy := -1; dy <= 1; dy++ {
|
||||
for dx := -1; dx <= 1; dx++ {
|
||||
if dx == 0 && dy == 0 && dz == 0 {
|
||||
continue
|
||||
}
|
||||
if g.cells[g.idx(ix+dx, iy+dy, iz+dz)] > v {
|
||||
isMax = false
|
||||
break outer
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
if isMax {
|
||||
px := g.ox + (float64(ix)+0.5)*g.cellSize
|
||||
py := g.oy + (float64(iy)+0.5)*g.cellSize
|
||||
pz := g.oz + (float64(iz)+0.5)*g.cellSize
|
||||
candidates = append(candidates, peak{px, py, pz, v})
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Sort descending by weight (insertion sort — candidate count is small).
|
||||
for i := 1; i < len(candidates); i++ {
|
||||
for j := i; j > 0 && candidates[j].w > candidates[j-1].w; j-- {
|
||||
candidates[j], candidates[j-1] = candidates[j-1], candidates[j]
|
||||
}
|
||||
}
|
||||
|
||||
if n > len(candidates) {
|
||||
n = len(candidates)
|
||||
}
|
||||
out := make([][4]float64, n)
|
||||
for i := 0; i < n; i++ {
|
||||
c := candidates[i]
|
||||
out[i] = [4]float64{c.x, c.y, c.z, c.w}
|
||||
}
|
||||
return out
|
||||
}
|
||||
|
||||
// Dims returns (nx, ny, nz, cellSize, ox, oy, oz).
|
||||
func (g *Grid3D) Dims() (int, int, int, float64, float64, float64, float64) {
|
||||
return g.nx, g.ny, g.nz, g.cellSize, g.ox, g.oy, g.oz
|
||||
}
|
||||
|
||||
// Snapshot returns a copy of all voxel values (z-major, then y, then x).
|
||||
func (g *Grid3D) Snapshot() []float64 {
|
||||
g.mu.RLock()
|
||||
defer g.mu.RUnlock()
|
||||
out := make([]float64, len(g.cells))
|
||||
copy(out, g.cells)
|
||||
return out
|
||||
}
|
||||
Loading…
Add table
Reference in a new issue