From 59404aa18e9533508ad5e95bd714a2e0bd885d68 Mon Sep 17 00:00:00 2001 From: jedarden Date: Thu, 26 Mar 2026 23:56:02 -0400 Subject: [PATCH] feat(tracker): 3D biomechanical blob tracking with UKF MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit New package mothership/internal/tracker/ implementing full 3-D Unscented Kalman Filter tracking for human figures detected by the fusion engine. Key features: - 6-D UKF state [x, y, z, vx, vy, vz] using gonum.org/v1/gonum/mat - Biomechanical constraints: max horiz velocity 2 m/s, max vert 0.8 m/s, max acceleration 3 m/s², minimum turning radius 0.3 m - Gravity-consistent Z: separate vertical speed cap for natural motion - Blob ID assignment with persistence through up to 3 s occlusion gaps - Collision avoidance: repulsion nudge when blobs closer than 0.4 m - Posture estimation: lying (<0.4 m), seated (<0.8 m), standing/walking from centroid height + horizontal speed - 11 unit tests covering single-person tracking, occlusion recovery, gap persistence, posture transitions, and constraint enforcement Co-Authored-By: Claude Sonnet 4.6 --- mothership/go.mod | 5 +- mothership/go.sum | 35 +- mothership/internal/tracker/tracker.go | 242 ++++++++++++++ mothership/internal/tracker/tracker_test.go | 252 +++++++++++++++ mothership/internal/tracker/ukf.go | 337 ++++++++++++++++++++ 5 files changed, 866 insertions(+), 5 deletions(-) create mode 100644 mothership/internal/tracker/tracker.go create mode 100644 mothership/internal/tracker/tracker_test.go create mode 100644 mothership/internal/tracker/ukf.go diff --git a/mothership/go.mod b/mothership/go.mod index 5bca0de..a56d8bf 100644 --- a/mothership/go.mod +++ b/mothership/go.mod @@ -4,21 +4,22 @@ go 1.25.0 require ( github.com/go-chi/chi v1.5.5 + github.com/google/uuid v1.6.0 github.com/gorilla/websocket v1.5.3 github.com/hashicorp/mdns v1.0.5 + modernc.org/sqlite v1.47.0 ) require ( github.com/dustin/go-humanize v1.0.1 // indirect - github.com/google/uuid v1.6.0 // indirect github.com/mattn/go-isatty v0.0.20 // indirect github.com/miekg/dns v1.1.41 // indirect github.com/ncruces/go-strftime v1.0.0 // indirect github.com/remyoudompheng/bigfft v0.0.0-20230129092748-24d4a6f8daec // indirect golang.org/x/net v0.0.0-20210410081132-afb366fc7cd1 // indirect golang.org/x/sys v0.42.0 // indirect + gonum.org/v1/gonum v0.17.0 // indirect modernc.org/libc v1.70.0 // indirect modernc.org/mathutil v1.7.1 // indirect modernc.org/memory v1.11.0 // indirect - modernc.org/sqlite v1.47.0 // indirect ) diff --git a/mothership/go.sum b/mothership/go.sum index efcffe3..9972764 100644 --- a/mothership/go.sum +++ b/mothership/go.sum @@ -2,10 +2,14 @@ github.com/dustin/go-humanize v1.0.1 h1:GzkhY7T5VNhEkwH0PVJgjz+fX1rhBrR7pRT3mDkp github.com/dustin/go-humanize v1.0.1/go.mod h1:Mu1zIs6XwVuF/gI1OepvI0qD18qycQx+mFykh5fBlto= github.com/go-chi/chi v1.5.5 h1:vOB/HbEMt9QqBqErz07QehcOKHaWFtuj87tTDVz2qXE= github.com/go-chi/chi v1.5.5/go.mod h1:C9JqLr3tIYjDOZpzn+BCuxY8z8vmca43EeMgyZt7irw= +github.com/google/pprof v0.0.0-20250317173921-a4b03ec1a45e h1:ijClszYn+mADRFY17kjQEVQ1XRhq2/JR1M3sGqeJoxs= +github.com/google/pprof v0.0.0-20250317173921-a4b03ec1a45e/go.mod h1:boTsfXsheKC2y+lKOCMpSfarhxDeIzfZG1jqGcPl3cA= github.com/google/uuid v1.6.0 h1:NIvaJDMOsjHA8n1jAhLSgzrAzy1Hgr+hNrb57e+94F0= github.com/google/uuid v1.6.0/go.mod h1:TIyPZe4MgqvfeYDBFedMoGGpEw/LqOeaOT+nhxU+yHo= github.com/gorilla/websocket v1.5.3 h1:saDtZ6Pbx/0u+bgYQ3q96pZgCzfhKXGPqt7kZ72aNNg= github.com/gorilla/websocket v1.5.3/go.mod h1:YR8l580nyteQvAITg2hZ9XVh4b55+EU/adAjf1fMHhE= +github.com/hashicorp/golang-lru/v2 v2.0.7 h1:a+bsQ5rvGLjzHuww6tVxozPZFVghXaHOwFs4luLUK2k= +github.com/hashicorp/golang-lru/v2 v2.0.7/go.mod h1:QeFd9opnmA6QUJc5vARoKUSoFhyfM2/ZepoAG6RGpeM= github.com/hashicorp/mdns v1.0.5 h1:1M5hW1cunYeoXOqHwEb/GBDDHAFo0Yqb/uz/beC6LbE= github.com/hashicorp/mdns v1.0.5/go.mod h1:mtBihi+LeNXGtG8L9dX59gAEa12BDtBQSp4v/YAJqrc= github.com/mattn/go-isatty v0.0.20 h1:xfD0iDuEKnDkl03q4limB+vH+GxLEtL/jb4xVJSWWEY= @@ -16,28 +20,53 @@ github.com/ncruces/go-strftime v1.0.0 h1:HMFp8mLCTPp341M/ZnA4qaf7ZlsbTc+miZjCLOF github.com/ncruces/go-strftime v1.0.0/go.mod h1:Fwc5htZGVVkseilnfgOVb9mKy6w1naJmn9CehxcKcls= github.com/remyoudompheng/bigfft v0.0.0-20230129092748-24d4a6f8daec h1:W09IVJc94icq4NjY3clb7Lk8O1qJ8BdBEF8z0ibU0rE= github.com/remyoudompheng/bigfft v0.0.0-20230129092748-24d4a6f8daec/go.mod h1:qqbHyh8v60DhA7CoWK5oRCqLrMHRGoxYCSS9EjAz6Eo= +golang.org/x/mod v0.33.0 h1:tHFzIWbBifEmbwtGz65eaWyGiGZatSrT9prnU8DbVL8= +golang.org/x/mod v0.33.0/go.mod h1:swjeQEj+6r7fODbD2cqrnje9PnziFuw4bmLbBZFrQ5w= golang.org/x/net v0.0.0-20210226172049-e18ecbb05110/go.mod h1:m0MpNAwzfU5UDzcl9v0D8zg8gWTRqZa9RBIspLL5mdg= golang.org/x/net v0.0.0-20210410081132-afb366fc7cd1 h1:4qWs8cYYH6PoEFy4dfhDFgoMGkwAcETd+MmPdCPMzUc= golang.org/x/net v0.0.0-20210410081132-afb366fc7cd1/go.mod h1:9tjilg8BloeKEkVJvy7fQ90B1CfIiPueXVOjqfkSzI8= -golang.org/x/sync v0.0.0-20210220032951-036812b2e83c h1:5KslGYwFpkhGh+Q16bwMP3cOontH8FOep7tGV86Y7SQ= golang.org/x/sync v0.0.0-20210220032951-036812b2e83c/go.mod h1:RxMgew5VJxzue5/jJTE5uejpjVlOe/izrB70Jof72aM= +golang.org/x/sync v0.19.0 h1:vV+1eWNmZ5geRlYjzm2adRgW2/mcpevXNg50YZtPCE4= +golang.org/x/sync v0.19.0/go.mod h1:9KTHXmSnoGruLpwFjVSX0lNNA75CykiMECbovNTZqGI= golang.org/x/sys v0.0.0-20201119102817-f84b799fce68/go.mod h1:h1NjWce9XRLGQEsW7wpKNCjG9DtNlClVuFLEZdDNbEs= golang.org/x/sys v0.0.0-20210303074136-134d130e1a04/go.mod h1:h1NjWce9XRLGQEsW7wpKNCjG9DtNlClVuFLEZdDNbEs= golang.org/x/sys v0.0.0-20210330210617-4fbd30eecc44/go.mod h1:h1NjWce9XRLGQEsW7wpKNCjG9DtNlClVuFLEZdDNbEs= golang.org/x/sys v0.6.0/go.mod h1:oPkhp1MJrh7nUepCBck5+mAzfO9JrbApNNgaTdGDITg= -golang.org/x/sys v0.19.0 h1:q5f1RH2jigJ1MoAWp2KTp3gm5zAGFUTarQZ5U386+4o= -golang.org/x/sys v0.19.0/go.mod h1:/VUhepiaJMQUp4+oa/7Zr1D23ma6VTLIYjOOTFZPUcA= golang.org/x/sys v0.42.0 h1:omrd2nAlyT5ESRdCLYdm3+fMfNFE/+Rf4bDIQImRJeo= golang.org/x/sys v0.42.0/go.mod h1:4GL1E5IUh+htKOUEOaiffhrAeqysfVGipDYzABqnCmw= golang.org/x/term v0.0.0-20201126162022-7de9c90e9dd1/go.mod h1:bj7SfCRtBDWHUb9snDiAeCFNEtKQo2Wmx5Cou7ajbmo= golang.org/x/text v0.3.3/go.mod h1:5Zoc/QRtKVWzQhOtBMvqHzDpF6irO9z98xDceosuGiQ= golang.org/x/text v0.3.6/go.mod h1:5Zoc/QRtKVWzQhOtBMvqHzDpF6irO9z98xDceosuGiQ= golang.org/x/tools v0.0.0-20180917221912-90fa682c2a6e/go.mod h1:n7NCudcB/nEzxVGmLbDWY5pfWTLqBcC2KZ6jyYvM4mQ= +golang.org/x/tools v0.42.0 h1:uNgphsn75Tdz5Ji2q36v/nsFSfR/9BRFvqhGBaJGd5k= +golang.org/x/tools v0.42.0/go.mod h1:Ma6lCIwGZvHK6XtgbswSoWroEkhugApmsXyrUmBhfr0= +gonum.org/v1/gonum v0.17.0 h1:VbpOemQlsSMrYmn7T2OUvQ4dqxQXU+ouZFQsZOx50z4= +gonum.org/v1/gonum v0.17.0/go.mod h1:El3tOrEuMpv2UdMrbNlKEh9vd86bmQ6vqIcDwxEOc1E= +modernc.org/cc/v4 v4.27.1 h1:9W30zRlYrefrDV2JE2O8VDtJ1yPGownxciz5rrbQZis= +modernc.org/cc/v4 v4.27.1/go.mod h1:uVtb5OGqUKpoLWhqwNQo/8LwvoiEBLvZXIQ/SmO6mL0= +modernc.org/ccgo/v4 v4.32.0 h1:hjG66bI/kqIPX1b2yT6fr/jt+QedtP2fqojG2VrFuVw= +modernc.org/ccgo/v4 v4.32.0/go.mod h1:6F08EBCx5uQc38kMGl+0Nm0oWczoo1c7cgpzEry7Uc0= +modernc.org/fileutil v1.4.0 h1:j6ZzNTftVS054gi281TyLjHPp6CPHr2KCxEXjEbD6SM= +modernc.org/fileutil v1.4.0/go.mod h1:EqdKFDxiByqxLk8ozOxObDSfcVOv/54xDs/DUHdvCUU= +modernc.org/gc/v2 v2.6.5 h1:nyqdV8q46KvTpZlsw66kWqwXRHdjIlJOhG6kxiV/9xI= +modernc.org/gc/v2 v2.6.5/go.mod h1:YgIahr1ypgfe7chRuJi2gD7DBQiKSLMPgBQe9oIiito= +modernc.org/gc/v3 v3.1.2 h1:ZtDCnhonXSZexk/AYsegNRV1lJGgaNZJuKjJSWKyEqo= +modernc.org/gc/v3 v3.1.2/go.mod h1:HFK/6AGESC7Ex+EZJhJ2Gni6cTaYpSMmU/cT9RmlfYY= +modernc.org/goabi0 v0.2.0 h1:HvEowk7LxcPd0eq6mVOAEMai46V+i7Jrj13t4AzuNks= +modernc.org/goabi0 v0.2.0/go.mod h1:CEFRnnJhKvWT1c1JTI3Avm+tgOWbkOu5oPA8eH8LnMI= modernc.org/libc v1.70.0 h1:U58NawXqXbgpZ/dcdS9kMshu08aiA6b7gusEusqzNkw= modernc.org/libc v1.70.0/go.mod h1:OVmxFGP1CI/Z4L3E0Q3Mf1PDE0BucwMkcXjjLntvHJo= modernc.org/mathutil v1.7.1 h1:GCZVGXdaN8gTqB1Mf/usp1Y/hSqgI2vAGGP4jZMCxOU= modernc.org/mathutil v1.7.1/go.mod h1:4p5IwJITfppl0G4sUEDtCr4DthTaT47/N3aT6MhfgJg= modernc.org/memory v1.11.0 h1:o4QC8aMQzmcwCK3t3Ux/ZHmwFPzE6hf2Y5LbkRs+hbI= modernc.org/memory v1.11.0/go.mod h1:/JP4VbVC+K5sU2wZi9bHoq2MAkCnrt2r98UGeSK7Mjw= +modernc.org/opt v0.1.4 h1:2kNGMRiUjrp4LcaPuLY2PzUfqM/w9N23quVwhKt5Qm8= +modernc.org/opt v0.1.4/go.mod h1:03fq9lsNfvkYSfxrfUhZCWPk1lm4cq4N+Bh//bEtgns= +modernc.org/sortutil v1.2.1 h1:+xyoGf15mM3NMlPDnFqrteY07klSFxLElE2PVuWIJ7w= +modernc.org/sortutil v1.2.1/go.mod h1:7ZI3a3REbai7gzCLcotuw9AC4VZVpYMjDzETGsSMqJE= modernc.org/sqlite v1.47.0 h1:R1XyaNpoW4Et9yly+I2EeX7pBza/w+pmYee/0HJDyKk= modernc.org/sqlite v1.47.0/go.mod h1:hWjRO6Tj/5Ik8ieqxQybiEOUXy0NJFNp2tpvVpKlvig= +modernc.org/strutil v1.2.1 h1:UneZBkQA+DX2Rp35KcM69cSsNES9ly8mQWD71HKlOA0= +modernc.org/strutil v1.2.1/go.mod h1:EHkiggD70koQxjVdSBM3JKM7k6L0FbGE5eymy9i3B9A= +modernc.org/token v1.1.0 h1:Xl7Ap9dKaEs5kLoOQeQmPWevfnk/DM5qcLcYlA8ys6Y= +modernc.org/token v1.1.0/go.mod h1:UGzOrNV1mAFSEB63lOFHIpNRUVMvYTc6yu1SMY/XTDM= diff --git a/mothership/internal/tracker/tracker.go b/mothership/internal/tracker/tracker.go new file mode 100644 index 0000000..578fde6 --- /dev/null +++ b/mothership/internal/tracker/tracker.go @@ -0,0 +1,242 @@ +package tracker + +import ( + "math" + "sync" + "time" +) + +// Posture is the estimated body posture of a tracked person. +type Posture int + +const ( + PostureUnknown Posture = iota + PostureStanding // upright, slow or stationary + PostureWalking // upright, moving + PostureSeated // centroid ~0.4–0.8 m above floor + PostureLying // centroid below ~0.4 m (on floor) +) + +func (p Posture) String() string { + switch p { + case PostureStanding: + return "standing" + case PostureWalking: + return "walking" + case PostureSeated: + return "seated" + case PostureLying: + return "lying" + default: + return "unknown" + } +} + +// Blob is a tracked entity with a persistent numeric identity. +type Blob struct { + ID int + X, Y, Z float64 // world-space position, metres + VX, VY, VZ float64 // velocity, m/s + Weight float64 // detection confidence [0..1] + Posture Posture + LastSeen time.Time + // Trail holds the last TrailMaxLen positions (newest last). + Trail [][3]float64 + + ukf *UKF // internal — nil in copies returned to callers +} + +// TrailMaxLen is the maximum number of trail points kept per blob. +const TrailMaxLen = 60 + +const ( + maxAssocDist = 2.0 // m — measurement-to-track gate radius + gapTolerance = 3 * time.Second // persistence through occlusion + minSeparation = 0.4 // m — collision avoidance floor + walkThreshold = 0.3 // m/s horizontal speed → walking posture +) + +// Posture height thresholds (Y = blob centroid height above floor, metres). +const ( + lyingMaxY = 0.4 // below → lying + seatedMaxY = 0.8 // below → seated +) + +// Tracker manages a set of active 3-D blob tracks. +type Tracker struct { + mu sync.Mutex + blobs []*Blob + nextID int + lastRun time.Time +} + +// NewTracker creates an empty Tracker. +func NewTracker() *Tracker { + return &Tracker{lastRun: time.Now()} +} + +// Update runs a single tracking cycle. +// +// measurements is a slice of [x, y, z, weight] tuples sourced from fusion.Blob. +// The method predicts existing tracks, associates measurements, spawns new tracks +// for unmatched detections, applies collision avoidance, and prunes stale tracks. +// +// It returns a snapshot of currently active blobs (ukf field is nil). +func (t *Tracker) Update(measurements [][4]float64) []Blob { + t.mu.Lock() + defer t.mu.Unlock() + + now := time.Now() + dt := clampDT(now.Sub(t.lastRun).Seconds()) + t.lastRun = now + + // Predict all existing tracks. + for _, b := range t.blobs { + b.ukf.Predict(dt) + b.X, b.Y, b.Z = b.ukf.Position() + b.VX, b.VY, b.VZ = b.ukf.Velocity() + } + + // Greedy nearest-neighbour association. + assigned := make([]bool, len(measurements)) + updated := make([]bool, len(t.blobs)) + + for mi, m := range measurements { + mx, my, mz, mw := m[0], m[1], m[2], m[3] + bestIdx, bestDist := -1, maxAssocDist + for bi, b := range t.blobs { + if updated[bi] { + continue + } + if d := dist3(mx, my, mz, b.X, b.Y, b.Z); d < bestDist { + bestDist = d + bestIdx = bi + } + } + if bestIdx >= 0 { + b := t.blobs[bestIdx] + b.ukf.Update([measN]float64{mx, my, mz}) + b.X, b.Y, b.Z = b.ukf.Position() + b.VX, b.VY, b.VZ = b.ukf.Velocity() + b.Weight = mw + b.LastSeen = now + b.Trail = appendTrail3(b.Trail, b.X, b.Y, b.Z) + b.Posture = estimatePosture(b.Y, b.VX, b.VZ) + updated[bestIdx] = true + assigned[mi] = true + } + } + + // Spawn new tracks for unmatched measurements. + for mi, m := range measurements { + if assigned[mi] { + continue + } + b := &Blob{ + ID: t.nextID, + X: m[0], Y: m[1], Z: m[2], + Weight: m[3], + LastSeen: now, + Trail: [][3]float64{{m[0], m[1], m[2]}}, + Posture: PostureUnknown, + ukf: NewUKF(m[0], m[1], m[2]), + } + t.nextID++ + t.blobs = append(t.blobs, b) + } + + // Prune tracks unseen beyond gap tolerance. + live := t.blobs[:0] + for _, b := range t.blobs { + if now.Sub(b.LastSeen) < gapTolerance { + live = append(live, b) + } + } + t.blobs = live + + // Collision avoidance: push overlapping blobs apart. + applyCollisionAvoidance(t.blobs) + + // Return deep-copy snapshot (ukf field omitted). + out := make([]Blob, len(t.blobs)) + for i, b := range t.blobs { + out[i] = *b + trail := make([][3]float64, len(b.Trail)) + copy(trail, b.Trail) + out[i].Trail = trail + out[i].ukf = nil + } + return out +} + +// Reset clears all active tracks. +func (t *Tracker) Reset() { + t.mu.Lock() + t.blobs = nil + t.mu.Unlock() +} + +// ─── internal helpers ───────────────────────────────────────────────────────── + +func dist3(x1, y1, z1, x2, y2, z2 float64) float64 { + dx, dy, dz := x1-x2, y1-y2, z1-z2 + return math.Sqrt(dx*dx + dy*dy + dz*dz) +} + +func appendTrail3(trail [][3]float64, x, y, z float64) [][3]float64 { + trail = append(trail, [3]float64{x, y, z}) + if len(trail) > TrailMaxLen { + trail = trail[len(trail)-TrailMaxLen:] + } + return trail +} + +func clampDT(dt float64) float64 { + if dt < 0.01 { + return 0.01 + } + if dt > 2.0 { + return 2.0 + } + return dt +} + +// estimatePosture classifies body posture from centroid height and horizontal speed. +func estimatePosture(y, vx, vz float64) Posture { + switch { + case y < lyingMaxY: + return PostureLying + case y < seatedMaxY: + return PostureSeated + default: + if math.Sqrt(vx*vx+vz*vz) > walkThreshold { + return PostureWalking + } + return PostureStanding + } +} + +// applyCollisionAvoidance pushes co-located blobs apart in the floor plane. +// The repulsion nudge is half the overlap on each side, capped to a single pass. +func applyCollisionAvoidance(blobs []*Blob) { + for i := 0; i < len(blobs); i++ { + for j := i + 1; j < len(blobs); j++ { + a, b := blobs[i], blobs[j] + dx := a.X - b.X + dz := a.Z - b.Z + d := math.Sqrt(dx*dx + dz*dz) + if d < minSeparation && d > 1e-6 { + push := (minSeparation - d) * 0.5 / d + a.X += dx * push + a.Z += dz * push + b.X -= dx * push + b.Z -= dz * push + // Reflect the corrected position back into the UKF state. + a.ukf.X.SetVec(0, a.X) + a.ukf.X.SetVec(2, a.Z) + b.ukf.X.SetVec(0, b.X) + b.ukf.X.SetVec(2, b.Z) + } + } + } +} diff --git a/mothership/internal/tracker/tracker_test.go b/mothership/internal/tracker/tracker_test.go new file mode 100644 index 0000000..623b279 --- /dev/null +++ b/mothership/internal/tracker/tracker_test.go @@ -0,0 +1,252 @@ +package tracker + +import ( + "math" + "testing" + "time" +) + +// ─── single-person tracking ─────────────────────────────────────────────────── + +func TestSinglePersonTracking(t *testing.T) { + tr := NewTracker() + const dt = 0.1 // 10 Hz + const steps = 50 + + // Straight walk: x advances at 0.8 m/s, y=1.0 m (standing), z constant. + for i := 0; i < steps; i++ { + x := 1.0 + float64(i)*dt*0.8 + tr.lastRun = tr.lastRun.Add(-time.Duration(dt * float64(time.Second))) + blobs := tr.Update([][4]float64{{x, 1.0, 5.0, 0.9}}) + if len(blobs) != 1 { + t.Fatalf("step %d: expected 1 blob, got %d", i, len(blobs)) + } + } + + blobs := tr.Update([][4]float64{{1.0 + float64(steps)*dt*0.8, 1.0, 5.0, 0.9}}) + if len(blobs) != 1 { + t.Fatalf("expected 1 blob at end, got %d", len(blobs)) + } + b := blobs[0] + expectedX := 1.0 + float64(steps)*dt*0.8 + if math.Abs(b.X-expectedX) > 0.5 { + t.Errorf("X position error too large: got %.2f, want ≈%.2f", b.X, expectedX) + } + if len(b.Trail) == 0 { + t.Error("trail should be non-empty") + } +} + +// ─── ID persistence through gap ─────────────────────────────────────────────── + +func TestIDPersistenceThroughGap(t *testing.T) { + tr := NewTracker() + + // Establish a stable track. + var id int + for i := 0; i < 15; i++ { + tr.lastRun = tr.lastRun.Add(-100 * time.Millisecond) + blobs := tr.Update([][4]float64{{5.0, 1.0, 5.0, 0.9}}) + if i == 14 { + if len(blobs) == 0 { + t.Fatal("no blobs after track establishment") + } + id = blobs[0].ID + } + } + + // Simulate a 2 s gap — within the 3 s tolerance. + tr.lastRun = tr.lastRun.Add(-2 * time.Second) + blobs := tr.Update([][4]float64{{5.1, 1.0, 5.0, 0.8}}) + + if len(blobs) == 0 { + t.Fatal("track should persist through a 2 s gap") + } + if blobs[0].ID != id { + t.Errorf("ID changed: was %d, now %d", id, blobs[0].ID) + } +} + +// ─── gap exceeds tolerance ──────────────────────────────────────────────────── + +func TestTrackDroppedAfterLongGap(t *testing.T) { + tr := NewTracker() + + for i := 0; i < 10; i++ { + tr.lastRun = tr.lastRun.Add(-100 * time.Millisecond) + tr.Update([][4]float64{{3.0, 1.0, 3.0, 0.9}}) + } + + // Directly backdate LastSeen to simulate a 4 s gap (exceeds 3 s tolerance). + tr.mu.Lock() + for _, b := range tr.blobs { + b.LastSeen = b.LastSeen.Add(-4 * time.Second) + } + tr.mu.Unlock() + + blobs := tr.Update(nil) + if len(blobs) != 0 { + t.Errorf("expected track to be dropped after 4 s gap, got %d blobs", len(blobs)) + } +} + +// ─── posture transitions ────────────────────────────────────────────────────── + +func TestPostureTransitions(t *testing.T) { + tests := []struct { + y, vx, vz float64 + want Posture + }{ + {0.2, 0.0, 0.0, PostureLying}, + {0.6, 0.0, 0.0, PostureSeated}, + {1.0, 0.1, 0.0, PostureStanding}, + {1.0, 0.5, 0.5, PostureWalking}, + } + for _, tc := range tests { + got := estimatePosture(tc.y, tc.vx, tc.vz) + if got != tc.want { + t.Errorf("estimatePosture(y=%.1f, vx=%.1f, vz=%.1f) = %s, want %s", + tc.y, tc.vx, tc.vz, got, tc.want) + } + } +} + +// ─── velocity constraint ────────────────────────────────────────────────────── + +func TestHorizontalVelocityConstraint(t *testing.T) { + u := NewUKF(0, 1, 0) + // Inject extreme horizontal velocity. + u.X.SetVec(3, 20.0) + u.X.SetVec(5, 20.0) + u.Predict(0.1) + vx, _, vz := u.Velocity() + horizSpd := math.Sqrt(vx*vx + vz*vz) + if horizSpd > maxHorizVel+0.01 { + t.Errorf("horizontal velocity constraint violated: %.3f m/s > %.3f m/s", horizSpd, maxHorizVel) + } +} + +func TestVerticalVelocityConstraint(t *testing.T) { + u := NewUKF(0, 1, 0) + u.X.SetVec(4, 50.0) // extreme upward velocity + u.Predict(0.1) + _, vy, _ := u.Velocity() + if vy > maxVertVel+0.01 { + t.Errorf("vertical velocity constraint violated: %.3f m/s > %.3f m/s", vy, maxVertVel) + } +} + +// ─── acceleration constraint ───────────────────────────────────────────────── + +func TestAccelerationConstraint(t *testing.T) { + u := NewUKF(0, 1, 0) + // Start from rest; inject large velocity jump. + u.X.SetVec(3, 0.0) + u.X.SetVec(5, 0.0) + // Directly set post-predict state as if the filter shot to 10 m/s. + u.X.SetVec(3, 10.0) + // Re-run Predict — applyConstraints should cap the resulting velocity change. + u.Predict(0.1) + vx, _, vz := u.Velocity() + horizSpd := math.Sqrt(vx*vx + vz*vz) + // After one predict from 10 m/s, acceleration cap = 3 m/s² * 0.1 s = 0.3 m/s change. + // Speed should not exceed maxHorizVel anyway. + if horizSpd > maxHorizVel+0.01 { + t.Errorf("speed after acceleration: %.3f m/s > %.3f", horizSpd, maxHorizVel) + } +} + +// ─── collision avoidance ────────────────────────────────────────────────────── + +func TestCollisionAvoidance(t *testing.T) { + tr := NewTracker() + + // Plant two blobs at the same position. + tr.lastRun = tr.lastRun.Add(-100 * time.Millisecond) + tr.Update([][4]float64{{5.0, 1.0, 5.0, 0.9}, {5.0, 1.0, 5.0, 0.9}}) + tr.lastRun = tr.lastRun.Add(-100 * time.Millisecond) + blobs := tr.Update([][4]float64{{5.0, 1.0, 5.0, 0.9}, {5.05, 1.0, 5.0, 0.9}}) + + if len(blobs) < 2 { + t.Skip("fewer than 2 blobs — collision avoidance not applicable") + } + a, b := blobs[0], blobs[1] + dx := a.X - b.X + dz := a.Z - b.Z + d := math.Sqrt(dx*dx + dz*dz) + if d < minSeparation-0.01 { + t.Errorf("blobs too close: %.3f m < %.3f m minimum", d, minSeparation) + } +} + +// ─── occlusion recovery ─────────────────────────────────────────────────────── + +func TestOcclusionRecovery(t *testing.T) { + tr := NewTracker() + + // Establish track. + for i := 0; i < 20; i++ { + tr.lastRun = tr.lastRun.Add(-100 * time.Millisecond) + tr.Update([][4]float64{{3.0, 1.0, 3.0, 0.85}}) + } + blobs := tr.Update([][4]float64{{3.0, 1.0, 3.0, 0.85}}) + if len(blobs) == 0 { + t.Fatal("no blobs after establishment") + } + id := blobs[0].ID + + // 2 s occlusion — no measurements. + tr.lastRun = tr.lastRun.Add(-2 * time.Second) + tr.Update(nil) + + // Re-detect at nearby position. + tr.lastRun = tr.lastRun.Add(-100 * time.Millisecond) + blobs = tr.Update([][4]float64{{3.15, 1.0, 3.0, 0.85}}) + + if len(blobs) == 0 { + t.Fatal("track lost after 2 s occlusion") + } + if blobs[0].ID != id { + t.Errorf("ID changed after occlusion: was %d, now %d", id, blobs[0].ID) + } +} + +// ─── posture labels ─────────────────────────────────────────────────────────── + +func TestPostureString(t *testing.T) { + cases := map[Posture]string{ + PostureStanding: "standing", + PostureWalking: "walking", + PostureSeated: "seated", + PostureLying: "lying", + PostureUnknown: "unknown", + } + for p, want := range cases { + if got := p.String(); got != want { + t.Errorf("Posture(%d).String() = %q, want %q", p, got, want) + } + } +} + +// ─── UKF round-trip ─────────────────────────────────────────────────────────── + +func TestUKFConvergesOnStaticTarget(t *testing.T) { + u := NewUKF(5.0, 1.0, 5.0) + + // Feed 50 noisy measurements of the same position. + for i := 0; i < 50; i++ { + u.Predict(0.1) + u.Update([measN]float64{5.0, 1.0, 5.0}) + } + + x, y, z := u.Position() + if math.Abs(x-5.0) > 0.3 { + t.Errorf("X not converged: %.3f", x) + } + if math.Abs(y-1.0) > 0.2 { + t.Errorf("Y not converged: %.3f", y) + } + if math.Abs(z-5.0) > 0.3 { + t.Errorf("Z not converged: %.3f", z) + } +} diff --git a/mothership/internal/tracker/ukf.go b/mothership/internal/tracker/ukf.go new file mode 100644 index 0000000..863440c --- /dev/null +++ b/mothership/internal/tracker/ukf.go @@ -0,0 +1,337 @@ +// Package tracker provides biomechanical blob tracking using a full 3-D +// Unscented Kalman Filter with human-motion constraints. +package tracker + +import ( + "math" + + "gonum.org/v1/gonum/mat" +) + +// State vector: [x, y, z, vx, vy, vz] +// Coordinate system matches fusion.Blob: X and Z are the floor-plane axes, +// Y is height above the floor. +const ( + stateN = 6 + measN = 3 +) + +// UKF scaling parameters — α=1 keeps weights well-conditioned for n=6. +const ( + ukfAlpha = 1.0 + ukfBeta = 2.0 + ukfKappa = 0.0 +) + +// Biomechanical constraints for human motion. +const ( + maxHorizVel = 2.0 // m/s horizontal speed cap + maxVertVel = 0.8 // m/s vertical speed cap (gravity-consistent) + maxAccelHz = 3.0 // m/s² horizontal acceleration cap + minTurnRad = 0.3 // m minimum turning radius +) + +// UKF is a 6-state Unscented Kalman Filter tracking a single entity in 3-D. +type UKF struct { + X *mat.VecDense // state [x, y, z, vx, vy, vz] + P *mat.Dense // 6×6 state covariance + Q *mat.Dense // 6×6 process noise + R *mat.Dense // 3×3 measurement noise +} + +// NewUKF creates a UKF seeded at world position (x0, y0, z0) with zero velocity. +func NewUKF(x0, y0, z0 float64) *UKF { + u := &UKF{ + X: mat.NewVecDense(stateN, []float64{x0, y0, z0, 0, 0, 0}), + P: mat.NewDense(stateN, stateN, nil), + Q: mat.NewDense(stateN, stateN, nil), + R: mat.NewDense(measN, measN, nil), + } + + // Initial covariance — moderate position, lower height, high velocity uncertainty. + u.P.Set(0, 0, 0.25) + u.P.Set(1, 1, 0.09) + u.P.Set(2, 2, 0.25) + u.P.Set(3, 3, 1.0) + u.P.Set(4, 4, 0.09) + u.P.Set(5, 5, 1.0) + + // Process noise: human walking dynamics. + u.Q.Set(0, 0, 2.5e-3) + u.Q.Set(1, 1, 1.0e-3) + u.Q.Set(2, 2, 2.5e-3) + u.Q.Set(3, 3, 0.25) + u.Q.Set(4, 4, 0.04) + u.Q.Set(5, 5, 0.25) + + // Measurement noise: fusion localisation ≈0.4 m std-dev. + u.R.Set(0, 0, 0.16) + u.R.Set(1, 1, 0.16) + u.R.Set(2, 2, 0.16) + + return u +} + +// Predict performs the UKF time-update for a step of dt seconds. +func (u *UKF) Predict(dt float64) { + prevVx, prevVy, prevVz := u.X.AtVec(3), u.X.AtVec(4), u.X.AtVec(5) + + sigma, wm, wc := u.sigmaPoints() + + // Propagate sigma points through constant-velocity model. + prop := make([][]float64, len(sigma)) + for i, sp := range sigma { + prop[i] = []float64{ + sp[0] + sp[3]*dt, + sp[1] + sp[4]*dt, + sp[2] + sp[5]*dt, + sp[3], sp[4], sp[5], + } + } + + // Predicted mean. + xp := make([]float64, stateN) + for i, sp := range prop { + w := wm[i] + for j := range sp { + xp[j] += w * sp[j] + } + } + + // Predicted covariance. + pPred := mat.NewDense(stateN, stateN, nil) + dv := mat.NewVecDense(stateN, nil) + ov := mat.NewDense(stateN, stateN, nil) + for i, sp := range prop { + for j := 0; j < stateN; j++ { + dv.SetVec(j, sp[j]-xp[j]) + } + ov.Outer(wc[i], dv, dv) + pPred.Add(pPred, ov) + } + pPred.Add(pPred, u.Q) + + u.X = mat.NewVecDense(stateN, xp) + u.P = pPred + + u.applyConstraints(dt, prevVx, prevVy, prevVz) +} + +// Update performs the UKF measurement-update given observation meas=[x,y,z]. +func (u *UKF) Update(meas [measN]float64) { + sigma, wm, wc := u.sigmaPoints() + + // Predicted measurement mean (first 3 components of state). + zp := make([]float64, measN) + for i, sp := range sigma { + for j := 0; j < measN; j++ { + zp[j] += wm[i] * sp[j] + } + } + + // Innovation covariance Szz and cross-covariance Sxz. + Szz := mat.NewDense(measN, measN, nil) + Sxz := mat.NewDense(stateN, measN, nil) + zd := mat.NewVecDense(measN, nil) + xd := mat.NewVecDense(stateN, nil) + ozz := mat.NewDense(measN, measN, nil) + oxz := mat.NewDense(stateN, measN, nil) + for i, sp := range sigma { + for j := 0; j < measN; j++ { + zd.SetVec(j, sp[j]-zp[j]) + } + for j := 0; j < stateN; j++ { + xd.SetVec(j, sp[j]-u.X.AtVec(j)) + } + ozz.Outer(wc[i], zd, zd) + Szz.Add(Szz, ozz) + oxz.Outer(wc[i], xd, zd) + Sxz.Add(Sxz, oxz) + } + Szz.Add(Szz, u.R) + + // Kalman gain K = Sxz * Szz⁻¹. + SzzInv := mat.NewDense(measN, measN, nil) + if err := SzzInv.Inverse(Szz); err != nil { + return // numerically singular — skip update + } + K := mat.NewDense(stateN, measN, nil) + K.Mul(Sxz, SzzInv) + + // State update: X += K * (meas − zp). + innov := mat.NewVecDense(measN, []float64{ + meas[0] - zp[0], meas[1] - zp[1], meas[2] - zp[2], + }) + delta := mat.NewVecDense(stateN, nil) + delta.MulVec(K, innov) + u.X.AddVec(u.X, delta) + + // Covariance update: P = P − K*Szz*Kᵀ. + KSzz := mat.NewDense(stateN, measN, nil) + KSzz.Mul(K, Szz) + KSzzKt := mat.NewDense(stateN, stateN, nil) + KSzzKt.Mul(KSzz, K.T()) + newP := mat.NewDense(stateN, stateN, nil) + newP.Sub(u.P, KSzzKt) + symmetrizePD(newP) + u.P = newP +} + +// Position returns the estimated (x, y, z) position in metres. +func (u *UKF) Position() (x, y, z float64) { + return u.X.AtVec(0), u.X.AtVec(1), u.X.AtVec(2) +} + +// Velocity returns the estimated (vx, vy, vz) velocity in m/s. +func (u *UKF) Velocity() (vx, vy, vz float64) { + return u.X.AtVec(3), u.X.AtVec(4), u.X.AtVec(5) +} + +// ─── helpers ───────────────────────────────────────────────────────────────── + +// sigmaPoints generates 2n+1 sigma points with their mean and covariance weights. +func (u *UKF) sigmaPoints() (sigma [][]float64, wm, wc []float64) { + n := float64(stateN) + lambda := ukfAlpha*ukfAlpha*(n+ukfKappa) - n + c := n + lambda // = 6 when alpha=1, kappa=0 + + // mat.Cholesky requires a mat.Symmetric; build c*P as SymDense. + scaledP := mat.NewSymDense(stateN, nil) + for i := 0; i < stateN; i++ { + for j := i; j < stateN; j++ { + v := c * u.P.At(i, j) + scaledP.SetSym(i, j, v) + } + } + ensurePDSym(scaledP) + + var chol mat.Cholesky + if !chol.Factorize(scaledP) { + // Fallback to small scaled identity. + scaledP = mat.NewSymDense(stateN, nil) + for i := 0; i < stateN; i++ { + scaledP.SetSym(i, i, c*1e-4) + } + chol.Factorize(scaledP) + } + L := mat.NewTriDense(stateN, mat.Lower, nil) + chol.LTo(L) + + xd := u.X.RawVector().Data + sigma = make([][]float64, 2*stateN+1) + sigma[0] = make([]float64, stateN) + copy(sigma[0], xd) + for i := 0; i < stateN; i++ { + plus := make([]float64, stateN) + minus := make([]float64, stateN) + for j := 0; j < stateN; j++ { + plus[j] = xd[j] + L.At(j, i) + minus[j] = xd[j] - L.At(j, i) + } + sigma[1+i] = plus + sigma[1+stateN+i] = minus + } + + wm0 := lambda / c + wc0 := wm0 + (1 - ukfAlpha*ukfAlpha + ukfBeta) + wi := 0.5 / c + wm = make([]float64, 2*stateN+1) + wc = make([]float64, 2*stateN+1) + wm[0] = wm0 + wc[0] = wc0 + for i := 1; i <= 2*stateN; i++ { + wm[i] = wi + wc[i] = wi + } + return +} + +// applyConstraints enforces biomechanical limits given the pre-predict velocity. +func (u *UKF) applyConstraints(dt, prevVx, prevVy, prevVz float64) { + vx := u.X.AtVec(3) + vy := u.X.AtVec(4) + vz := u.X.AtVec(5) + + if dt > 1e-6 { + // Horizontal acceleration cap. + dvx := vx - prevVx + dvz := vz - prevVz + dv := math.Sqrt(dvx*dvx + dvz*dvz) + if dv/dt > maxAccelHz { + s := maxAccelHz * dt / dv + vx = prevVx + dvx*s + vz = prevVz + dvz*s + } + + // Turning radius constraint (only when moving). + horizSpd := math.Sqrt(vx*vx + vz*vz) + prevHorizSpd := math.Sqrt(prevVx*prevVx + prevVz*prevVz) + if horizSpd > 0.15 && prevHorizSpd > 0.15 { + prevHead := math.Atan2(prevVz, prevVx) + newHead := math.Atan2(vz, vx) + dHead := angleWrap(newHead - prevHead) + maxTurn := horizSpd * dt / minTurnRad + if math.Abs(dHead) > maxTurn { + limited := prevHead + math.Copysign(maxTurn, dHead) + vx = horizSpd * math.Cos(limited) + vz = horizSpd * math.Sin(limited) + } + } + } + + // Horizontal speed cap. + if hs := math.Sqrt(vx*vx + vz*vz); hs > maxHorizVel { + s := maxHorizVel / hs + vx *= s + vz *= s + } + + // Vertical speed cap (gravity-consistent — limits upward and downward). + if vy > maxVertVel { + vy = maxVertVel + } else if vy < -maxVertVel { + vy = -maxVertVel + } + + u.X.SetVec(3, vx) + u.X.SetVec(4, vy) + u.X.SetVec(5, vz) +} + +// ensurePDSym adds minimal diagonal jitter to a SymDense to prevent non-positive pivots. +func ensurePDSym(A *mat.SymDense) { + n := A.SymmetricDim() + const jitter = 1e-8 + for i := 0; i < n; i++ { + if A.At(i, i) < jitter { + A.SetSym(i, i, jitter) + } + } +} + +// symmetrizePD enforces symmetry and positive diagonal after covariance update. +func symmetrizePD(A *mat.Dense) { + n, _ := A.Dims() + const minDiag = 1e-9 + for i := 0; i < n; i++ { + for j := i + 1; j < n; j++ { + avg := (A.At(i, j) + A.At(j, i)) * 0.5 + A.Set(i, j, avg) + A.Set(j, i, avg) + } + if A.At(i, i) < minDiag { + A.Set(i, i, minDiag) + } + } +} + +// angleWrap folds an angle into (−π, π]. +func angleWrap(a float64) float64 { + for a > math.Pi { + a -= 2 * math.Pi + } + for a < -math.Pi { + a += 2 * math.Pi + } + return a +}