用于调整出最优参数

.idea/claudeCodeTabState.xml

@@ -7,9 +7,9 @@
           <value>
             <TabSessionState>
               <option name="provider" value="claude" />
-              <option name="sessionId" value="dd69d44e-8774-4d83-adda-209d86f8bb78" />
+              <option name="sessionId" value="36e0672c-c35d-4e77-8799-d9ea097d6209" />
               <option name="cwd" value="$PROJECT_DIR$" />
-              <option name="model" value="claude-opus-4-6" />
+              <option name="model" value="claude-opus-4-6[1m]" />
               <option name="permissionMode" value="plan" />
               <option name="reasoningEffort" value="medium" />
             </TabSessionState>

.idea/editor.xml

@@ -249,101 +249,5 @@
     <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=IfStdIsConstantEvaluatedCanBeReplaced/@EntryIndexedValue" value="SUGGESTION" type="string" />
     <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=StdIsConstantEvaluatedWillAlwaysEvaluateToConstant/@EntryIndexedValue" value="WARNING" type="string" />
     <option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=StringLiteralTypo/@EntryIndexedValue" value="DO_NOT_SHOW" type="string" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppClangFormat/EnableClangFormatSupport/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/ALIGN_MULTILINE_ARGUMENT/@EntryValue" value="true" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/ALIGN_MULTILINE_BINARY_EXPRESSIONS_CHAIN/@EntryValue" value="true" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/ALIGN_MULTILINE_CALLS_CHAIN/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/ALIGN_MULTILINE_EXPRESSION/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/ALIGN_MULTILINE_EXTENDS_LIST/@EntryValue" value="true" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/ALIGN_MULTILINE_FOR_STMT/@EntryValue" value="true" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/ALIGN_MULTILINE_PARAMETER/@EntryValue" value="true" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/ALIGN_MULTILINE_TYPE_ARGUMENT/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/ALIGN_MULTILINE_TYPE_PARAMETER/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/ALIGN_MULTIPLE_DECLARATION/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/ALIGN_TERNARY/@EntryValue" value="ALIGN_ALL" type="string" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/ANONYMOUS_METHOD_DECLARATION_BRACES/@EntryValue" value="END_OF_LINE" type="string" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/BLANK_LINES_AROUND_CLASS_DEFINITION/@EntryValue" value="1" type="int" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/BLANK_LINES_AROUND_DECLARATIONS/@EntryValue" value="0" type="int" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/BLANK_LINES_AROUND_FUNCTION_DECLARATION/@EntryValue" value="1" type="int" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/BLANK_LINES_AROUND_FUNCTION_DEFINITION/@EntryValue" value="1" type="int" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/BREAK_TEMPLATE_DECLARATION/@EntryValue" value="LINE_BREAK" type="string" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/CASE_BLOCK_BRACES/@EntryValue" value="END_OF_LINE" type="string" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/CONTINUOUS_LINE_INDENT/@EntryValue" value="Double" type="string" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/INDENT_ACCESS_SPECIFIERS_FROM_CLASS/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/INDENT_CASE_FROM_SWITCH/@EntryValue" value="true" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/INDENT_CLASS_MEMBERS_FROM_ACCESS_SPECIFIERS/@EntryValue" value="true" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/INDENT_COMMENT/@EntryValue" value="true" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/INDENT_SIZE/@EntryValue" value="4" type="int" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/INDENT_STYLE/@EntryValue" value="Space" type="string" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/INITIALIZER_BRACES/@EntryValue" value="END_OF_LINE_NO_SPACE" type="string" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/INT_ALIGN_EQ/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/INVOCABLE_DECLARATION_BRACES/@EntryValue" value="END_OF_LINE" type="string" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/KEEP_BLANK_LINES_IN_CODE/@EntryValue" value="2" type="int" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/KEEP_BLANK_LINES_IN_DECLARATIONS/@EntryValue" value="2" type="int" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/KEEP_USER_LINEBREAKS/@EntryValue" value="true" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/LINE_BREAK_AFTER_COLON_IN_MEMBER_INITIALIZER_LISTS/@EntryValue" value="ON_SINGLE_LINE" type="string" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/MEMBER_INITIALIZER_LIST_STYLE/@EntryValue" value="DO_NOT_CHANGE" type="string" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/NAMESPACE_DECLARATION_BRACES/@EntryValue" value="END_OF_LINE" type="string" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/NAMESPACE_INDENTATION/@EntryValue" value="All" type="string" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/OTHER_BRACES/@EntryValue" value="END_OF_LINE" type="string" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/PLACE_CATCH_ON_NEW_LINE/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/PLACE_ELSE_ON_NEW_LINE/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/PLACE_NAMESPACE_DEFINITIONS_ON_SAME_LINE/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/PLACE_WHILE_ON_NEW_LINE/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SIMPLE_BLOCK_STYLE/@EntryValue" value="DO_NOT_CHANGE" type="string" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_AFTER_CAST_EXPRESSION_PARENTHESES/@EntryValue" value="true" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_AFTER_COLON_IN_BITFIELD_DECLARATOR/@EntryValue" value="true" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_AFTER_COMMA_IN_TEMPLATE_ARGS/@EntryValue" value="true" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_AFTER_COMMA_IN_TEMPLATE_PARAMS/@EntryValue" value="true" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_AFTER_EXTENDS_COLON/@EntryValue" value="true" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_AFTER_FOR_COLON/@EntryValue" value="true" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_AFTER_FOR_SEMICOLON/@EntryValue" value="true" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_AFTER_PTR_IN_DATA_MEMBER/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_AFTER_PTR_IN_DATA_MEMBERS/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_AFTER_PTR_IN_METHOD/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_AFTER_PTR_IN_NESTED_DECLARATOR/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_AFTER_REF_IN_DATA_MEMBER/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_AFTER_REF_IN_DATA_MEMBERS/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_AFTER_REF_IN_METHOD/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_AFTER_UNARY_OPERATOR/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_BEFORE_COLON_IN_BITFIELD_DECLARATOR/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_BEFORE_EXTENDS_COLON/@EntryValue" value="true" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_BEFORE_FOR_COLON/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_BEFORE_FOR_SEMICOLON/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_BEFORE_PTR_IN_ABSTRACT_DECL/@EntryValue" value="true" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_BEFORE_PTR_IN_DATA_MEMBER/@EntryValue" value="true" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_BEFORE_PTR_IN_DATA_MEMBERS/@EntryValue" value="true" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_BEFORE_PTR_IN_METHOD/@EntryValue" value="true" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_BEFORE_REF_IN_ABSTRACT_DECL/@EntryValue" value="true" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_BEFORE_REF_IN_DATA_MEMBER/@EntryValue" value="true" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_BEFORE_REF_IN_DATA_MEMBERS/@EntryValue" value="true" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_BEFORE_REF_IN_METHOD/@EntryValue" value="true" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_BEFORE_TEMPLATE_ARGS/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_BEFORE_TEMPLATE_PARAMS/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_BETWEEN_CLOSING_ANGLE_BRACKETS_IN_TEMPLATE_ARGS/@EntryValue" value="true" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_WITHIN_ARRAY_ACCESS_BRACKETS/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_WITHIN_CAST_EXPRESSION_PARENTHESES/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_WITHIN_DECLARATION_PARENTHESES/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_WITHIN_EMPTY_BLOCKS/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_WITHIN_EMPTY_INITIALIZER_BRACES/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_WITHIN_EMPTY_METHOD_PARENTHESES/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_WITHIN_EMPTY_TEMPLATE_PARAMS/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_WITHIN_INITIALIZER_BRACES/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_WITHIN_TEMPLATE_ARGS/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPACE_WITHIN_TEMPLATE_PARAMS/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/SPECIAL_ELSE_IF_TREATMENT/@EntryValue" value="true" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/TAB_WIDTH/@EntryValue" value="4" type="int" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/TYPE_DECLARATION_BRACES/@EntryValue" value="END_OF_LINE" type="string" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/WRAP_AFTER_BINARY_OPSIGN/@EntryValue" value="true" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/WRAP_AFTER_DECLARATION_LPAR/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/WRAP_AFTER_INVOCATION_LPAR/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/WRAP_ARGUMENTS_STYLE/@EntryValue" value="WRAP_IF_LONG" type="string" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/WRAP_BEFORE_DECLARATION_LPAR/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/WRAP_BEFORE_DECLARATION_RPAR/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/WRAP_BEFORE_INVOCATION_LPAR/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/WRAP_BEFORE_INVOCATION_RPAR/@EntryValue" value="false" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/WRAP_BEFORE_TERNARY_OPSIGNS/@EntryValue" value="true" type="bool" />
-    <option name="/Default/CodeStyle/CodeFormatting/CppFormatting/WRAP_PARAMETERS_STYLE/@EntryValue" value="WRAP_IF_LONG" type="string" />
-    <option name="/Default/CodeStyle/EditorConfig/EnableClangFormatSupport/@EntryValue" value="false" type="bool" />
   </component>
 </project>

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 {
         /*
-         * 简化原则：
+         * 横向偏差始终使用 EKF 滤波输出 state->e_y，
-         * - IMU 只管航向 (e_th + wz)
+         * EKF 内部已做分侧 inset 补偿、马氏距离异常检测和协方差加权，
-         * - 左右激光只管居中
+         * 控制器不应越级用原始观测差值覆盖。
-         *
-         * 只要左右两侧都完整有效，就直接用左右平均距离差做横向误差：
-         *   e_y_direct = (d_right - d_left)/2
-         * 车在正中时 d_left ~= d_right，因此误差应接近 0。
-         *
-         * 若当前帧双侧不完整，再退回滤波器给出的 e_y。
          */
         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) {
+        /* 正常控制: IMU 管航向, EKF 管居中 */
-            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 管航向, 左右激光管居中 */
         w_cmd = -(s_cfg.kp_theta * state->e_th
                 + s_cfg.kd_theta * imu_wz
                 + s_cfg.kp_y    * e_y_ctrl);

横向偏移问题代码分析.md

@@ -0,0 +1,142 @@
+# 垄沟横向偏移问题 — 代码层面分析
+
+> 基于 `问题诊断报告.md` 的二次审查，对根因优先级提出不同判断。
+
+---
+
+## 现象回顾
+
+- 车头笔直（IMU 航向控制正常）
+- 车身稳定偏向一侧墙壁，无法居中
+
+---
+
+## 代码审查发现
+
+### 发现 1：EKF 的 `side_sensor_inset` fallback 从未实现（主因）
+
+**文件：** `App/est/corridor_ekf.c:451-454`
+
+```c
+float d_center_left  = (W - Rw) / 2.0f + s_cfg.left_sensor_inset;   // = 0.10 + 0.0 = 0.10
+float d_center_right = (W - Rw) / 2.0f + s_cfg.right_sensor_inset;  // = 0.10 + 0.0 = 0.10
+```
+
+**文件：** `App/robot_params.h:96-113`
+
+```c
+#define PARAM_VL53_SIDE_INSET       (-(0.018f))   // 已实测！但从未被 EKF 使用
+#define PARAM_VL53_LEFT_INSET       0.0f           // 未标定
+#define PARAM_VL53_RIGHT_INSET      0.0f           // 未标定
+```
+
+**问题：**
+
+`robot_params.h` 注释写着 `side_sensor_inset` 是"未分侧设置时的默认值"，但 `corridor_ekf.c` 中**没有任何 fallback 逻辑**——它直接用 `left_sensor_inset`（0.0f）和 `right_sensor_inset`（0.0f），完全忽略了已经实测好的 `-0.018f`。
+
+**影响：** EKF 观测方程在双侧有效时退化为：
+
+```
+z_ey = (d_right_avg - d_left_avg) / 2
+```
+
+没有任何安装偏置补偿。如果传感器安装面距车体边缘实际有 18mm 的偏差（`side_sensor_inset = -0.018`），或者左右不对称，EKF **本身就会算出带系统性偏置的 `e_y`**。
+
+### 发现 2：控制器 bypass 绕过 EKF（阻塞性 bug）
+
+**文件：** `App/nav/corridor_ctrl.c:75-85`
+
+```c
+float e_y_ctrl = state->e_y;
+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);   // 覆盖 EKF 结果
+}
+```
+
+**问题：**
+
+当双侧激光都有效时，控制器丢弃 `state->e_y`，改用原始 `(d_right - d_left)/2`。这个公式**不含任何 inset 补偿**。
+
+**关键推论：** 当前 `left/right_sensor_inset` 都是 0.0f 时，EKF 的 `z_ey` 和 bypass 的公式**完全等价**：
+
+```
+EKF:    z_ey = [(W-Rw)/2 + 0 - d_left] + [d_right - (W-Rw)/2 - 0] / 2 = (d_right - d_left) / 2
+Bypass: e_y  = (d_right - d_left) / 2
+```
+
+所以**当前状态下 bypass 不是偏移的直接制造者**——EKF 输出与 bypass 结果几乎相同。但 bypass 的架构问题在于：即使将来标定了 inset，控制器依然用不上——它直接跳过了 EKF 的补偿结果。
+
+### 发现 3：无其他符号/算法错误
+
+- 控制律符号正确：`e_y > 0`（偏左）→ `w_cmd < 0`（右转回中）
+- EKF 预测/更新、Joseph 协方差更新、马氏距离检验均无明显问题
+- IMU 航向观测通道独立且正确
+
+---
+
+## 与原诊断报告的分歧
+
+| 因素 | 原报告评级 | 本次评级 | 理由 |
+|------|-----------|---------|------|
+| 控制器 bypass | **主因** | 阻塞性 bug | 当前 inset=0 时 bypass 与 EKF 输出等价，不是偏移的直接来源；但堵死了标定修复的路 |
+| inset 未生效 | 叠加因素 | **主因** | `-0.018f` 已实测但 EKF 从未使用；left/right=0 导致观测方程无偏置补偿 |
+
+**核心观点：** 原报告说"删 bypass 就一劳永逸"，但如果只删 bypass 而不修 inset，EKF 自身的 `state->e_y` 同样带偏——问题不会改善。
+
+---
+
+## 修复建议
+
+### 修复 1：EKF 添加 inset fallback（立刻见效）
+
+**文件：** `App/est/corridor_ekf.c` 第 451-454 行附近
+
+```c
+// 修改前：
+float d_center_left  = (W - Rw) / 2.0f + s_cfg.left_sensor_inset;
+float d_center_right = (W - Rw) / 2.0f + s_cfg.right_sensor_inset;
+
+// 修改后：
+float l_inset = (s_cfg.left_sensor_inset  != 0.0f) ? s_cfg.left_sensor_inset  : s_cfg.side_sensor_inset;
+float r_inset = (s_cfg.right_sensor_inset != 0.0f) ? s_cfg.right_sensor_inset : s_cfg.side_sensor_inset;
+float d_center_left  = (W - Rw) / 2.0f + l_inset;
+float d_center_right = (W - Rw) / 2.0f + r_inset;
+```
+
+这样 `-0.018f` 会立即作为两侧默认值生效，消除统一偏置。
+
+### 修复 2：删除控制器 bypass（必须配合修复 1）
+
+**文件：** `App/nav/corridor_ctrl.c` 第 75-85 行
+
+删除 `if (left_ok && right_ok) { ... }` 分支，始终使用 `state->e_y`。
+
+### 修复 3：实车标定分侧 inset（消除左右不对称）
+
+1. 将车精确放在垄沟正中央（卷尺确认）
+2. 记录左右 VL53 前后平均读数
+3. 计算：
+   - `left_inset  = 0.10 - 实测左侧平均读数`
+   - `right_inset = 0.10 - 实测右侧平均读数`
+4. 填入 `PARAM_VL53_LEFT_INSET` 和 `PARAM_VL53_RIGHT_INSET`
+
+---
+
+## 验证方法
+
+### 快速验证（不需要上车）
+
+在调试日志中对比以下两个值：
+- `state->e_y`（EKF 输出）
+- `(d_right - d_left) / 2`（bypass 计算）
+
+如果两者几乎相同，证实 bypass 不是偏移的直接来源，inset 缺失才是。
+
+### 上车验证
+
+修复 1+2 后：
+1. 车应从垄沟正中启动后保持居中
+2. 手推偏后应能自动回中
+3. `wall_escape` 不应频繁触发

问题诊断报告.md

@@ -0,0 +1,107 @@
+# 垄沟行走无法居中/持续侧墙问题诊断报告
+
+## 现象描述
+
+- 垄沟内可以**笔直向前走**，说明航向控制（IMU）正常。
+- 但**无法在垄沟正中行驶**，车身稳定偏向一侧墙壁。
+- 表现为：持续侧墙行驶，无法回中。
+
+> **初步判断**：不是 IMU 精度问题（你确认 IMU 精度很高），而是**横向居中控制存在系统性偏差**。
+
+---
+
+## 根因分析
+
+### 1. 核心 Bug：`corridor_ctrl.c` 控制器绕过 EKF 滤波结果
+
+**文件：** `App/nav/corridor_ctrl.c:75-85`
+
+```c
+float e_y_ctrl = state->e_y;
+bool left_ok = /* 左前+左后有效 */;
+bool right_ok = /* 右前+右后有效 */;
+
+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);   // <-- 直接覆盖 EKF 结果！
+}
+```
+
+**问题：**
+
+当双侧激光都有效时，控制器**丢弃了 EKF 输出**的 `state->e_y`，改用原始的 `(d_right - d_left) / 2` 作为横向偏差。
+
+这个做法隐含一个致命假设：**"车在正中时，左右激光读数相等"**。
+
+但实际情况是：
+- 左右 VL53L0X 传感器安装**不可能绝对对称**，哪怕差 2~3mm，在 40cm 垄沟 + 20cm 车体 = 每边仅 10cm 的裕量下，就是 **2~3% 的系统误差**。
+- 控制器拿到这个带偏差的 `e_y_ctrl`，认为"偏左"的位置实际上是正中，于是稳定地把车锁在偏右的位置行驶。
+
+**为什么车头能走直但回不了中？**
+
+- `kp_theta * e_th` + `kd_theta * imu_wz` 这组负责**航向**，与激光无关 → 车头笔直。
+- `kp_y * e_y_ctrl` 这组负责**横向回中**，但 `e_y_ctrl` 本身带系统性偏置 → 回中目标点本身就是歪的。
+
+---
+
+### 2. 叠加因素：传感器内缩参数 `left/right_sensor_inset` 未标定
+
+**文件：** `App/robot_params.h:107,113`
+
+```c
+#define PARAM_VL53_LEFT_INSET       0.0f   // 注释：当前为 0.0 表示未标定
+#define PARAM_VL53_RIGHT_INSET      0.0f   // 注释：当前为 0.0 表示未标定
+```
+
+虽然控制器 bug 是直接原因，但 `left/right_sensor_inset` 未标定也会影响：
+
+- **EKF 层面：** `corridor_ekf.c:453-454` 用 `d_center_left = (W - Rw)/2 + left_sensor_inset` 计算期望居中读数。`inset = 0` 时，EKF 本身对单侧退化场景的计算也存在偏差。
+- **单侧退化时恶性循环：** 车因 bug 偏向一侧 → 近墙激光进入饱和/退化区（< 3cm）→ 系统退化为单侧观测 → 未标定的 `inset` 让 EKF 进一步算错位置 → 偏差放大 → 更贴墙。
+
+---
+
+## 诊断结论
+
+| 因素 | 影响程度 | 说明 |
+|------|---------|------|
+| **控制器绕过 EKF（Bug）** | **主因** | 双侧有效时直接丢弃滤波结果，用原始差值驱动回中，引入不可消除的安装不对称误差 |
+| **left/right_sensor_inset 未标定** | 叠加 | 即使修复 bug，单侧退化场景下 EKF 输出仍不精确；标定后整体鲁棒性更好 |
+| IMU 精度 | 无关 | 航向控制正常，IMU 工作良好 |
+| 激光硬件 | 无关 | 传感器能正常输出读数，问题在于怎么用读数 |
+
+**总体结论：这是软件/参数问题，不是硬件问题。**
+
+---
+
+## 修复建议
+
+### 方案 A：改代码（推荐，一劳永逸）
+
+删除 `App/nav/corridor_ctrl.c` 中第 `75-85` 行的 `if (left_ok && right_ok)` 分支，**始终使用 `state->e_y`** 作为横向偏差输入控制器。
+
+理由：
+- EKF 已经综合了 `corridor_width`、`robot_width`、`left/right_sensor_inset` 做了完整几何补偿。
+- EKF 还做了马氏距离异常检测和协方差加权，噪声滤波远好于控制器层直接算差值。
+- 控制器不应该"越级"跳过 EKF 做原始观测融合。
+
+### 方案 B：标定参数（也必须做）
+
+按 `robot_params.h` 注释中的方法：
+1. 用卷尺将车**精确放在垄沟正中央**。
+2. 记录左右 VL53 前后平均读数。
+3. 计算：
+   - `left_inset = 0.10 - 实测左侧平均读数`
+   - `right_inset = 0.10 - 实测右侧平均读数`
+4. 填入 `PARAM_VL53_LEFT_INSET` 和 `PARAM_VL53_RIGHT_INSET`。
+
+> 标定完成后，即使不删代码，控制器的原始差值 `d_right - d_left` 也会更接近真实居中偏差（因为 EKF 标定后输出的 `state->e_y` 更准确，但 bug 依然意味着双侧有效时 EKF 被绕过）。
+
+---
+
+## 验证方法
+
+修复后上垄沟测试，预期表现：
+1. **笔直 + 居中**：车头不偏，车身稳定在垄沟正中间。
+2. 故意用手把车推向一侧，松手后应自动回中。
+3. 近墙时近墙脱离保护（`wall_escape`）不应频繁触发——因为车本就在中间，不需要"救"。