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 23:24:36 +08:00
parent 5e0901a4d9
commit 7c2396a282
11 changed files with 132 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
App/est/corridor_ekf.c
View File

@@ -376,23 +376,24 @@ int CorridorEKF_Update(const CorridorObs_t *obs, CorridorState_t *out_state)
float d_rf = obs->d_rf, d_rr = obs->d_rr;
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;
float Rw = s_cfg.robot_width;
/* 传感器居中时的理论读数 (考虑车体宽度和传感器内缩) */
float d_center = (W - Rw) / 2.0f + inset;
/* [改进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 - ((d_lf + d_lr) / 2.0f) - yoff;
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 - yoff;
z_ey += ((d_rf + d_rr) / 2.0f) - d_center_right - yoff;
valid_sides++;
}
@@ -427,9 +428,15 @@ int CorridorEKF_Update(const CorridorObs_t *obs, CorridorState_t *out_state)
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;