# 垄沟横向偏移问题 — 代码层面分析

> 基于 `问题诊断报告.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` 不应频繁触发