162 lines
5.7 KiB
C
162 lines
5.7 KiB
C
/**
|
||
* @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 */
|