first commit

utils/tests/visual_track_control/frame_transform_test.py

@@ -0,0 +1,43 @@
+"""frame_transform_test.py —— rotate_body_to_ned 单元测试。
+
+移植自 reference-c++/visual_track_control/tests/frame_transform_test.cpp。
+覆盖（物理追踪思路 §2.3，A4）：零航向恒等、朝东时机体前向 → 东向、
+垂直分量不变、模长守恒。
+"""
+
+import math
+
+import pytest
+
+from visual_track_control.frame_transform import rotate_body_to_ned
+
+
+def test_zero_yaw_is_identity():
+    north, east, down = rotate_body_to_ned(1.0, 0.5, -2.0, 0.0)
+    assert north == pytest.approx(1.0, abs=1e-12)   # 北 = 前
+    assert east == pytest.approx(0.5, abs=1e-12)    # 东 = 右
+    assert down == pytest.approx(-2.0, abs=1e-12)
+
+
+def test_yaw_east_90_maps_forward_to_east():
+    north, east, _ = rotate_body_to_ned(2.0, 0.0, 0.0, math.pi / 2.0)  # 机头朝东
+    assert north == pytest.approx(0.0, abs=1e-9)  # 北向 ≈ 0
+    assert east == pytest.approx(2.0, abs=1e-9)   # 东向 = 前向速度
+
+
+def test_right_is_east_when_heading_north():
+    north, east, _ = rotate_body_to_ned(0.0, 1.0, 0.0, 0.0)
+    assert north == pytest.approx(0.0, abs=1e-12)
+    assert east == pytest.approx(1.0, abs=1e-12)
+
+
+def test_down_component_unchanged():
+    _, _, down = rotate_body_to_ned(1.0, 1.0, 3.5, math.pi / 3.0)
+    assert down == 3.5
+
+
+def test_preserves_horizontal_norm():
+    # 旋转为正交变换：水平合速度不变（限幅约束不被破坏的前提）
+    north, east, _ = rotate_body_to_ned(3.0, -4.0, 1.0, 0.7)
+    assert math.hypot(north, east) == pytest.approx(math.hypot(3.0, -4.0),
+                                                    abs=1e-9)