feat: UWB anchor mount bracket (Issue #564)

jetson/ros2_ws/src/saltybot_uwb_position/test/test_uwb_position.py

@@ -0,0 +1,89 @@
+"""
+Unit tests for saltybot_uwb_position.uwb_position_node (Issue #546).
+No ROS2 or hardware required — tests the covariance math only.
+"""
+
+import math
+import sys
+import os
+
+# Make the package importable without a ROS2 install
+sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
+
+
+# ── Covariance helper (extracted from node for unit testing) ──────────────────
+
+def polar_to_cartesian_cov(bearing_rad, range_m, sigma_r, sigma_theta):
+    """Compute 2×2 Cartesian covariance from polar uncertainty."""
+    cos_b = math.cos(bearing_rad)
+    sin_b = math.sin(bearing_rad)
+    j00 =  cos_b;   j01 = -range_m * sin_b
+    j10 =  sin_b;   j11 =  range_m * cos_b
+    sr2 = sigma_r * sigma_r
+    st2 = sigma_theta * sigma_theta
+    cov_xx = j00 * j00 * sr2 + j01 * j01 * st2
+    cov_xy = j00 * j10 * sr2 + j01 * j11 * st2
+    cov_yy = j10 * j10 * sr2 + j11 * j11 * st2
+    return cov_xx, cov_xy, cov_yy
+
+
+# ── Tests ─────────────────────────────────────────────────────────────────────
+
+class TestPolarToCartesianCovariance:
+
+    def test_forward_bearing_zero(self):
+        """At bearing=0 (directly ahead) covariance aligns with axes."""
+        cov_xx, cov_xy, cov_yy = polar_to_cartesian_cov(
+            bearing_rad=0.0, range_m=5.0, sigma_r=0.10, sigma_theta=0.087
+        )
+        assert cov_xx > 0
+        assert cov_yy > 0
+        # At bearing=0: cov_xx = σ_r², cov_yy = (r·σ_θ)², cov_xy ≈ 0
+        assert abs(cov_xx - 0.10 ** 2) < 1e-9
+        assert abs(cov_xy) < 1e-9
+        expected_yy = (5.0 * 0.087) ** 2
+        assert abs(cov_yy - expected_yy) < 1e-6
+
+    def test_sideways_bearing(self):
+        """At bearing=90° covariance axes swap."""
+        sigma_r = 0.10
+        sigma_theta = 0.10
+        r = 3.0
+        cov_xx, cov_xy, cov_yy = polar_to_cartesian_cov(
+            bearing_rad=math.pi / 2, range_m=r,
+            sigma_r=sigma_r, sigma_theta=sigma_theta
+        )
+        # At bearing=90°: cov_xx = (r·σ_θ)², cov_yy = σ_r²
+        assert abs(cov_xx - (r * sigma_theta) ** 2) < 1e-9
+        assert abs(cov_yy - sigma_r ** 2) < 1e-9
+
+    def test_covariance_positive_definite(self):
+        """Matrix must be positive semi-definite (det ≥ 0, diag > 0)."""
+        for bearing in [0, 0.5, 1.0, 1.5, 2.0, 2.5, 3.0]:
+            for r in [1.0, 5.0, 10.0]:
+                cov_xx, cov_xy, cov_yy = polar_to_cartesian_cov(
+                    bearing, r, sigma_r=0.10, sigma_theta=0.087
+                )
+                assert cov_xx > 0
+                assert cov_yy > 0
+                det = cov_xx * cov_yy - cov_xy ** 2
+                assert det >= -1e-12, f"Non-PSD at bearing={bearing}, r={r}: det={det}"
+
+    def test_inflation_single_anchor(self):
+        """Covariance doubles (variance ×4) when only one anchor active."""
+        sigma_r = 0.10
+        sigma_theta = 0.087
+        bearing = 0.5
+        r = 4.0
+        cov_xx_full, _, _ = polar_to_cartesian_cov(bearing, r, sigma_r, sigma_theta)
+        cov_xx_half, _, _ = polar_to_cartesian_cov(
+            bearing, r,
+            sigma_r * math.sqrt(4.0),
+            sigma_theta * math.sqrt(4.0),
+        )
+        assert abs(cov_xx_half / cov_xx_full - 4.0) < 1e-9
+
+
+if __name__ == "__main__":
+    import pytest
+    pytest.main([__file__, "-v"])