diff --git a/jetson/ros2_ws/src/saltybot_uwb_position/test/test_uwb_position.py b/jetson/ros2_ws/src/saltybot_uwb_position/test/test_uwb_position.py new file mode 100644 index 0000000..174ee39 --- /dev/null +++ b/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"])