first commit

utils/visual_track_control/frame_transform.py

@@ -0,0 +1,25 @@
+"""机体系（前/右/下）→ 局部 NED（北/东/地）坐标旋转。
+
+对应 C++ 参考实现 reference-c++/visual_track_control/include/visual_track_control/frame_transform.h
+（规格：docs/物理追踪思路.md §2.3）。
+
+速度设定值只随航向角 ψ（绕下轴旋转）旋转：滚转/俯仰对速度指令为二阶
+小量，由飞控内环处理。旋转为正交变换：合速度大小不变，因此"限幅在机
+体系完成、旋转在输出端执行"不破坏 §8 限幅约束。
+
+正方向验证（NED：x=北，y=东，z=地；航向 ψ 自北顺时针为正）：
+  机体前向 (1,0) 在 ψ=90°（机头朝东）时 → (0, 1) = 东；
+  机体右向 (0,1) 在 ψ=0°（机头朝北）时 → (0, 1) = 东。
+"""
+
+import math
+
+
+def rotate_body_to_ned(v_forward: float, v_right: float, v_down: float,
+                       yaw_rad: float) -> tuple[float, float, float]:
+    """绕下轴按航向角 ψ 旋转，返回 (北, 东, 地)；垂直分量不变。"""
+    c = math.cos(yaw_rad)
+    s = math.sin(yaw_rad)
+    north = v_forward * c - v_right * s
+    east = v_forward * s + v_right * c
+    return north, east, v_down