导航部分效果最好的版本，主分支

App/est/corridor_ekf.h

@@ -0,0 +1,161 @@
+/**
+ * @file   corridor_ekf.h
+ * @brief  鲁棒 EKF 走廊相对定位滤波器
+ *
+ *         状态向量：x = [e_y, e_th, s]^T
+ *           - e_y  : 横向偏差 (m)
+ *           - e_th : 航向偏差 (rad)
+ *           - s    : 沿走廊进度 (m)
+ *
+ *         观测模型 (方向 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° 航向噪声)，不适合做航向源
+ *
+ *         鲁棒机制：
+ *           - χ² 马氏距离检验拒绝异常观测
+ *           - 分侧 VL53 内缩补偿 (消除左右安装不对称引起的系统性偏置)
+ *           - 单侧观测时自适应增大 R (降低退化状态下的信任度)
+ *           - 协方差上界保护 (防止发散)
+ */
+
+#ifndef CORRIDOR_EKF_H
+#define CORRIDOR_EKF_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/* 先引入消息定义 (含 EKF_STATE_DIM/EKF_OBS_DIM 宏) */
+#include "preproc/corridor_msgs.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* =========================================================
+ * EKF 配置参数
+ * ========================================================= */
+typedef struct {
+    /* 过程噪声协方差 Q */
+    float q_ey;         // e_y 过程噪声方差
+    float q_eth;        // e_th 过程噪声方差
+    float q_s;          // s 过程噪声方差
+    float q_ey_eth;     // [改进] e_y 和 e_th 耦合噪声 (横向-航向动力学耦合)
+
+    /* 观测噪声协方差 R */
+    float r_ey;         // 横向观测噪声方差 (侧墙)
+    float r_eth;        // 航向观测噪声方差 (侧墙)
+    float r_eth_imu;    // 航向观测噪声方差 (IMU yaw)，IMU 准确度高时可设较小值
+
+    /* 初始协方差 */
+    float P0_diag[3];   // 初始 P 对角线
+
+    /* χ² 检验门限 */
+    float chi2_1dof;    // 1 自由度门限 (默认 3.84)
+    float chi2_2dof;    // 2 自由度门限 (默认 5.99)
+
+    /* 走廊几何参数 */
+    float sensor_base_length;  // 同侧前后雷达间距 L_s
+    float corridor_width;      // 走廊标准宽度
+    float y_offset;            // 期望偏置
+    float side_sensor_inset;   // [兼容] 侧向传感器统一内缩距离 (当 left/right 未单独设置时使用)
+    float left_sensor_inset;   // [改进A] 左侧 VL53 内缩距离 (传感器面到车体左外边缘, 实测)
+    float right_sensor_inset;  // [改进A] 右侧 VL53 内缩距离 (传感器面到车体右外边缘, 实测)
+    float robot_width;         // 车体外轮廓宽度 (用于单侧退化时的精确定位)
+} CorridorEKFConfig_t;
+
+/* =========================================================
+ * EKF 内部状态
+ * ========================================================= */
+typedef struct {
+    float x[EKF_STATE_DIM];               // 状态向量
+    float P[EKF_STATE_DIM][EKF_STATE_DIM]; // 状态协方差
+    float K[EKF_STATE_DIM][EKF_OBS_DIM];   // 卡尔曼增益
+    float S[EKF_OBS_DIM][EKF_OBS_DIM];     // 新息协方差
+    float S_inv[EKF_OBS_DIM][EKF_OBS_DIM]; // S 的逆
+} CorridorEKFState_t;
+
+/* =========================================================
+ * API 接口
+ * ========================================================= */
+
+/**
+ * @brief 初始化 EKF 滤波器
+ * @param config 配置参数
+ */
+void CorridorEKF_Init(const CorridorEKFConfig_t *config);
+
+/**
+ * @brief EKF 预测步 (时间更新)
+ * @param odom_vx   里程计线速度 (m/s)
+ * @param imu_wz    IMU 角速度 (rad/s)
+ * @param dt        时间间隔 (s)
+ */
+void CorridorEKF_Predict(float odom_vx, float imu_wz, float dt);
+
+/**
+ * @brief EKF 观测步 (测量更新) - 鲁棒版本
+ * @param obs       预处理后的观测快照
+ * @param out_state 输出状态 (含马氏距离、拒绝掩码等)
+ * @return          成功更新的观测数
+ */
+int CorridorEKF_Update(const CorridorObs_t *obs, CorridorState_t *out_state);
+
+/**
+ * @brief EKF IMU 航向观测更新 (独立于侧墙观测，在 Update 后调用)
+ *
+ * 将 IMU 连续 yaw 的变化量作为 e_th 的额外标量观测，执行 1DOF EKF 更新。
+ * 在侧墙观测丢失时 (转弯/单侧退化) 提供航向约束，防止 e_th 漂移。
+ *
+ * @param imu_yaw_rad    IMU 连续 yaw 当前值 (rad)
+ * @param imu_yaw_ref_rad 进入走廊时锁定的 IMU yaw 参考值 (rad)
+ *                        z_eth_imu = imu_yaw_rad - imu_yaw_ref_rad
+ * @param valid          IMU 数据是否有效
+ */
+void CorridorEKF_UpdateIMUYaw(float imu_yaw_rad, float imu_yaw_ref_rad, bool valid);
+
+/**
+ * @brief 获取当前状态估计
+ */
+void CorridorEKF_GetState(CorridorState_t *out);
+
+/**
+ * @brief 重置 EKF 状态
+ */
+void CorridorEKF_Reset(void);
+
+/**
+ * @brief 仅重置航向相关状态，用于掉头后重新建立走廊朝向基准
+ *
+ * 保留横向位置 e_y 与进度 s，只将 e_th 清零并清理其相关协方差，
+ * 避免上一趟积累的航向误差在返程首拍继续驱动控制器猛打方向。
+ */
+void CorridorEKF_ResetHeading(void);
+
+/**
+ * @brief 180° 掉头后重建走廊状态
+ *
+ * 对同一条走廊原地掉头后：
+ * - 航向误差 e_th 应回到 0
+ * - 横向误差 e_y 在新的前进方向下符号需要翻转
+ */
+void CorridorEKF_RebaseAfterTurnaround(void);
+
+/**
+ * @brief 设置过程噪声 (运行时可调)
+ */
+void CorridorEKF_SetProcessNoise(float q_ey, float q_eth, float q_s);
+
+/**
+ * @brief 设置观测噪声 (运行时可调，用于自适应)
+ */
+void CorridorEKF_SetMeasurementNoise(float r_ey, float r_eth);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CORRIDOR_EKF_H */