feat(tracker): 3D biomechanical blob tracking with UKF
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 <noreply@anthropic.com>
This commit is contained in:
jedarden
parent
9c56a3795b
commit
59404aa18e
5 changed files with 866 additions and 5 deletions
|
|
@ -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
|
||||
)
|
||||
|
|
|
|||
|
|
@ -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=
|
||||
|
|
|
|||
242
mothership/internal/tracker/tracker.go
Normal file
242
mothership/internal/tracker/tracker.go
Normal file
|
|
@ -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)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
252
mothership/internal/tracker/tracker_test.go
Normal file
252
mothership/internal/tracker/tracker_test.go
Normal file
|
|
@ -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)
|
||||
}
|
||||
}
|
||||
337
mothership/internal/tracker/ukf.go
Normal file
337
mothership/internal/tracker/ukf.go
Normal file
|
|
@ -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
|
||||
}
|
||||
Loading…
Add table
Reference in a new issue