1.0

App/est/corridor_ekf.h

@@ -0,0 +1,137 @@
+/**
+ * @file   corridor_ekf.h
+ * @brief  鲁棒 EKF 走廊相对定位滤波器
+ *
+ *         状态向量：x = [e_y, e_th, s]^T
+ *           - e_y  : 横向偏差 (m)
+ *           - 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))
+ *
+ *         鲁棒机制：
+ *           - χ² 马氏距离检验拒绝异常观测
+ *           - 自适应观测噪声 (根据传感器健康度调整 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 过程噪声方差
+
+    /* 观测噪声协方差 R */
+    float r_ey;         // 横向观测噪声方差 (侧墙)
+    float r_eth;        // 航向观测噪声方差 (侧墙)
+    float r_eth_imu;    // 航向观测噪声方差 (IMU yaw)，建议远大于 r_eth
+
+    /* 初始协方差 */
+    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;   // VL53L0X 侧向传感器内缩距离 (传感器面到车体外边缘)
+    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 设置过程噪声 (运行时可调)
+ */
+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 */