UWB

UWB multilateration and EKF position filter.

class omnitiles.uwb.UwbEkf(anchors=((0.0, 0.0), (1.778, 0.0), (-0.508, 1.7018), (3.0861, 1.7018)), range_std_m=0.07)[source]

Bases: object

Extended Kalman Filter fusing UWB ranges with IMU motion detection.

State vector: [px, py, vx, vy] — 2D position and velocity in meters.

  • Predict uses a constant-velocity model. Process noise Q is modulated by the IMU: when the accelerometer/gyroscope indicate the tile is stationary, Q is very small so the filter aggressively smooths UWB noise. When motion is detected, Q grows so the filter tracks the new position.

  • Update ingests one or more UWB range observations. Each range is individually gated (Mahalanobis chi² test) to reject multipath outliers before the state is updated.

Parameters:

  • anchors (Sequence[tuple[float, float]])

  • range_std_m (float | Sequence[float])

GRAVITY = 9.81
ACCEL_MOTION_THRESH = 1.0
GYRO_YAW_MOTION_THRESH = 0.1
SIGMA_A_STATIONARY = 0.01
SIGMA_A_MOVING = 5.0
GATE_CHI2 = 9.0
property initialized: bool
property position: tuple[float, float] | None
property velocity: tuple[float, float] | None
reset()[source]
Return type:

None

step(distances_mm, timestamp, imu=None, z_offset_m=0.0)[source]

Run one predict + update cycle.

Returns the filtered (x, y) in meters, or None if the filter has not yet been initialised (needs at least one trilateration fix).

Parameters:

  • distances_mm (Sequence[int | None])

  • timestamp (float)

  • imu (ImuSample | None)

  • z_offset_m (float)

Return type:

tuple[float, float] | None

omnitiles.uwb.trilaterate(distances_mm, anchors=((0.0, 0.0), (1.778, 0.0), (-0.508, 1.7018), (3.0861, 1.7018)), z_offset_m=0.0)[source]

Compute a 2D position from anchor distances using least-squares.

Works with 3 or more anchors. When more than 3 valid distances are available the overdetermined system is solved via least-squares, averaging out per-anchor ranging noise.

Parameters:

  • distances_mm (Sequence[int | None]) – Distances from each anchor, in millimeters. None entries are skipped; at least 3 valid distances are required.

  • anchors (Sequence[tuple[float, float]]) – Anchor (x, y) positions in meters. Must have the same length as distances_mm.

  • z_offset_m (float) – Vertical distance between the tag and the (shared) anchor plane, in meters. Measured 3D ranges are projected onto the floor plane via r = sqrt(d**2 - z**2) before solving.

Return type:

tuple[float, float] | None

Returns:

(x, y) in meters, or None if fewer than 3 valid ranges or any valid range is shorter than z_offset_m.