1.0

App/VL53L0X_API/platform/vl53_board.c

@@ -4,26 +4,22 @@
 #include "robot_params.h"
 #include "vl53_calibration_config.h"
 
-/* ================= 卡尔曼滤波底层实现 ================= */
+/* ================= EMA滤波底层实现 ================= */
-static void vl53_kalman_init(Vl53Kalman_t *kf, float q, float r) {
+static void vl53_ema_init(Vl53EMA_t *ema, float alpha) {
-    kf->x = 0.0f;
+    ema->x = 0.0f;
-    kf->p = 1.0f;
+    ema->alpha = alpha;
-    kf->q = q;
+    ema->initialized = 0;
-    kf->r = r;
-    kf->initialized = 0;
 }
 
-static float vl53_kalman_update(Vl53Kalman_t *kf, float measurement) {
+static float vl53_ema_update(Vl53EMA_t *ema, float measurement) {
-    if (kf->initialized == 0) {
+    if (ema->initialized == 0) {
-        kf->x = measurement;
+        ema->x = measurement;
-        kf->initialized = 1;
+        ema->initialized = 1;
-        return kf->x;
+        return ema->x;
     }
-    kf->p = kf->p + kf->q;
+    // EMA公式: x_new = alpha * measurement + (1 - alpha) * x_old
-    float k = kf->p / (kf->p + kf->r);
+    ema->x = ema->alpha * measurement + (1.0f - ema->alpha) * ema->x;
-    kf->x = kf->x + k * (measurement - kf->x);
+    return ema->x;
-    kf->p = (1.0f - k) * kf->p;
-    return kf->x;
 }
 
 static const Vl53RuntimeCalibration_t *vl53_get_runtime_calibration(uint8_t id)
@@ -124,8 +120,8 @@ VL53L0X_Error Vl53Board_Init(Vl53Board_t *board, const Vl53BoardHwCfg_t *hw_cfgs
 
         VL53L0X_PlatformSetXShut(&board->dev[i], GPIO_PIN_RESET);
 
-        /* 初始化卡尔曼滤波器：Q/R 从 robot_params.h 读取 */
+        /* 初始化EMA滤波器：alpha 从 robot_params.h 读取 */
-        vl53_kalman_init(&board->kf[i], PARAM_VL53_KALMAN_Q, PARAM_VL53_KALMAN_R);
+        vl53_ema_init(&board->ema[i], PARAM_VL53_EMA_ALPHA);
     }
 
     vTaskDelay(pdMS_TO_TICKS(10));
@@ -191,25 +187,25 @@ VL53L0X_Error Vl53Board_ReadAll(Vl53Board_t *board, Vl53BoardSnapshot_t *snapsho
                 snapshot->range_status[i] = data.RangeStatus;
 
                 if (data.RangeStatus == 0u) {
-                    /* 2. 标记有效并按开关决定是否应用卡尔曼滤波 */
+                    /* 2. 标记有效并按开关决定是否应用EMA滤波 */
                     snapshot->valid_mask |= (1u << i);
-#if PARAM_VL53_USE_KALMAN_FILTER
+#if PARAM_VL53_USE_EMA_FILTER
-                    snapshot->range_mm_filtered[i] = vl53_kalman_update(&board->kf[i], (float)data.RangeMilliMeter);
+                    snapshot->range_mm_filtered[i] = vl53_ema_update(&board->ema[i], (float)data.RangeMilliMeter);
 #else
                     snapshot->range_mm_filtered[i] = (float)data.RangeMilliMeter;
-                    board->kf[i].x = (float)data.RangeMilliMeter;
+                    board->ema[i].x = (float)data.RangeMilliMeter;
-                    board->kf[i].initialized = 1u;
+                    board->ema[i].initialized = 1u;
 #endif
                 } else {
                     /* 测距失败时，滤波值维持上一次的历史最佳估计不变 */
-                    snapshot->range_mm_filtered[i] = board->kf[i].x;
+                    snapshot->range_mm_filtered[i] = board->ema[i].x;
                 }
 
                 VL53L0X_ClearInterruptMask(&board->dev[i], 0u);
             }
         } else {
             /* 如果没准备好，把上一帧的历史值顺延下来，防止读到 0 */
-            snapshot->range_mm_filtered[i] = board->kf[i].x;
+            snapshot->range_mm_filtered[i] = board->ema[i].x;
         }
     }
     return VL53L0X_ERROR_NONE;

App/VL53L0X_API/platform/vl53_board.h

@@ -21,27 +21,25 @@ extern "C" {
         uint8_t id;
     } Vl53BoardHwCfg_t;
 
-    /* ================= 内部卡尔曼滤波器结构 ================= */
+    /* ================= 内部EMA滤波器结构 ================= */
     typedef struct {
-        float x;  // 最优估计值 (滤波后的距离)
+        float x;             // 当前滤波值 (滤波后的距离)
-        float p;  // 估计误差协方差
+        float alpha;         // 平滑系数 (0.0~1.0, 越大响应越快)
-        float q;  // 过程噪声
-        float r;  // 测量噪声
         uint8_t initialized; // 防止第一次开机从0缓慢爬升
-    } Vl53Kalman_t;
+    } Vl53EMA_t;
 
     /* ================= 快照数据结构 (对外接口) ================= */
     typedef struct {
         uint32_t tick_ms;
         uint16_t range_mm[VL53_MAX_DEVS_PER_BOARD];          /* 接口1：原始硬件数据 */
-        float    range_mm_filtered[VL53_MAX_DEVS_PER_BOARD]; /* 接口2：卡尔曼平滑数据 */
+        float    range_mm_filtered[VL53_MAX_DEVS_PER_BOARD]; /* 接口2：EMA平滑数据 */
         uint8_t  range_status[VL53_MAX_DEVS_PER_BOARD];
         uint8_t  valid_mask;
     } Vl53BoardSnapshot_t;
 
     typedef struct {
         VL53L0X_Dev_t dev[VL53_MAX_DEVS_PER_BOARD];
-        Vl53Kalman_t  kf[VL53_MAX_DEVS_PER_BOARD];  /* 每个传感器配备一个专属卡尔曼滤波器 */
+        Vl53EMA_t     ema[VL53_MAX_DEVS_PER_BOARD];  /* 每个传感器配备一个专属EMA滤波器 */
         uint8_t init_mask;
         uint8_t dev_count;
         uint32_t timing_budget_us;

App/VL53L0X_API/platform/vl53_calibration_config.h

@@ -22,13 +22,13 @@ typedef struct {
  */
 static const Vl53RuntimeCalibration_t k_vl53_left_calibration[2] = {
     {
-        .offset_micro_meters = 10000,
+        .offset_micro_meters = 8000,
         .xtalk_compensation_rate_mcps = 0,
         .offset_calibrated = 1,
         .xtalk_calibrated = 0,
     },
     {
-        .offset_micro_meters = 10000,
+        .offset_micro_meters = 8000,
         .xtalk_compensation_rate_mcps = 0,
         .offset_calibrated = 1,
         .xtalk_calibrated = 0,
@@ -37,13 +37,13 @@ static const Vl53RuntimeCalibration_t k_vl53_left_calibration[2] = {
 
 static const Vl53RuntimeCalibration_t k_vl53_right_calibration[2] = {
     {
-        .offset_micro_meters = 9000,
+        .offset_micro_meters = 2000,
         .xtalk_compensation_rate_mcps = 0,
         .offset_calibrated = 1,
         .xtalk_calibrated = 0,
     },
     {
-        .offset_micro_meters = 13000,
+        .offset_micro_meters = 9000,
         .xtalk_compensation_rate_mcps = 0,
         .offset_calibrated = 1,
         .xtalk_calibrated = 0,

App/est/corridor_ekf.c

@@ -46,12 +46,29 @@ static inline float clampf(float val, float lo, float hi)
     return val;
 }
 
-/** 对称矩阵拷贝 + 上三角 symmetrize */
+/** 对称矩阵拷贝 + 双向取平均 (减少舍入误差传播) */
 static void symmetrize(float M[3][3])
 {
-    M[1][0] = M[0][1];
+    float avg;
-    M[2][0] = M[0][2];
+    avg = (M[0][1] + M[1][0]) * 0.5f;
-    M[2][1] = M[1][2];
+    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 上界保护 */
@@ -63,6 +80,57 @@ static void protect_P(float P[3][3])
     }
 }
 
+/** 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])
 {
@@ -302,11 +370,13 @@ void CorridorEKF_Predict(float odom_vx, float imu_wz, float dt)
     F[2][1] = -vx * sin_th * dt;  // ds/de_th
     F[2][2] = 1.0f;
 
-    /* 过程噪声协方差 Q (对角) */
+    /* 过程噪声协方差 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};
@@ -324,8 +394,8 @@ void CorridorEKF_Predict(float odom_vx, float imu_wz, float dt)
             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];  // 加过程噪声 (含非对角项)
         }
-        P_pred[i][i] += Q[i][i];  // 加过程噪声
     }
 
     symmetrize(P_pred);
@@ -462,18 +532,10 @@ int CorridorEKF_Update(const CorridorObs_t *obs, CorridorState_t *out_state)
     s_state.x[1] += K_ey[1] * y_ey;
     s_state.x[2] += K_ey[2] * y_ey;
 
-    float P_new[3][3];
+    /* Joseph 形式协方差更新: P = (I-KH)*P*(I-KH)^T + K*R*K^T
-    for (int i = 0; i < 3; i++) {
+     * H = [1, 0, 0] (仅观测 e_y) */
-        for (int j = 0; j < 3; j++) {
+    float H_ey[3] = {1.0f, 0.0f, 0.0f};
-            P_new[i][j] = s_state.P[i][j] - K_ey[i] * s_state.P[0][j];
+    joseph_update_P(s_state.P, K_ey, H_ey, R_ey);
-        }
-    }
-
-    for (int i = 0; i < 3; i++) {
-        for (int j = 0; j < 3; j++) {
-            s_state.P[i][j] = P_new[i][j];
-        }
-    }
 
     symmetrize(s_state.P);
     protect_P(s_state.P);
@@ -525,11 +587,13 @@ void CorridorEKF_UpdateIMUYaw(float imu_yaw_rad, float imu_yaw_ref_rad, bool val
 {
     if (!s_initialized || !valid) return;
 
-    /* 观测值: IMU 相对于走廊参考方向的航向偏差 */
+    /* 观测值: IMU 相对于走廊参考方向的航向偏差
-    float z_eth_imu = imu_yaw_rad - imu_yaw_ref_rad;
+     * [改进] 角度归一化防止 ±π 跨越时新息突变 */
+    float z_eth_imu = wrap_angle(imu_yaw_rad - imu_yaw_ref_rad);
 
-    /* 新息: y = z - h(x), h(x) = e_th = x[1] */
+    /* 新息: y = z - h(x), h(x) = e_th = x[1]
-    float y_imu = z_eth_imu - s_state.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;
@@ -553,20 +617,10 @@ void CorridorEKF_UpdateIMUYaw(float imu_yaw_rad, float imu_yaw_ref_rad, bool val
     s_state.x[1] += K[1] * y_imu;
     s_state.x[2] += K[2] * y_imu;
 
-    /* 协方差更新: P = (I - K*H) * P
+    /* Joseph 形式协方差更新: P = (I-KH)*P*(I-KH)^T + K*R*K^T
-     * H = [0, 1, 0], 所以 KH[i][j] = K[i] * H[j] = K[i] * δ(j,1) */
+     * H = [0, 1, 0] (仅观测 e_th) */
-    float P_new[3][3];
+    float H_imu[3] = {0.0f, 1.0f, 0.0f};
-    for (int i = 0; i < 3; i++) {
+    joseph_update_P(s_state.P, K, H_imu, R_imu);
-        for (int j = 0; j < 3; j++) {
-            P_new[i][j] = s_state.P[i][j] - K[i] * s_state.P[1][j];
-        }
-    }
-
-    for (int i = 0; i < 3; i++) {
-        for (int j = 0; j < 3; j++) {
-            s_state.P[i][j] = P_new[i][j];
-        }
-    }
 
     symmetrize(s_state.P);
     protect_P(s_state.P);

App/est/corridor_ekf.h

@@ -43,6 +43,7 @@ typedef struct {
     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;         // 横向观测噪声方差 (侧墙)

App/est/corridor_filter.c

@@ -47,6 +47,7 @@ void CorridorFilter_Init(const CorridorFilterConfig_t *config)
     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;       /* 横向观测噪声 (侧墙) */

App/robot_params.h

@@ -168,6 +168,15 @@ extern "C" {
  */
 #define PARAM_EKF_Q_S               0.1f
 
+/** @brief [改进] 横向-航向耦合过程噪声 [m·rad]
+ *  含义：e_y 和 e_th 的动力学耦合强度
+ *  物理意义：横向偏差会导致航向修正，航向偏差会导致横向漂移
+ *  调优方法：从 0 开始，如果发现横向和航向震荡相关可适当增大
+ *  典型值：0~0.005
+ *  设为 0 则退化为对角 Q 矩阵
+ */
+#define PARAM_EKF_Q_EY_ETH          0.0f
+
 /* --- EKF 观测噪声 R (对传感器信任度) --- */
 /** @brief [调优] 横向观测噪声方差 [m²]
  *  含义：VL53L0X 测距的可靠程度
@@ -368,23 +377,23 @@ extern "C" {
  */
 #define PARAM_VL53_TIMING_BUDGET    33000U
 
-/** @brief [调试] 是否启用 VL53L0X 端卡尔曼滤波
+/** @brief [调试] 是否启用 VL53L0X 端EMA滤波
  *  1: 使用滤波后的 range_mm_filtered
  *  0: 直接输出原始测距到 range_mm_filtered，便于做 A/B 对比
  */
-#define PARAM_VL53_USE_KALMAN_FILTER 1
+#define PARAM_VL53_USE_EMA_FILTER 1
 
-/** @brief [调优] VL53L0X 卡尔曼滤波参数 Q
+/** @brief [调优] VL53L0X EMA滤波平滑系数 alpha
- *  含义：测距过程噪声
+ *  含义：新测量值的权重 (0.0~1.0)
- *  典型值：5.0~20.0
+ *  - 越大响应越快，延迟越低，但抖动越大
+ *  - 越小输出越平滑，但延迟越高
+ *  典型值：
+ *    0.3 - 平滑优先 (延迟约100ms)
+ *    0.4 - 折中 (延迟约60ms，推荐)
+ *    0.5 - 快速响应 (延迟约40ms)
+ *    0.6 - 极速响应 (延迟约25ms，抖动较大)
  */
-#define PARAM_VL53_KALMAN_Q         10.0f
+#define PARAM_VL53_EMA_ALPHA        0.4f
-
-/** @brief [调优] VL53L0X 卡尔曼滤波参数 R
- *  含义：测距观测噪声
- *  典型值：10.0~30.0
- */
-#define PARAM_VL53_KALMAN_R         14.1f
 
 /* --- IMU --- */
 /** @brief [实测] HWT101 IMU 安装偏置 (航向) [rad]