Falling_jasmine/ASER-NAV
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
nitiantuhao
2026-04-13 23:30:22 +08:00
commit 350fd830f4
1679 changed files with 1464081 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

649
App/est/corridor_ekf.c Normal file
View File

@@ -0,0 +1,649 @@
/**
* @file corridor_ekf.c
* @brief 鲁棒 EKF 走廊相对定位滤波器实现
*
* 完整算法流程：
*
* 【预测步】Predict(vx, wz, dt)
* x_pred = f(x, u) -- 非线性状态转移
* P_pred = F * P * F^T + Q -- 协方差预测
*
* 【更新步】Update(obs)
* z = h(x_pred) -- 观测预测
* y = z_meas - z -- 新息 (Innovation)
* S = H * P_pred * H^T + R -- 新息协方差
* d² = y^T * S^(-1) * y -- 马氏距离平方
* if d² > χ²_threshold: 拒绝观测 (鲁棒)
* K = P_pred * H^T * S^(-1) -- 卡尔曼增益
* x = x_pred + K * y -- 状态更新
* P = (I - K * H) * P_pred -- 协方差更新
*/
#include "corridor_ekf.h"
#include <math.h>
#include <string.h>
/* =========================================================
* 内部静态状态
* ========================================================= */
static CorridorEKFConfig_t s_cfg;
static CorridorEKFState_t s_state;
static bool s_initialized = false;
static uint32_t s_last_update_ms = 0U;
/* 协方差上界保护阈值 */
#define P_MAX_DIAG 100.0f
/* =========================================================
* 内部辅助函数
* ========================================================= */
/** 限幅 */
static inline float clampf(float val, float lo, float hi)
{
if (val < lo) return lo;
if (val > hi) return hi;
return val;
}
/** 对称矩阵拷贝 + 双向取平均 (减少舍入误差传播) */
static void symmetrize(float M[3][3])
{
float avg;
avg = (M[0][1] + M[1][0]) * 0.5f;
M[0][1] = M[1][0] = avg;
avg = (M[0][2] + M[2][0]) * 0.5f;
M[0][2] = M[2][0] = avg;
avg = (M[1][2] + M[2][1]) * 0.5f;
M[1][2] = M[2][1] = avg;
}
/** 角度归一化到 [-π, π]
* 防止 IMU yaw 累积角度跨越 ±π 时导致新息突变
*/
static float wrap_angle(float angle)
{
const float PI = 3.14159265358979323846f;
while (angle > PI) angle -= 2.0f * PI;
while (angle < -PI) angle += 2.0f * PI;
return angle;
}
/** P 上界保护 */
static void protect_P(float P[3][3])
{
for (int i = 0; i < 3; i++) {
if (P[i][i] > P_MAX_DIAG) P[i][i] = P_MAX_DIAG;
if (P[i][i] < 0.0f) P[i][i] = 0.0f;
}
}
/** Joseph 形式协方差更新 (1DOF 标量观测)
* P_new = (I - K*H) * P * (I - K*H)^T + K * R * K^T
*
* 参数:
* P[3][3] - 先验协方差 (输入/输出)
* K[3] - 卡尔曼增益向量
* H[3] - 观测矩阵 (1x3 行向量)
* R - 观测噪声方差 (标量)
*
* 优点: 保证数值稳定性，即使有舍入误差也能保持 P 正定
*/
static void joseph_update_P(float P[3][3], const float K[3], const float H[3], float R)
{
/* 计算 A = (I - K*H) */
float A[3][3];
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
A[i][j] = (i == j ? 1.0f : 0.0f) - K[i] * H[j];
}
}
/* 计算 A * P */
float AP[3][3];
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
AP[i][j] = 0.0f;
for (int k = 0; k < 3; k++) {
AP[i][j] += A[i][k] * P[k][j];
}
}
}
/* 计算 A * P * A^T */
float APAT[3][3];
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
APAT[i][j] = 0.0f;
for (int k = 0; k < 3; k++) {
APAT[i][j] += AP[i][k] * A[j][k]; // A^T[k][j] = A[j][k]
}
}
}
/* 计算 K * R * K^T 并加到 APAT */
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
P[i][j] = APAT[i][j] + K[i] * R * K[j];
}
}
}
/** 计算 2x2 对称矩阵的逆 (原地) */
static bool invert_2x2_sym(float S[2][2])
{
float det = S[0][0] * S[1][1] - S[0][1] * S[1][0];
if (fabsf(det) < 1e-8f) {
return false; // 奇异矩阵
}
float inv_det = 1.0f / det;
float S00 = S[0][0];
S[0][0] = inv_det * S[1][1];
S[1][1] = inv_det * S00;
S[0][1] = -inv_det * S[0][1];
S[1][0] = S[0][1];
return true;
}
/** 2x2 对称矩阵求逆 (原地) */
static bool invert_3x3_cholesky(float S[3][3])
{
// 使用 Cholesky 分解求逆
float L[3][3] = {0};
for (int i = 0; i < 3; i++) {
for (int j = 0; j <= i; j++) {
float sum = S[i][j];
for (int k = 0; k < j; k++) {
sum -= L[i][k] * L[j][k];
}
if (i == j) {
if (sum <= 0.0f) return false;
L[i][j] = sqrtf(sum);
} else {
L[i][j] = sum / L[j][j];
}
}
}
// 求逆: S_inv = L^(-T) * L^(-1)
float Linv[3][3] = {0};
for (int i = 0; i < 3; i++) {
Linv[i][i] = 1.0f / L[i][i];
for (int j = i - 1; j >= 0; j--) {
float sum = 0.0f;
for (int k = j + 1; k <= i; k++) {
sum += L[k][j] * Linv[k][i];
}
Linv[j][i] = -sum / L[j][j];
}
}
// S_inv = Linv^T * Linv
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
float sum = 0.0f;
for (int k = 0; k < 3; k++) {
sum += Linv[k][i] * Linv[k][j];
}
S[i][j] = sum;
}
}
return true;
}
/** 计算马氏距离平方 (新息向量 y, 新息协方差 S_inv) */
static float mahalanobis_d2_3dof(const float y[3], const float S_inv[3][3])
{
// d² = y^T * S_inv * y
float tmp[3] = {0};
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
tmp[i] += S_inv[i][j] * y[j];
}
}
float d2 = 0.0f;
for (int i = 0; i < 3; i++) {
d2 += y[i] * tmp[i];
}
return d2;
}
/** 计算马氏距离平方 (1DOF: 只用 e_y) */
static float mahalanobis_d2_1dof(float y_ey, float S_inv_00)
{
return y_ey * y_ey * S_inv_00;
}
/** 计算马氏距离平方 (2DOF: e_ey + e_th_avg) */
static float mahalanobis_d2_2dof(const float y[2], const float S_inv[2][2])
{
float tmp[2] = {0};
for (int i = 0; i < 2; i++) {
for (int j = 0; j < 2; j++) {
tmp[i] += S_inv[i][j] * y[j];
}
}
float d2 = 0.0f;
for (int i = 0; i < 2; i++) {
d2 += y[i] * tmp[i];
}
return d2;
}
/** 零矩阵 */
static void zero_3x3(float M[3][3])
{
memset(M, 0, sizeof(float) * 9);
}
/** 单位矩阵 */
static void eye_3x3(float M[3][3])
{
zero_3x3(M);
M[0][0] = M[1][1] = M[2][2] = 1.0f;
}
/* =========================================================
* API 实现
* ========================================================= */
void CorridorEKF_Init(const CorridorEKFConfig_t *config)
{
s_cfg = *config;
memset(&s_state, 0, sizeof(s_state));
/* 初始化状态 */
s_state.x[0] = 0.0f; // e_y
s_state.x[1] = 0.0f; // e_th
s_state.x[2] = 0.0f; // s
/* 初始化协方差 */
eye_3x3(s_state.P);
s_state.P[0][0] = config->P0_diag[0];
s_state.P[1][1] = config->P0_diag[1];
s_state.P[2][2] = config->P0_diag[2];
s_initialized = true;
}
void CorridorEKF_Reset(void)
{
if (!s_initialized) return;
s_state.x[0] = 0.0f;
s_state.x[1] = 0.0f;
s_state.x[2] = 0.0f;
eye_3x3(s_state.P);
s_state.P[0][0] = s_cfg.P0_diag[0];
s_state.P[1][1] = s_cfg.P0_diag[1];
s_state.P[2][2] = s_cfg.P0_diag[2];
s_last_update_ms = 0U;
}
void CorridorEKF_ResetHeading(void)
{
if (!s_initialized) return;
s_state.x[1] = 0.0f;
/* 清理航向与其它状态的耦合，避免旧航向误差继续通过协方差传播。 */
s_state.P[0][1] = 0.0f;
s_state.P[1][0] = 0.0f;
s_state.P[1][2] = 0.0f;
s_state.P[2][1] = 0.0f;
s_state.P[1][1] = s_cfg.P0_diag[1];
}
void CorridorEKF_RebaseAfterTurnaround(void)
{
if (!s_initialized) return;
/* 同一条走廊掉头后，新的前进方向相反，横向误差符号需要镜像。 */
s_state.x[0] = -s_state.x[0];
s_state.x[1] = 0.0f;
/* e_y 与 e_th 的相关项在掉头后不再可直接沿用，清零重新收敛。 */
s_state.P[0][1] = 0.0f;
s_state.P[1][0] = 0.0f;
s_state.P[1][2] = 0.0f;
s_state.P[2][1] = 0.0f;
s_state.P[1][1] = s_cfg.P0_diag[1];
}
void CorridorEKF_SetProcessNoise(float q_ey, float q_eth, float q_s)
{
s_cfg.q_ey = q_ey;
s_cfg.q_eth = q_eth;
s_cfg.q_s = q_s;
}
void CorridorEKF_SetMeasurementNoise(float r_ey, float r_eth)
{
s_cfg.r_ey = r_ey;
s_cfg.r_eth = r_eth;
}
/* =========================================================
* 预测步 (Predict)
* ========================================================= */
void CorridorEKF_Predict(float odom_vx, float imu_wz, float dt)
{
if (!s_initialized || dt <= 0.0f) return;
float e_y = s_state.x[0];
float e_th = s_state.x[1];
float s = s_state.x[2];
float vx = odom_vx;
float wz = imu_wz;
/* 状态预测: x_pred = f(x, u) */
float cos_th = cosf(e_th);
float sin_th = sinf(e_th);
/* 安全检查: cos_th 不能太小 (防止数值爆炸) */
if (fabsf(cos_th) < 0.01f) cos_th = (cos_th >= 0.0f) ? 0.01f : -0.01f;
float e_y_pred = e_y + vx * sin_th * dt;
float e_th_pred = e_th + wz * dt;
float s_pred = s + vx * cos_th * dt;
s_state.x[0] = e_y_pred;
s_state.x[1] = e_th_pred;
s_state.x[2] = s_pred;
/* 雅可比矩阵 F (状态转移的 Jacobian) */
float F[3][3] = {0};
F[0][0] = 1.0f;
F[0][1] = vx * cos_th * dt; // de_y/de_th
F[0][2] = 0.0f;
F[1][0] = 0.0f;
F[1][1] = 1.0f;
F[1][2] = 0.0f;
F[2][0] = 0.0f;
F[2][1] = -vx * sin_th * dt; // ds/de_th
F[2][2] = 1.0f;
/* 过程噪声协方差 Q (含耦合项)
* [改进] 添加 e_y 和 e_th 的耦合噪声，反映横向-航向动力学耦合 */
float Q[3][3] = {0};
Q[0][0] = s_cfg.q_ey * dt * dt;
Q[1][1] = s_cfg.q_eth * dt * dt;
Q[2][2] = s_cfg.q_s * dt * dt;
Q[0][1] = Q[1][0] = s_cfg.q_ey_eth * dt * dt; // 横向-航向耦合
/* 协方差预测: P_pred = F * P * F^T + Q */
float F_P[3][3] = {0};
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
F_P[i][j] += F[i][k] * s_state.P[k][j];
}
}
}
float P_pred[3][3] = {0};
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
P_pred[i][j] += F_P[i][k] * F[j][k]; // F^T: F[j][k] = F[k][j]
}
P_pred[i][j] += Q[i][j]; // 加过程噪声 (含非对角项)
}
}
symmetrize(P_pred);
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
s_state.P[i][j] = P_pred[i][j];
}
}
protect_P(s_state.P);
}
/* =========================================================
* 观测步 (Update) - 鲁棒 EKF
*
* 设计决策 (方向 B — IMU 主导航向)：
* 侧墙激光仅用于更新横向位置 e_y，不再构建航向观测 z_eth_L/z_eth_R。
* 航向 e_th 完全由 IMU 主导：
* - 预测步: imu_wz 驱动 e_th 积分
* - 观测步: CorridorEKF_UpdateIMUYaw() 提供 yaw_continuous 标量约束
* 侧墙前后差分 (d_lr-d_lf) 的噪声在 ±2cm 误差下过大，不适合做航向主观测。
* ========================================================= */
int CorridorEKF_Update(const CorridorObs_t *obs, CorridorState_t *out_state)
{
if (!s_initialized) return 0;
/* 维护最近一次 EKF 输出对应的观测时间戳，供 GetState() 返回一致结果。 */
s_last_update_ms = obs->t_ms;
int updated_obs_count = 0;
/* 清除新息和拒绝掩码 */
memset(s_state.K, 0, sizeof(s_state.K));
uint8_t reject_mask = 0U;
float max_maha_d2 = 0.0f;
/* ----------------------------------------------------
* 提取有效观测
* ---------------------------------------------------- */
bool left_ok = ((obs->valid_mask & (1U << 0)) != 0U) &&
((obs->valid_mask & (1U << 1)) != 0U);
bool right_ok = ((obs->valid_mask & (1U << 2)) != 0U) &&
((obs->valid_mask & (1U << 3)) != 0U);
/* 左右侧横向平均距离 */
float d_lf = obs->d_lf, d_lr = obs->d_lr;
float d_rf = obs->d_rf, d_rr = obs->d_rr;
float W = s_cfg.corridor_width;
float yoff = s_cfg.y_offset;
float Rw = s_cfg.robot_width;
/* [改进A] 分侧传感器内缩补偿 — 消除左右安装不对称引起的系统性偏置
* 左右各自使用独立的 inset 值计算期望居中读数 d_center */
float d_center_left = (W - Rw) / 2.0f + s_cfg.left_sensor_inset;
float d_center_right = (W - Rw) / 2.0f + s_cfg.right_sensor_inset;
/* 观测值 (测量) — 仅横向位置，不含航向 */
float z_ey = 0.0f;
int valid_sides = 0;
if (left_ok) {
z_ey += d_center_left - ((d_lf + d_lr) / 2.0f) - yoff;
valid_sides++;
}
if (right_ok) {
z_ey += ((d_rf + d_rr) / 2.0f) - d_center_right - yoff;
valid_sides++;
}
if (valid_sides == 0) {
out_state->t_ms = obs->t_ms;
out_state->e_y = s_state.x[0];
out_state->e_th = s_state.x[1];
out_state->s = s_state.x[2];
out_state->conf = clampf(1.0f - (s_state.P[0][0] + s_state.P[1][1]) * 0.1f, 0.0f, 1.0f);
out_state->obs_reject_mask = 0xFF;
out_state->mahalanobis_d2 = 0.0f;
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
out_state->P[i][j] = s_state.P[i][j];
}
}
/* 协方差膨胀 (无观测时的信任衰减) — 仅膨胀 e_y */
s_state.P[0][0] += s_cfg.q_ey * 5.0f;
protect_P(s_state.P);
return 0;
}
if (valid_sides == 2) {
z_ey /= 2.0f;
}
/* ----------------------------------------------------
* 1DOF 标量 EKF 更新 — 仅 e_y
* ---------------------------------------------------- */
float e_y = s_state.x[0];
float y_ey = z_ey - e_y;
/* [改进F] 自适应观测噪声 R:
* 双侧观测: R × 0.5 (噪声互相平均)
* 单侧观测: R × 3.0 (缺少交叉验证, VL53 可信度低时尤需降低信任)
*/
float R_ey = s_cfg.r_ey;
if (valid_sides == 2) {
R_ey *= 0.5f;
} else if (valid_sides == 1) {
R_ey *= 3.0f;
}
float S_ey = s_state.P[0][0] + R_ey;
if (fabsf(S_ey) < 1e-8f) {
goto output_result;
}
float d2_ey = y_ey * y_ey / S_ey;
max_maha_d2 = d2_ey;
if (d2_ey > s_cfg.chi2_1dof) {
reject_mask |= (1U << 0);
goto output_result;
}
float S_inv_ey = 1.0f / S_ey;
float K_ey[3];
K_ey[0] = s_state.P[0][0] * S_inv_ey;
K_ey[1] = s_state.P[1][0] * S_inv_ey;
K_ey[2] = s_state.P[2][0] * S_inv_ey;
s_state.x[0] += K_ey[0] * y_ey;
s_state.x[1] += K_ey[1] * y_ey;
s_state.x[2] += K_ey[2] * y_ey;
/* Joseph 形式协方差更新: P = (I-KH)*P*(I-KH)^T + K*R*K^T
* H = [1, 0, 0] (仅观测 e_y) */
float H_ey[3] = {1.0f, 0.0f, 0.0f};
joseph_update_P(s_state.P, K_ey, H_ey, R_ey);
symmetrize(s_state.P);
protect_P(s_state.P);
updated_obs_count = 1;
output_result:
out_state->t_ms = obs->t_ms;
out_state->e_y = s_state.x[0];
out_state->e_th = s_state.x[1];
out_state->s = s_state.x[2];
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
out_state->P[i][j] = s_state.P[i][j];
}
}
out_state->mahalanobis_d2 = max_maha_d2;
out_state->obs_reject_mask = reject_mask;
float P_trace = s_state.P[0][0] + s_state.P[1][1] + s_state.P[2][2];
float conf_from_P = clampf(1.0f - P_trace * 0.5f, 0.0f, 1.0f);
float side_factor = (valid_sides == 2) ? 1.0f : 0.7f;
float reject_penalty = (reject_mask & (1U << 0)) ? 0.5f : 1.0f;
out_state->conf = clampf(conf_from_P * side_factor * reject_penalty, 0.0f, 1.0f);
return updated_obs_count;
}
/* =========================================================
* 辅助 API
* ========================================================= */
/* =========================================================
* IMU 航向观测更新 (独立 1DOF 标量 EKF 更新)
*
* 观测方程: z_eth_imu = imu_yaw_rad - imu_yaw_ref_rad
* 对应状态: e_th (x[1])
* H = [0, 1, 0] (只观测 e_th)
*
* 设计意图：
* - IMU 内部维护的 yaw 经过模块校准，比外部积分 wz 更稳定
* - 在侧墙观测丢失 (转弯/单侧退化) 时提供航向约束
* - 使用较大 R 值，让侧墙观测在有效时主导
* ========================================================= */
void CorridorEKF_UpdateIMUYaw(float imu_yaw_rad, float imu_yaw_ref_rad, bool valid)
{
if (!s_initialized || !valid) return;
/* 观测值: IMU 相对于走廊参考方向的航向偏差
* [改进] 角度归一化防止 ±π 跨越时新息突变 */
float z_eth_imu = wrap_angle(imu_yaw_rad - imu_yaw_ref_rad);
/* 新息: y = z - h(x), h(x) = e_th = x[1]
* [改进] 再次归一化，防止 e_th 和 z_eth_imu 符号不同时差值超出 [-π, π] */
float y_imu = wrap_angle(z_eth_imu - s_state.x[1]);
/* H = [0, 1, 0] → S = P[1][1] + R_imu */
float R_imu = s_cfg.r_eth_imu;
float S_imu = s_state.P[1][1] + R_imu;
if (fabsf(S_imu) < 1e-8f) return;
/* χ² 1DOF 检验: d² = y² / S */
float d2_imu = y_imu * y_imu / S_imu;
if (d2_imu > s_cfg.chi2_1dof) return; /* 拒绝异常观测 */
/* 卡尔曼增益: K = P * H^T / S = P[:][1] / S */
float K[3];
float S_inv = 1.0f / S_imu;
K[0] = s_state.P[0][1] * S_inv;
K[1] = s_state.P[1][1] * S_inv;
K[2] = s_state.P[2][1] * S_inv;
/* 状态更新: x += K * y */
s_state.x[0] += K[0] * y_imu;
s_state.x[1] += K[1] * y_imu;
s_state.x[2] += K[2] * y_imu;
/* Joseph 形式协方差更新: P = (I-KH)*P*(I-KH)^T + K*R*K^T
* H = [0, 1, 0] (仅观测 e_th) */
float H_imu[3] = {0.0f, 1.0f, 0.0f};
joseph_update_P(s_state.P, K, H_imu, R_imu);
symmetrize(s_state.P);
protect_P(s_state.P);
}
void CorridorEKF_GetState(CorridorState_t *out)
{
if (!s_initialized || out == NULL) return;
out->t_ms = s_last_update_ms;
out->e_y = s_state.x[0];
out->e_th = s_state.x[1];
out->s = s_state.x[2];
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
out->P[i][j] = s_state.P[i][j];
}
}
out->mahalanobis_d2 = 0.0f;
out->obs_reject_mask = 0U;
float P_trace = s_state.P[0][0] + s_state.P[1][1] + s_state.P[2][2];
out->conf = clampf(1.0f - P_trace * 0.5f, 0.0f, 1.0f);
}

161
App/est/corridor_ekf.h Normal file
View File

@@ -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 */

167
App/est/corridor_filter.c Normal file
View File

@@ -0,0 +1,167 @@
/**
* @file corridor_filter.c
* @brief 走廊相对定位滤波器 - 鲁棒 EKF 实现
*
* 本文件是对外统一接口层，内部调用 corridor_ekf 模块
* 保持与原有互补滤波接口兼容，方便无缝替换
*/
#include "corridor_filter.h"
#include "corridor_ekf.h"
#include "preproc/corridor_msgs.h" /* 引入 CHI2_THRESHOLD 宏 */
#include "robot_params.h" /* 引入 PARAM_EKF_* 宏 */
#include <math.h>
#include <string.h>
/* =========================================================
* 兼容层配置
* ========================================================= */
static CorridorFilterConfig_t s_cfg;
static bool s_initialized = false;
/* IMU yaw 参考值管理：进入走廊时锁定，用于计算相对走廊方向的航向偏差 */
static float s_imu_yaw_ref_rad = 0.0f; // 走廊参考 yaw (rad)
static bool s_imu_yaw_ref_set = false; // 参考值是否已锁定
/* =========================================================
* 初始化
* ========================================================= */
void CorridorFilter_Init(const CorridorFilterConfig_t *config)
{
s_cfg = *config;
/* 转换为 EKF 配置 */
CorridorEKFConfig_t ekf_cfg;
memset(&ekf_cfg, 0, sizeof(ekf_cfg));
/* 走廊几何参数 */
ekf_cfg.sensor_base_length = config->sensor_base_length;
ekf_cfg.corridor_width = config->corridor_width;
ekf_cfg.y_offset = config->y_offset;
ekf_cfg.side_sensor_inset = config->side_sensor_inset;
ekf_cfg.left_sensor_inset = config->left_sensor_inset; /* [改进A] 左侧独立内缩 */
ekf_cfg.right_sensor_inset = config->right_sensor_inset; /* [改进A] 右侧独立内缩 */
ekf_cfg.robot_width = config->robot_width;
/* 过程噪声 Q —— 统一从 robot_params.h 读取，改参数只改那一个文件 */
ekf_cfg.q_ey = PARAM_EKF_Q_EY; /* 横向过程噪声 */
ekf_cfg.q_eth = PARAM_EKF_Q_ETH; /* 航向过程噪声 */
ekf_cfg.q_s = PARAM_EKF_Q_S; /* 里程过程噪声 */
ekf_cfg.q_ey_eth = PARAM_EKF_Q_EY_ETH; /* [改进] 横向-航向耦合噪声 */
/* 观测噪声 R */
ekf_cfg.r_ey = PARAM_EKF_R_EY; /* 横向观测噪声 (侧墙) */
ekf_cfg.r_eth = PARAM_EKF_R_ETH; /* 航向观测噪声 (侧墙) */
ekf_cfg.r_eth_imu = PARAM_EKF_R_ETH_IMU; /* 航向观测噪声 (IMU yaw)，较大 */
/* 初始协方差 */
ekf_cfg.P0_diag[0] = PARAM_EKF_P0_EY; /* e_y 初始不确定度 */
ekf_cfg.P0_diag[1] = PARAM_EKF_P0_ETH; /* e_th 初始不确定度 */
ekf_cfg.P0_diag[2] = 0.0f; /* s 初始确定（已知从 0 出发） */
/* χ² 门限 */
ekf_cfg.chi2_1dof = PARAM_CHI2_1DOF;
ekf_cfg.chi2_2dof = PARAM_CHI2_2DOF;
CorridorEKF_Init(&ekf_cfg);
s_initialized = true;
}
/* =========================================================
* 核心更新函数
* ========================================================= */
void CorridorFilter_Update(const CorridorObs_t *obs, float imu_wz, float odom_vx,
float dt_s, float imu_yaw_continuous_rad, bool imu_yaw_valid,
CorridorState_t *out_state)
{
if (!s_initialized || dt_s <= 0.0f) {
return;
}
/* 预测步 */
CorridorEKF_Predict(odom_vx, imu_wz, dt_s);
/* 侧墙观测更新步 (含马氏距离异常检测) */
CorridorEKF_Update(obs, out_state);
/* ---- IMU yaw 航向观测更新 ---- *
* 在侧墙观测之后独立执行 1DOF 标量更新。
*
* 方向 B 改造：IMU 现在是航向 e_th 的唯一观测来源。
* 侧墙不再提供航向观测。
*
* [改进E] 参考值管理策略 (IMU 驱动即时锁定):
* IMU yaw 准确度高，转向完成后 IMU 指向即走廊方向。
* 不再等 VL53 收敛 (conf >= 0.3)，而是在 EKF reset 后
* 首次收到有效 IMU yaw 即锁定参考值。
* 此时 e_th = 0 (刚 reset)，ref = imu_yaw。
*/
if (imu_yaw_valid) {
if (!s_imu_yaw_ref_set) {
/* [改进E] 即时锁定: 不依赖 VL53 置信度，IMU 有效即锁定 */
s_imu_yaw_ref_rad = imu_yaw_continuous_rad; /* e_th 刚 reset 为 0，ref = imu_yaw */
s_imu_yaw_ref_set = true;
}
if (s_imu_yaw_ref_set) {
CorridorEKF_UpdateIMUYaw(imu_yaw_continuous_rad, s_imu_yaw_ref_rad, true);
/* [改进D] 只更新状态值 (e_y, e_th, s, P)，
* 保留 CorridorEKF_Update() 计算的 conf, obs_reject_mask, mahalanobis_d2。
* 原来直接调用 GetState() 会用纯 P_trace 重算 conf 并清零诊断字段。 */
CorridorState_t imu_updated;
CorridorEKF_GetState(&imu_updated);
out_state->e_y = imu_updated.e_y;
out_state->e_th = imu_updated.e_th;
out_state->s = imu_updated.s;
memcpy(out_state->P, imu_updated.P, sizeof(out_state->P));
/* conf, obs_reject_mask, mahalanobis_d2 保留自 CorridorEKF_Update() 的值 */
out_state->t_ms = obs->t_ms;
}
}
}
/* =========================================================
* 重置 (进入新垄沟时调用)
* ========================================================= */
void CorridorFilter_Reset(void)
{
if (!s_initialized) return;
/* 重置 EKF 内核: 状态归零, 协方差恢复初始值 */
CorridorEKF_Reset();
/* 解锁 IMU yaw 参考值, 等待在新沟中重新锁定 */
s_imu_yaw_ref_rad = 0.0f;
s_imu_yaw_ref_set = false;
}
void CorridorFilter_RebaseAfterTurnaround(float imu_yaw_continuous_rad)
{
if (!s_initialized) return;
/* 同一条沟原地掉头后：当前朝向成为新参考，
* 同时横向误差符号要镜像，航向误差要回零。 */
CorridorEKF_RebaseAfterTurnaround();
s_imu_yaw_ref_rad = imu_yaw_continuous_rad;
s_imu_yaw_ref_set = true;
}
void CorridorFilter_RebaseHeading(float imu_yaw_continuous_rad)
{
if (!s_initialized) return;
CorridorEKF_ResetHeading();
s_imu_yaw_ref_rad = imu_yaw_continuous_rad;
s_imu_yaw_ref_set = true;
}
void CorridorFilter_CorrectIMUReference(float heading_correction_rad)
{
if (!s_initialized || !s_imu_yaw_ref_set) return;
/* 修正IMU参考值，用于消除转向系统性偏差
* 例如：VL53检测到车头偏右2度，heading_correction_rad = -0.035 rad
* 修正后，EKF会认为当前IMU方向才是正确的走廊方向 */
s_imu_yaw_ref_rad += heading_correction_rad;
}

86
App/est/corridor_filter.h Normal file
View File

@@ -0,0 +1,86 @@
#ifndef CORRIDOR_FILTER_H
#define CORRIDOR_FILTER_H
#include "preproc/corridor_preproc.h"
#include "preproc/corridor_msgs.h"
/* 滤波器配置参数结构体 */
typedef struct {
float sensor_base_length; // 同侧前后雷达的纵向安装间距 L_s (m)
float corridor_width; // 走廊标准宽度 (m)，比赛规则为 0.4m
float y_offset; // 期望的偏置行走量 (m)，0 表示绝对居中
float side_sensor_inset; // [兼容] VL53L0X 传感器统一内缩距离 (m)，当分侧值未设置时使用
float left_sensor_inset; // [改进A] 左侧 VL53 内缩距离 (传感器面到车体左外边缘) (m)
float right_sensor_inset; // [改进A] 右侧 VL53 内缩距离 (传感器面到车体右外边缘) (m)
float robot_width; // 车体外轮廓宽度 (m)
float alpha_theta; // 航向互补滤波系数 (0~1)
// 【注】因为您的IMU极好，此值建议设为 0.98~0.995
float alpha_y; // 横向低通滤波系数 (0~1)，建议设为 0.6~0.8 防止地毯颠簸
} CorridorFilterConfig_t;
#ifdef __cplusplus
extern "C" {
#endif
/**
* @brief 初始化走廊滤波器
* @param config 滤波器调参配置
*/
void CorridorFilter_Init(const CorridorFilterConfig_t *config);
/**
* @brief 核心函数：执行一次走廊状态估计 (建议与 CAN 发送同频，如 50Hz/20ms运行)
* @param obs 预处理模块输出的、清洗干净的雷达观测快照
* @param imu_wz 当前的 IMU Z轴角速度 (rad/s)，左转为正
* @param odom_vx 底盘反馈的当前里程计线速度 (m/s)
* @param dt_s 距离上一次调用的时间间隔 (s)
* @param imu_yaw_continuous_rad IMU unwrap 后的连续偏航角 (rad)
* @param imu_yaw_valid IMU 数据是否有效
* @param out_state 输出平滑后的 e_y (横向误差), e_th (航向误差) 和健康度
*/
void CorridorFilter_Update(const CorridorObs_t *obs, float imu_wz, float odom_vx,
float dt_s, float imu_yaw_continuous_rad, bool imu_yaw_valid,
CorridorState_t *out_state);
/**
* @brief 重置滤波器状态 (进入新垄沟时必须调用)
*
* 重置内容：
* - EKF 状态向量清零 (e_y=0, e_th=0, s=0)
* - 协方差恢复到初始值
* - IMU yaw 参考值解锁，等待重新锁定
*/
void CorridorFilter_Reset(void);
/**
* @brief 单沟 180° 掉头后重建滤波器参考
*
* 同一条沟掉头后需要同时：
* - 将当前 IMU yaw 作为新的走廊航向参考
* - 清零航向误差 e_th
* - 镜像横向误差 e_y 的符号
*/
void CorridorFilter_RebaseAfterTurnaround(float imu_yaw_continuous_rad);
/**
* @brief 仅重建航向参考，不改横向位置
*
* 用于进入新垄沟后，已经通过侧墙把车头摆正，
* 这时把当前 IMU yaw 设为新的走廊参考，同时仅清零 e_th。
*/
void CorridorFilter_RebaseHeading(float imu_yaw_continuous_rad);
/**
* @brief 修正IMU航向参考值（用于消除转向系统性偏差）
*
* 用于赛道模式转向后，用VL53检测到的墙壁航向误差修正IMU参考值
* 只修正参考值，不改变当前状态估计
*
* @param heading_correction_rad 航向修正量 (rad)，正值表示车头需要左转
*/
void CorridorFilter_CorrectIMUReference(float heading_correction_rad);
#ifdef __cplusplus
}
#endif
#endif // CORRIDOR_FILTER_H