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

1.0

Browse Source
This commit is contained in:
nitiantuhao
2026-04-04 17:09:19 +08:00
parent ee63bee6e5
commit 1ecdf6c187
9 changed files with 201 additions and 320 deletions
Download Patch File Download Diff File Expand all files Collapse all files

370
App/est/corridor_ekf.c
View File

@@ -217,6 +217,36 @@ void CorridorEKF_Reset(void)
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;
@@ -311,11 +341,21 @@ void CorridorEKF_Predict(float odom_vx, float imu_wz, float dt)
/* =========================================================
* 观测步 (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;
/* 清除新息和拒绝掩码 */
@@ -334,54 +374,29 @@ int CorridorEKF_Update(const CorridorObs_t *obs, CorridorState_t *out_state)
/* 左右侧横向平均距离 */
float d_lf = obs->d_lf, d_lr = obs->d_lr;
float d_rf = obs->d_rf, d_rr = obs->d_rr;
float Ls = s_cfg.sensor_base_length;
float W = s_cfg.corridor_width;
float yoff = s_cfg.y_offset;
float inset = s_cfg.side_sensor_inset;
float Rw = s_cfg.robot_width;
/* 传感器居中时的理论读数 (考虑车体宽度和传感器内缩)
*
* 推导 (以左侧为例)：
* 沟道宽 W，车体宽 Rw，传感器内缩 inset
* 居中时：
* 车体左边缘到左墙 = (W - Rw) / 2
* 传感器到左墙 = (W - Rw) / 2 + inset (传感器比边缘更靠内)
*
* 所以 d_center = (W - Rw) / 2 + inset
*
* 验证：W=0.40, Rw=0.20, inset=0
* d_center = (0.40-0.20)/2 + 0 = 0.10m ✓ (每边余量10cm)
*
* 单侧公式：e_y = d_center - d_left (左侧传感器越近墙，偏差越大)
* 双侧公式：e_y = [(d_center - d_left) + (d_right - d_center)] / 2
* = (d_right - d_left) / 2 (d_center 被消掉)
*
* ⚠ 当 inset = 0 且 Rw = 0 时，d_center = W/2，退化回原始行为
*/
float d_center = (W - Rw) / 2.0f + inset; /* 传感器居中时的理论读数 */
/* 传感器居中时的理论读数 (考虑车体宽度和传感器内缩) */
float d_center = (W - Rw) / 2.0f + inset;
/* 观测值 (测量) */
/* 观测值 (测量) — 仅横向位置，不含航向 */
float z_ey = 0.0f;
float z_eth_L = 0.0f;
float z_eth_R = 0.0f;
int valid_sides = 0;
if (left_ok) {
z_ey += d_center - ((d_lf + d_lr) / 2.0f) - yoff;
z_eth_L = atan2f(d_lr - d_lf, Ls);
z_ey += d_center - ((d_lf + d_lr) / 2.0f) - yoff;
valid_sides++;
}
if (right_ok) {
z_ey += ((d_rf + d_rr) / 2.0f) - d_center - yoff;
z_eth_R = atan2f(d_rf - d_rr, Ls);
z_ey += ((d_rf + d_rr) / 2.0f) - d_center - 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];
@@ -396,290 +411,69 @@ int CorridorEKF_Update(const CorridorObs_t *obs, CorridorState_t *out_state)
}
}
/* 协方差膨胀 (无观测时的信任衰减) */
s_state.P[0][0] += s_cfg.q_ey * 5.0f;
s_state.P[1][1] += s_cfg.q_eth * 5.0f;
/* 协方差膨胀 (无观测时的信任衰减) — 仅膨胀 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;
}
/* ----------------------------------------------------
* 构建观测向量 z 和预测观测 h(x)
* 1DOF 标量 EKF 更新 — 仅 e_y
* ---------------------------------------------------- */
float e_y = s_state.x[0];
float e_th = s_state.x[1];
float e_y = s_state.x[0];
float y_ey = z_ey - e_y;
/* 预测观测 h(x) */
float h_ey = e_y; // 横向: z_ey ≈ e_y
float h_eth_L = e_th; // 左侧航向: z_eth_L ≈ e_th
float h_eth_R = e_th; // 右侧航向: z_eth_R ≈ e_th
float R_ey = s_cfg.r_ey;
if (valid_sides == 2) {
R_ey *= 0.5f;
}
float S_ey = s_state.P[0][0] + R_ey;
/* 新息向量 y = z - h(x) */
float y[3] = {0};
int obs_idx = 0;
/* e_y 观测 */
y[obs_idx++] = z_ey - h_ey;
/* e_th_L 观测 */
if (left_ok) y[obs_idx++] = z_eth_L - h_eth_L;
/* e_th_R 观测 */
if (right_ok) y[obs_idx++] = z_eth_R - h_eth_R;
/* ----------------------------------------------------
* 构建 H 矩阵 (Jacobian of h(x))
* ---------------------------------------------------- */
/* H 布局:
* 航向角观测对应列是 1,0,0 (e_y 观测量)
* 航向角观测对应列是 0,1,0 (e_th 观测量)
* s 不被直接观测
*/
float H[3][3] = {0};
int H_row = 0;
/* e_y 行: H = [1, 0, 0] */
H[H_row][0] = 1.0f; H[H_row][1] = 0.0f; H[H_row][2] = 0.0f;
H_row++;
/* e_th_L 行: H = [0, 1, 0] */
if (left_ok) {
H[H_row][0] = 0.0f; H[H_row][1] = 1.0f; H[H_row][2] = 0.0f;
H_row++;
if (fabsf(S_ey) < 1e-8f) {
goto output_result;
}
/* e_th_R 行: H = [0, 1, 0] */
if (right_ok) {
H[H_row][0] = 0.0f; H[H_row][1] = 1.0f; H[H_row][2] = 0.0f;
H_row++;
}
float d2_ey = y_ey * y_ey / S_ey;
max_maha_d2 = d2_ey;
int obs_count = H_row;
/* ----------------------------------------------------
* 构建观测噪声协方差 R (根据有效侧数量调整)
* ---------------------------------------------------- */
float R[3][3] = {0};
R[0][0] = s_cfg.r_ey; // e_y 的噪声
if (left_ok && right_ok) {
/* 双侧: 航向噪声更小 (两个独立观测平均) */
R[1][1] = s_cfg.r_eth * 0.5f; // e_th_L
R[2][2] = s_cfg.r_eth * 0.5f; // e_th_R
} else {
/* 单侧: 航向噪声较大 */
if (left_ok) {
R[1][1] = s_cfg.r_eth;
}
if (right_ok) {
R[1][1] = s_cfg.r_eth;
}
}
/* ----------------------------------------------------
* 计算新息协方差 S = H * P * H^T + R
* ---------------------------------------------------- */
float HP[3][3] = {0};
for (int i = 0; i < obs_count; i++) {
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
HP[i][j] += H[i][k] * s_state.P[k][j];
}
}
}
float S[3][3] = {0};
for (int i = 0; i < obs_count; i++) {
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
S[i][j] += HP[i][k] * H[j][k]; // H^T: H[j][k] = H[k][j]
}
}
S[i][i] += R[i][i]; // 加观测噪声
}
/* ----------------------------------------------------
* 计算 S 的逆
* ---------------------------------------------------- */
float S_inv[3][3] = {0};
if (obs_count == 1) {
/* 1 观测: 标量 */
if (fabsf(S[0][0]) < 1e-8f) {
goto output_result;
}
S_inv[0][0] = 1.0f / S[0][0];
} else if (obs_count == 2) {
/* 2 观测：2x2 - 拷贝到局部矩阵 */
float S_2x2[2][2];
S_2x2[0][0] = S[0][0]; S_2x2[0][1] = S[0][1];
S_2x2[1][0] = S[1][0]; S_2x2[1][1] = S[1][1];
if (!invert_2x2_sym(S_2x2)) {
goto output_result;
}
/* 拷贝回 S_inv */
S_inv[0][0] = S_2x2[0][0]; S_inv[0][1] = S_2x2[0][1];
S_inv[1][0] = S_2x2[1][0]; S_inv[1][1] = S_2x2[1][1];
} else {
/* 3 观测: 3x3 */
memcpy(S_inv, S, sizeof(float) * 9);
if (!invert_3x3_cholesky(S_inv)) {
goto output_result;
}
}
/* ----------------------------------------------------
* χ² 马氏距离检验 (鲁棒拒绝)
* ---------------------------------------------------- */
float d2_total = 0.0f;
if (obs_count == 1) {
d2_total = mahalanobis_d2_1dof(y[0], S_inv[0][0]);
} else if (obs_count == 2) {
d2_total = mahalanobis_d2_2dof(y, (const float (*)[2])S_inv);
} else {
d2_total = mahalanobis_d2_3dof(y, S_inv);
}
max_maha_d2 = d2_total;
/* 单自由度检验: e_y 单独检验 */
float d2_ey = mahalanobis_d2_1dof(y[0], S_inv[0][0]);
if (d2_ey > s_cfg.chi2_1dof) {
reject_mask |= (1U << 0); // 拒绝 e_y
reject_mask |= (1U << 0);
goto output_result;
}
/* 1 DOF 门限约 3.84 (95%), 约 6.63 (99%) */
/* 如果整体 d² 过大，拒绝最可疑的观测 */
if (obs_count >= 2) {
/* 检验 e_th_L */
if (left_ok && !(reject_mask & (1U << 0))) {
/* 需要重新计算不含 e_y 的马氏距离 */
/* 简化: 用 y[1]^2 / S[1][1] 作为 1DOF 近似 */
if (fabsf(S[1][1]) > 1e-8f) {
float d2_eth_L = y[1] * y[1] / S[1][1];
if (d2_eth_L > s_cfg.chi2_1dof) {
reject_mask |= (1U << 1); // 拒绝 e_th_L
}
}
}
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;
/* 检验 e_th_R */
if (right_ok && !(reject_mask & (1U << 0)) && obs_count >= 3) {
if (fabsf(S[2][2]) > 1e-8f) {
float d2_eth_R = y[2] * y[2] / S[2][2];
if (d2_eth_R > s_cfg.chi2_1dof) {
reject_mask |= (1U << 2); // 拒绝 e_th_R
}
}
}
}
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;
/* ----------------------------------------------------
* 计算卡尔曼增益 K = P * H^T * S^(-1)
* P1 修复: 必须做完整矩阵乘法 (P*H^T) * S_inv，
* 而不能只乘 S_inv 的对角项 S_inv[j][j]。
* 后者会忽略 S 的非对角元素（观测间相关性），
* 导致卡尔曼增益不正确，影响滤波收敛性。
* ---------------------------------------------------- */
float HT[3][3] = {0};
for (int i = 0; i < 3; i++) {
for (int j = 0; j < obs_count; j++) {
HT[i][j] = H[j][i]; // H^T
}
}
/* Step 1: PH^T = P * H^T, 结果为 3×obs_count */
float PHT[3][3] = {0};
for (int i = 0; i < 3; i++) {
for (int j = 0; j < obs_count; j++) {
for (int k = 0; k < 3; k++) {
PHT[i][j] += s_state.P[i][k] * HT[k][j];
}
}
}
/* Step 2: K = PHT * S_inv, 结果为 3×obs_count */
float K[3][3] = {0};
for (int i = 0; i < 3; i++) {
for (int j = 0; j < obs_count; j++) {
for (int k = 0; k < obs_count; k++) {
K[i][j] += PHT[i][k] * S_inv[k][j];
}
}
}
/* ----------------------------------------------------
* 跳过被拒绝的观测，更新剩余观测
* ---------------------------------------------------- */
int used_obs = 0;
for (int i = 0; i < obs_count; i++) {
uint8_t bit = (i == 0) ? (1U << 0) : ((i == 1) ? (1U << 1) : (1U << 2));
if (reject_mask & bit) {
/*
* 关键：被拒绝的观测不仅不能更新状态，
* 也不能参与后续 KH/P 更新，否则会错误收缩协方差。
*/
K[0][i] = 0.0f;
K[1][i] = 0.0f;
K[2][i] = 0.0f;
continue;
}
/* 状态更新: x += K[:, i] * y[i] */
s_state.x[0] += K[0][i] * y[i];
s_state.x[1] += K[1][i] * y[i];
s_state.x[2] += K[2][i] * y[i];
used_obs++;
}
/* ----------------------------------------------------
* 协方差更新: P = (I - K * H) * P_pred
* 简化 Joseph 形式: P = (I - K*H) * P * (I - K*H)^T + K * R * K^T
* 这里使用简化形式: P = (I - K*H) * P
* ---------------------------------------------------- */
float KH[3][3] = {0};
float P_new[3][3];
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
for (int k = 0; k < obs_count; k++) {
KH[i][j] += K[i][k] * H[k][j];
}
}
}
float I_KH[3][3];
I_KH[0][0] = 1.0f - KH[0][0]; I_KH[0][1] = -KH[0][1]; I_KH[0][2] = -KH[0][2];
I_KH[1][0] = -KH[1][0]; I_KH[1][1] = 1.0f - KH[1][1]; I_KH[1][2] = -KH[1][2];
I_KH[2][0] = -KH[2][0]; I_KH[2][1] = -KH[2][1]; I_KH[2][2] = 1.0f - KH[2][2];
float P_tmp[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_tmp[i][j] += I_KH[i][k] * s_state.P[k][j];
}
P_new[i][j] = s_state.P[i][j] - K_ey[i] * s_state.P[0][j];
}
}
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
s_state.P[i][j] = P_tmp[i][j];
s_state.P[i][j] = P_new[i][j];
}
}
symmetrize(s_state.P);
protect_P(s_state.P);
updated_obs_count = used_obs;
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];
@@ -694,24 +488,12 @@ output_result:
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_ratio = 0.0f;
if (obs_count > 0) {
int rejected = 0;
if (reject_mask & (1U << 0)) rejected++;
if (reject_mask & (1U << 1)) rejected++;
if (reject_mask & (1U << 2)) rejected++;
reject_ratio = (float)rejected / (float)obs_count;
}
out_state->conf = clampf(conf_from_P * side_factor * (1.0f - reject_ratio * 0.5f), 0.0f, 1.0f);
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;
}