1.0

App/est/corridor_filter.c

@@ -146,3 +146,22 @@ void CorridorFilter_RebaseAfterTurnaround(float imu_yaw_continuous_rad)
     s_imu_yaw_ref_rad = imu_yaw_continuous_rad;
     s_imu_yaw_ref_set = true;
 }
+
+void CorridorFilter_RebaseHeading(float imu_yaw_continuous_rad)
+{
+    if (!s_initialized) return;
+
+    CorridorEKF_ResetHeading();
+    s_imu_yaw_ref_rad = imu_yaw_continuous_rad;
+    s_imu_yaw_ref_set = true;
+}
+
+void CorridorFilter_CorrectIMUReference(float heading_correction_rad)
+{
+    if (!s_initialized || !s_imu_yaw_ref_set) return;
+
+    /* 修正IMU参考值，用于消除转向系统性偏差
+     * 例如：VL53检测到车头偏右2度，heading_correction_rad = -0.035 rad
+     * 修正后，EKF会认为当前IMU方向才是正确的走廊方向 */
+    s_imu_yaw_ref_rad += heading_correction_rad;
+}

App/est/corridor_filter.h

@@ -61,6 +61,24 @@ extern "C" {
    */
   void CorridorFilter_RebaseAfterTurnaround(float imu_yaw_continuous_rad);
 
+  /**
+   * @brief 仅重建航向参考，不改横向位置
+   *
+   * 用于进入新垄沟后，已经通过侧墙把车头摆正，
+   * 这时把当前 IMU yaw 设为新的走廊参考，同时仅清零 e_th。
+   */
+  void CorridorFilter_RebaseHeading(float imu_yaw_continuous_rad);
+
+  /**
+   * @brief 修正IMU航向参考值（用于消除转向系统性偏差）
+   *
+   * 用于赛道模式转向后，用VL53检测到的墙壁航向误差修正IMU参考值
+   * 只修正参考值，不改变当前状态估计
+   * 
+   * @param heading_correction_rad 航向修正量 (rad)，正值表示车头需要左转
+   */
+  void CorridorFilter_CorrectIMUReference(float heading_correction_rad);
+
 #ifdef __cplusplus
 }
 #endif