first commit

utils/tests/visual_track_control/pid_controller_test.py

@@ -0,0 +1,103 @@
+"""pid_controller_test.py —— PidController 单元测试。
+
+移植自 reference-c++/visual_track_control/tests/pid_controller_test.cpp。
+覆盖（物理追踪思路 §6.2/§6.4）：位置式 P/I/D 三项、积分钳位、输出限幅、
+异常节拍保护、anti-windup 条件积分（C1 修复回归）、ResetIntegral / Reset。
+"""
+
+import pytest
+
+from visual_track_control.pid_controller import PidController, PidParams
+
+
+def make_params(kp, ki, kd, integral_limit, output_limit, filter_coef=0.0):
+    return PidParams(kp=kp, ki=ki, kd=kd, integral_limit=integral_limit,
+                     derivative_filter_coef=filter_coef, output_limit=output_limit)
+
+
+def test_proportional_only():
+    pid = PidController(make_params(2.0, 0.0, 0.0, 0.0, 0.0))
+    assert pid.update(3.0, 0.1) == 6.0
+    assert pid.update(-1.5, 0.1) == -3.0
+
+
+def test_integral_accumulates():
+    pid = PidController(make_params(0.0, 1.0, 0.0, 0.0, 0.0))
+    # 两拍 error=2、dt=0.5：积分累计 1.0 → 2.0
+    assert pid.update(2.0, 0.5) == 1.0
+    assert pid.update(2.0, 0.5) == 2.0
+
+
+def test_integral_clamped_to_limit():
+    pid = PidController(make_params(0.0, 1.0, 0.0, integral_limit=1.0, output_limit=0.0))
+    for _ in range(5):
+        pid.update(10.0, 1.0)
+    # 积分候选值被钳位在 1.0 → 输出恒为 1.0
+    assert pid.last_output == 1.0
+
+
+def test_output_clamped_to_limit():
+    pid = PidController(make_params(10.0, 0.0, 0.0, 0.0, output_limit=3.0))
+    assert pid.update(1.0, 0.1) == 3.0
+    assert pid.update(-1.0, 0.1) == -3.0
+
+
+def test_anti_windup_freeze_while_saturated_same_direction():
+    # kp=0、ki=2、output_limit=5：error=+1 持续饱和时积分冻结，
+    # 输出恒钉在 +5（10 拍累计也不会让积分继续增长）。
+    pid = PidController(make_params(0.0, 2.0, 0.0, 0.0, 5.0))
+    for _ in range(10):
+        assert pid.update(10.0, 1.0) == 5.0
+
+
+def test_anti_windup_unwinds_after_error_flips():
+    # C1 回归：误差反号后积分必须立即回卷（旧判据"积分与输出同向"会
+    # 冻结回卷、输出一直钉在 +5）。
+    # kp=0、ki=2、limit=5：+10 饱和 3 拍、积分冻结在 0；
+    # error=-1 首拍 → 输出 2×(-1) = -2（立即回卷）；
+    # 持续负误差使输出到达负饱和 -5（积分冻结在 -2，输出 = clamp(-6)）。
+    pid = PidController(make_params(0.0, 2.0, 0.0, 0.0, 5.0))
+    for _ in range(3):
+        assert pid.update(10.0, 1.0) == 5.0
+    assert pid.update(-1.0, 1.0) == -2.0  # 首拍立即回卷
+    out = -2.0
+    for _ in range(11):
+        out = pid.update(-1.0, 1.0)
+    assert out == pytest.approx(-5.0, abs=1e-9)  # 旧判据此处仍为 +5.0
+
+
+def test_derivative_first_step_is_zero():
+    pid = PidController(make_params(0.0, 0.0, 1.0, 0.0, 0.0))
+    # 首拍无微分历史：微分项为 0
+    assert pid.update(5.0, 1.0) == 0.0
+
+
+def test_derivative_with_low_pass_filter():
+    pid = PidController(make_params(0.0, 0.0, 1.0, 0.0, 0.0, filter_coef=0.5))
+    pid.update(0.0, 1.0)  # 建立微分历史（导数 0）
+    # 第二拍原始导数 1.0，低通后 0.5×0 + 0.5×1.0 = 0.5
+    assert pid.update(1.0, 1.0) == 0.5
+
+
+def test_zero_or_negative_dt_holds_last_output():
+    pid = PidController(make_params(1.0, 0.0, 0.0, 0.0, 0.0))
+    assert pid.update(2.0, 0.1) == 2.0
+    assert pid.update(99.0, 0.0) == 2.0
+    assert pid.update(99.0, -1.0) == 2.0
+
+
+def test_reset_integral_clears_only_integral():
+    pid = PidController(make_params(0.0, 1.0, 1.0, 0.0, 0.0))
+    pid.update(2.0, 1.0)  # 积分 = 2
+    pid.reset_integral()
+    # 积分清零、微分历史保留：输出应为 kd×(0−2)/1 = −2
+    assert pid.update(0.0, 1.0) == -2.0
+
+
+def test_reset_clears_everything():
+    pid = PidController(make_params(1.0, 1.0, 1.0, 0.0, 0.0))
+    pid.update(5.0, 1.0)
+    pid.reset()
+    assert pid.last_output == 0.0
+    # 全清后等同全新控制器：首拍无微分历史
+    assert pid.update(3.0, 1.0) == 3.0 + 3.0  # kp·e + ki·∫e