1.0

App/est/corridor_filter.c

@@ -106,3 +106,18 @@ void CorridorFilter_Update(const CorridorObs_t *obs, float imu_wz, float odom_vx
         }
     }
 }
+
+/* =========================================================
+ * 重置 (进入新垄沟时调用)
+ * ========================================================= */
+void CorridorFilter_Reset(void)
+{
+    if (!s_initialized) return;
+
+    /* 重置 EKF 内核: 状态归零, 协方差恢复初始值 */
+    CorridorEKF_Reset();
+
+    /* 解锁 IMU yaw 参考值, 等待在新沟中重新锁定 */
+    s_imu_yaw_ref_rad = 0.0f;
+    s_imu_yaw_ref_set = false;
+}

App/est/corridor_filter.h

@@ -39,6 +39,16 @@ extern "C" {
                              float dt_s, float imu_yaw_continuous_rad, bool imu_yaw_valid,
                              CorridorState_t *out_state);
 
+  /**
+   * @brief 重置滤波器状态 (进入新垄沟时必须调用)
+   *
+   * 重置内容：
+   *   - EKF 状态向量清零 (e_y=0, e_th=0, s=0)
+   *   - 协方差恢复到初始值
+   *   - IMU yaw 参考值解锁，等待重新锁定
+   */
+  void CorridorFilter_Reset(void);
+
 #ifdef __cplusplus
 }
 #endif