用于调整出最优参数

App/nav/corridor_ctrl.c

@@ -62,29 +62,13 @@ void CorridorCtrl_Compute(const CorridorState_t *state,
         w_cmd = -(s_cfg.kd_theta * imu_wz);
     } else {
         /*
-         * 简化原则：
-         * - IMU 只管航向 (e_th + wz)
-         * - 左右激光只管居中
-         *
-         * 只要左右两侧都完整有效，就直接用左右平均距离差做横向误差：
-         *   e_y_direct = (d_right - d_left)/2
-         * 车在正中时 d_left ~= d_right，因此误差应接近 0。
-         *
-         * 若当前帧双侧不完整，再退回滤波器给出的 e_y。
+         * 横向偏差始终使用 EKF 滤波输出 state->e_y，
+         * EKF 内部已做分侧 inset 补偿、马氏距离异常检测和协方差加权，
+         * 控制器不应越级用原始观测差值覆盖。
          */
         float e_y_ctrl = state->e_y;
-        bool left_ok = ((obs->valid_mask & (1U << 0)) != 0U) &&
-                       ((obs->valid_mask & (1U << 1)) != 0U);
-        bool right_ok = ((obs->valid_mask & (1U << 2)) != 0U) &&
-                        ((obs->valid_mask & (1U << 3)) != 0U);
 
-        if (left_ok && right_ok) {
-            float d_left = (obs->d_lf + obs->d_lr) * 0.5f;
-            float d_right = (obs->d_rf + obs->d_rr) * 0.5f;
-            e_y_ctrl = 0.5f * (d_right - d_left);
-        }
-
-        /* 正常控制: IMU 管航向, 左右激光管居中 */
+        /* 正常控制: IMU 管航向, EKF 管居中 */
         w_cmd = -(s_cfg.kp_theta * state->e_th
                 + s_cfg.kd_theta * imu_wz
                 + s_cfg.kp_y    * e_y_ctrl);