c0ae81a6b0
Some checks are pending
CI Benchmark - Fusion Loop Timing / Fusion Loop Timing Benchmark (push) Waiting to run
Ran gofmt across the entire mothership codebase to ensure consistent code formatting per Go standards. All tests pass after formatting.
301 lines
7.9 KiB
Go
301 lines
7.9 KiB
Go
// Package simulator provides shared physics functions for CSI simulation.
|
||
// This package is used by both the pre-deployment simulator and the CSI CLI simulator.
|
||
package simulator
|
||
|
||
import (
|
||
"math"
|
||
"math/rand"
|
||
)
|
||
|
||
// PhysicsModel provides physics calculations for CSI simulation
|
||
type PhysicsModel struct {
|
||
space *Space
|
||
noiseSigma float64 // Gaussian noise standard deviation for I/Q
|
||
walls []WallDefinition
|
||
}
|
||
|
||
// WallDefinition defines a wall segment for attenuation calculations
|
||
type WallDefinition struct {
|
||
X1, Y1, X2, Y2 float64 // Wall endpoints (floor coordinates)
|
||
Attenuation float64 // dB attenuation
|
||
}
|
||
|
||
// NewPhysicsModel creates a new physics model for the given space
|
||
func NewPhysicsModel(space *Space) *PhysicsModel {
|
||
return &PhysicsModel{
|
||
space: space,
|
||
noiseSigma: 0.005, // Default noise level
|
||
walls: make([]WallDefinition, 0),
|
||
}
|
||
}
|
||
|
||
// SetNoiseSigma sets the Gaussian noise standard deviation
|
||
func (pm *PhysicsModel) SetNoiseSigma(sigma float64) {
|
||
pm.noiseSigma = sigma
|
||
}
|
||
|
||
// AddWall adds a wall definition to the physics model
|
||
func (pm *PhysicsModel) AddWall(x1, y1, x2, y2, attenuation float64) {
|
||
pm.walls = append(pm.walls, WallDefinition{
|
||
X1: x1,
|
||
Y1: y1,
|
||
X2: x2,
|
||
Y2: y2,
|
||
Attenuation: attenuation,
|
||
})
|
||
}
|
||
|
||
// PathLossdB computes path loss in dB using log-distance model
|
||
// PL(d) = PL_0 + 10*n*log10(d/d_0)
|
||
// where PL_0 = 40 dB at d_0 = 1m, n = 2.0 (free space)
|
||
func (pm *PhysicsModel) PathLossdB(distance float64) float64 {
|
||
const PL0 = 40.0 // dB at 1m reference
|
||
const d0 = 1.0 // reference distance in meters
|
||
const n = 2.0 // path loss exponent (free space)
|
||
|
||
if distance < 0.01 {
|
||
distance = 0.01 // Avoid log(0)
|
||
}
|
||
|
||
return PL0 + 10*n*math.Log10(distance/d0)
|
||
}
|
||
|
||
// WallAttenuation computes total wall attenuation for a path
|
||
func (pm *PhysicsModel) WallAttenuation(from, to Point) float64 {
|
||
totalLoss := 0.0
|
||
|
||
for _, wall := range pm.walls {
|
||
if pm.pathIntersectsWall(from.X, from.Y, to.X, to.Y,
|
||
wall.X1, wall.Y1, wall.X2, wall.Y2) {
|
||
totalLoss += wall.Attenuation
|
||
}
|
||
}
|
||
|
||
return totalLoss
|
||
}
|
||
|
||
// pathIntersectsWall checks if a path intersects a wall segment (2D)
|
||
func (pm *PhysicsModel) pathIntersectsWall(x1, y1, x2, y2, wx1, wy1, wx2, wy2 float64) bool {
|
||
// Compute orientations
|
||
ccw := func(ax, ay, bx, by, cx, cy float64) float64 {
|
||
return (bx-ax)*(cy-ay) - (by-ay)*(cx-ax)
|
||
}
|
||
|
||
o1 := ccw(x1, y1, x2, y2, wx1, wy1)
|
||
o2 := ccw(x1, y1, x2, y2, wx2, wy2)
|
||
o3 := ccw(wx1, wy1, wx2, wy2, x1, y1)
|
||
o4 := ccw(wx1, wy1, wx2, wy2, x2, y2)
|
||
|
||
// Check for intersection
|
||
return o1*o2 < 0 && o3*o4 < 0
|
||
}
|
||
|
||
// ComputeRSSI computes the RSSI in dBm for a given distance
|
||
// Returns RSSI in range [-90, -30] dBm
|
||
func (pm *PhysicsModel) ComputeRSSI(distance float64) int8 {
|
||
pathLoss := pm.PathLossdB(distance)
|
||
txPower := -30.0 // Reference transmit power in dBm
|
||
|
||
rssi := txPower - pathLoss
|
||
|
||
// Clamp to realistic range
|
||
if rssi < -90 {
|
||
rssi = -90
|
||
}
|
||
if rssi > -30 {
|
||
rssi = -30
|
||
}
|
||
|
||
return int8(rssi)
|
||
}
|
||
|
||
// DeltaRMS computes the expected deltaRMS motion score
|
||
// when a walker is at the given position (vs empty room)
|
||
func (pm *PhysicsModel) DeltaRMS(tx, rx, walker Point) float64 {
|
||
// Calculate Fresnel zone number
|
||
zone := FresnelZoneNumber(tx, rx, walker)
|
||
|
||
// DeltaRMS is highest in zone 1, decreases with zone number
|
||
// Zone 1: 0.15, Zone 2: 0.08, Zone 3: 0.04, Zone 4: 0.02, Zone 5+: 0.01
|
||
switch zone {
|
||
case 1:
|
||
return 0.15
|
||
case 2:
|
||
return 0.08
|
||
case 3:
|
||
return 0.04
|
||
case 4:
|
||
return 0.02
|
||
default:
|
||
return 0.01
|
||
}
|
||
}
|
||
|
||
// GenerateIQPair generates a synthetic I/Q pair for a subcarrier
|
||
// with amplitude and phase, plus Gaussian noise
|
||
func (pm *PhysicsModel) GenerateIQPair(amplitude, phase float64) (int8, int8) {
|
||
// Generate Gaussian noise using Box-Muller transform
|
||
u1 := rand.Float64()
|
||
u2 := rand.Float64()
|
||
z0 := math.Sqrt(-2.0*math.Log(u1)) * math.Cos(2.0*math.Pi*u2)
|
||
z1 := math.Sqrt(-2.0*math.Log(u1)) * math.Sin(2.0*math.Pi*u2)
|
||
|
||
noiseI := z0 * pm.noiseSigma
|
||
noiseQ := z1 * pm.noiseSigma
|
||
|
||
// Convert to I/Q
|
||
i := amplitude*math.Cos(phase) + noiseI
|
||
q := amplitude*math.Sin(phase) + noiseQ
|
||
|
||
// Clamp to int8 range [-127, 127]
|
||
// Note: We avoid -128 to prevent overflow issues
|
||
if i > 127 {
|
||
i = 127
|
||
}
|
||
if i < -127 {
|
||
i = -127
|
||
}
|
||
if q > 127 {
|
||
q = 127
|
||
}
|
||
if q < -127 {
|
||
q = -127
|
||
}
|
||
|
||
return int8(i), int8(q)
|
||
}
|
||
|
||
// GenerateSubcarrierCSI generates CSI data for all subcarriers
|
||
func (pm *PhysicsModel) GenerateSubcarrierCSI(tx, rx, walker Point, nSub int, frameNum int) []struct{ I, Q int8 } {
|
||
result := make([]struct{ I, Q int8 }, nSub)
|
||
|
||
// Base amplitude from deltaRMS
|
||
deltaRMS := pm.DeltaRMS(tx, rx, walker)
|
||
amplitude := deltaRMS * 500.0 // Scale to reasonable I/Q range
|
||
|
||
for k := 0; k < nSub; k++ {
|
||
// Compute phase at this subcarrier
|
||
phase := pm.PhaseAtSubcarrier(tx, rx, walker, k, frameNum)
|
||
|
||
// Add subcarrier-dependent amplitude variation
|
||
// Simulates frequency-selective fading
|
||
freqFading := 0.8 + 0.4*math.Sin(2*math.Pi*float64(k)/16.0)
|
||
subAmplitude := amplitude * freqFading
|
||
|
||
result[k].I, result[k].Q = pm.GenerateIQPair(subAmplitude, phase)
|
||
}
|
||
|
||
return result
|
||
}
|
||
|
||
// PhaseAtSubcarrier computes phase for a given subcarrier index
|
||
func (pm *PhysicsModel) PhaseAtSubcarrier(tx, rx, walker Point, subcarrierIndex, frameNum int) float64 {
|
||
// Total path length (TX -> walker -> RX)
|
||
d1 := tx.Distance(walker)
|
||
d2 := walker.Distance(rx)
|
||
totalDist := d1 + d2
|
||
|
||
// Phase = 2π × k × Δf × (d / c) + temporal_variation
|
||
phase := 2 * math.Pi * float64(subcarrierIndex) * SubcarrierSpacing * (totalDist / C)
|
||
|
||
// Add small temporal variation for realism
|
||
temporalPhase := 0.1 * math.Sin(2*math.Pi*float64(frameNum)/100.0)
|
||
phase += temporalPhase
|
||
|
||
// Normalize to [-π, π]
|
||
for phase > math.Pi {
|
||
phase -= 2 * math.Pi
|
||
}
|
||
for phase < -math.Pi {
|
||
phase += 2 * math.Pi
|
||
}
|
||
|
||
return phase
|
||
}
|
||
|
||
// ValidateRSSI validates that RSSI is within expected range for distance
|
||
func ValidateRSSI(rssi int8, distance float64) bool {
|
||
// Expected RSSI range for given distance
|
||
expectedPathLoss := 40.0 + 20.0*math.Log10(distance/1.0)
|
||
expectedRSSI := -30.0 - expectedPathLoss
|
||
|
||
// Allow ±20 dB tolerance
|
||
minRSSI := expectedRSSI - 20.0
|
||
maxRSSI := expectedRSSI + 20.0
|
||
|
||
// Clamp to realistic bounds
|
||
if minRSSI < -90 {
|
||
minRSSI = -90
|
||
}
|
||
if maxRSSI > -30 {
|
||
maxRSSI = -30
|
||
}
|
||
|
||
return float64(rssi) >= minRSSI && float64(rssi) <= maxRSSI
|
||
}
|
||
|
||
// ValidateIQValues checks that I/Q values are in valid int8 range
|
||
// Note: int8 values are always in range, but we check for sensible CSI values
|
||
func ValidateIQValues(i, q int8) bool {
|
||
return i >= -127 && q >= -127 // Upper bound implicit in int8 type
|
||
}
|
||
|
||
// IsInFresnelZones checks if a point is within the first N Fresnel zones
|
||
func IsInFresnelZones(tx, rx, point Point, maxZone int) bool {
|
||
zone := FresnelZoneNumber(tx, rx, point)
|
||
return zone <= maxZone && zone > 0
|
||
}
|
||
|
||
// ComputeFresnelModulation computes the Fresnel zone modulation factor
|
||
// Returns a value between 0 and 1, where 1 is maximum modulation (zone 1)
|
||
func ComputeFresnelModulation(tx, rx, point Point) float64 {
|
||
zone := FresnelZoneNumber(tx, rx, point)
|
||
|
||
// Zone 1: maximum modulation, Zone 5+: minimum
|
||
if zone <= 1 {
|
||
return 1.0
|
||
}
|
||
if zone >= 5 {
|
||
return 0.0
|
||
}
|
||
|
||
return 1.0 / math.Pow(float64(zone), 2.0)
|
||
}
|
||
|
||
// ComputeLinkQuality estimates link quality (0-1) based on geometry
|
||
// Higher quality when links have good angular diversity
|
||
func ComputeLinkQuality(nodes []Point) float64 {
|
||
if len(nodes) < 2 {
|
||
return 0.0
|
||
}
|
||
|
||
// Simple metric: spread of node positions
|
||
// Compute centroid
|
||
var cx, cy, cz float64
|
||
for _, n := range nodes {
|
||
cx += n.X
|
||
cy += n.Y
|
||
cz += n.Z
|
||
}
|
||
cx /= float64(len(nodes))
|
||
cy /= float64(len(nodes))
|
||
cz /= float64(len(nodes))
|
||
|
||
// Compute average distance from centroid
|
||
avgDist := 0.0
|
||
for _, n := range nodes {
|
||
dx := n.X - cx
|
||
dy := n.Y - cy
|
||
dz := n.Z - cz
|
||
avgDist += math.Sqrt(dx*dx + dy*dy + dz*dz)
|
||
}
|
||
avgDist /= float64(len(nodes))
|
||
|
||
// Normalize: 5m spread = excellent quality (1.0)
|
||
quality := avgDist / 5.0
|
||
if quality > 1.0 {
|
||
quality = 1.0
|
||
}
|
||
|
||
return quality
|
||
}
|