138 lines
4.5 KiB
C
138 lines
4.5 KiB
C
/**
|
||
* @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 */
|