1.0

App/est/corridor_ekf.h

@@ -7,14 +7,18 @@
  *           - e_th : 航向偏差 (rad)
  *           - s    : 沿走廊进度 (m)
  *
- *         观测向量：z = [z_ey, z_eth_L, z_eth_R]^T
- *           - z_ey   : 横向偏差观测 (由左右侧平均距离差计算)
- *           - z_eth_L: 左侧航向观测 (atan2((d_lr-d_lf), L_s))
- *           - z_eth_R: 右侧航向观测 (atan2((d_rf-d_rr), L_s))
+ *         观测模型 (方向 B — IMU 主导航向):
+ *           - 侧墙观测: 1DOF 标量更新，仅更新 e_y (横向偏差)
+ *             z_ey = 左右侧 VL53 平均距离差 (支持分侧内缩补偿)
+ *           - IMU 航向观测: 独立 1DOF 标量更新，更新 e_th
+ *             z_eth_imu = imu_yaw_rad - imu_yaw_ref_rad
+ *           - 侧墙航向观测 (z_eth_L/z_eth_R) 已取消:
+ *             VL53 前后差分噪声过大 (±2cm → ~13° 航向噪声)，不适合做航向源
  *
  *         鲁棒机制：
  *           - χ² 马氏距离检验拒绝异常观测
- *           - 自适应观测噪声 (根据传感器健康度调整 R 矩阵)
+ *           - 分侧 VL53 内缩补偿 (消除左右安装不对称引起的系统性偏置)
+ *           - 单侧观测时自适应增大 R (降低退化状态下的信任度)
  *           - 协方差上界保护 (防止发散)
  */
 
@@ -43,7 +47,7 @@ typedef struct {
     /* 观测噪声协方差 R */
     float r_ey;         // 横向观测噪声方差 (侧墙)
     float r_eth;        // 航向观测噪声方差 (侧墙)
-    float r_eth_imu;    // 航向观测噪声方差 (IMU yaw)，建议远大于 r_eth
+    float r_eth_imu;    // 航向观测噪声方差 (IMU yaw)，IMU 准确度高时可设较小值
 
     /* 初始协方差 */
     float P0_diag[3];   // 初始 P 对角线
@@ -56,7 +60,9 @@ typedef struct {
     float sensor_base_length;  // 同侧前后雷达间距 L_s
     float corridor_width;      // 走廊标准宽度
     float y_offset;            // 期望偏置
-    float side_sensor_inset;   // VL53L0X 侧向传感器内缩距离 (传感器面到车体外边缘)
+    float side_sensor_inset;   // [兼容] 侧向传感器统一内缩距离 (当 left/right 未单独设置时使用)
+    float left_sensor_inset;   // [改进A] 左侧 VL53 内缩距离 (传感器面到车体左外边缘, 实测)
+    float right_sensor_inset;  // [改进A] 右侧 VL53 内缩距离 (传感器面到车体右外边缘, 实测)
     float robot_width;         // 车体外轮廓宽度 (用于单侧退化时的精确定位)
 } CorridorEKFConfig_t;