增加GCP运动学并完善车体平面速度反馈分析

data_process/plot_new_controller_experiment.py

@@ -293,11 +293,22 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
     )
     if np.any(has_control_reference):
         reference_speed[~has_control_reference] = np.nan
+    motion_frame_yaw_radians = np.deg2rad(
+        numeric_column(frame, "ReferenceMotionFrameYawDegrees", 0.0)
+    )
+    motion_direction_cosine = np.cos(motion_frame_yaw_radians)
+    motion_direction_sine = np.sin(motion_frame_yaw_radians)
     state_body_vx = numeric_column(frame, "StateBodyVxMetersPerSecond")
+    state_body_vy = numeric_column(frame, "StateBodyVyMetersPerSecond")
     velocity_valid = (
         numeric_column(frame, "StateVelocityEstimateValid", 0.0) > 0.5
     )
     state_body_vx[~velocity_valid] = np.nan
+    state_body_vy[~velocity_valid] = np.nan
+    state_motion_speed = (
+        state_body_vx * motion_direction_cosine
+        + state_body_vy * motion_direction_sine
+    )
     has_velocity_diagnostics = (
         numeric_column(frame, "HasVelocityDiagnostics", 0.0) > 0.5
     )
@@ -317,14 +328,22 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
         )
     )
     detour_speed[~detour_speed_valid] = np.nan
-    wheel_raw_speed = numeric_column(
+    wheel_raw_body_vx = numeric_column(
         frame,
         "WheelFeedbackRawBodyVxMetersPerSecond",
     )
-    wheel_filtered_speed = numeric_column(
+    wheel_filtered_body_vx = numeric_column(
         frame,
         "WheelFeedbackFilteredBodyVxMetersPerSecond",
     )
+    wheel_raw_body_vy = numeric_column(
+        frame,
+        "WheelFeedbackRawBodyVyMetersPerSecond",
+    )
+    wheel_filtered_body_vy = numeric_column(
+        frame,
+        "WheelFeedbackFilteredBodyVyMetersPerSecond",
+    )
     wheel_speed_valid = (
         has_velocity_diagnostics
         & (
@@ -336,12 +355,33 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
             > 0.5
         )
     )
+    wheel_raw_speed = (
+        wheel_raw_body_vx * motion_direction_cosine
+        + wheel_raw_body_vy * motion_direction_sine
+    )
+    wheel_filtered_speed = (
+        wheel_filtered_body_vx * motion_direction_cosine
+        + wheel_filtered_body_vy * motion_direction_sine
+    )
+
+    # 兼容尚未记录轮速Vy的旧版β=0实验；非零β缺少Vy时不能伪造投影速度。
+    body_x_motion = np.abs(motion_direction_sine) <= 1e-12
+    missing_raw_projection = ~np.isfinite(wheel_raw_speed)
+    missing_filtered_projection = ~np.isfinite(wheel_filtered_speed)
+    wheel_raw_speed[body_x_motion & missing_raw_projection] = (
+        wheel_raw_body_vx[body_x_motion & missing_raw_projection]
+    )
+    wheel_filtered_speed[
+        body_x_motion & missing_filtered_projection
+    ] = wheel_filtered_body_vx[
+        body_x_motion & missing_filtered_projection
+    ]
     wheel_raw_speed[~wheel_speed_valid] = np.nan
     wheel_filtered_speed[~wheel_speed_valid] = np.nan
     actual_speed = np.where(
         np.isfinite(wheel_filtered_speed),
         wheel_filtered_speed,
-        state_body_vx,
+        state_motion_speed,
     )
     command_speed = numeric_column(frame, "CommandSpeed")
 
@@ -504,7 +544,7 @@ def plot_experiment(
     )
     axis.grid(True, alpha=0.3)
 
-    # 4. 参考、命令、Detour估计和轮速解算速度。
+    # 4. 参考、命令、Detour车头分量和沿β投影的轮速解算速度。
     speed_error = data["actual_speed"] - data["reference_speed"]
     speed_rmse = finite_rmse(speed_error)
     axis = axes[1, 1]
@@ -527,7 +567,7 @@ def plot_experiment(
             data["detour_speed"],
             ":",
             linewidth=1.2,
-            label="Detour估计Vx",
+            label="Detour估计Vx（车头分量）",
         )
     if np.any(np.isfinite(data["wheel_filtered_speed"])):
         wheel_filtered_valid = np.isfinite(
@@ -540,7 +580,7 @@ def plot_experiment(
             s=14,
             marker="o",
             zorder=5,
-            label="轮速解算滤波Vx（控制使用）",
+            label="轮速解算β方向速度（控制使用）",
         )
     else:
         axis.plot(
@@ -553,7 +593,7 @@ def plot_experiment(
     axis.set_ylabel("速度 / (m/s)")
     axis.set_title(
         "参考速度、控制命令与观测速度\n"
-        f"轮速Vx相对参考速度RMSE={speed_rmse:.4f}m/s"
+        f"轮速β方向速度相对参考速度RMSE={speed_rmse:.4f}m/s"
     )
     axis.grid(True, alpha=0.3)
     axis.legend(fontsize=8)