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-05 10:15:40 +08:00
parent 46e7fcdbb2
commit c2a70c9470
7 changed files with 148 additions and 89 deletions
Download Patch File Download Diff File Expand all files Collapse all files

126
App/est/corridor_ekf.c
View File

@@ -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];
M[2][0] = M[0][2];
M[2][1] = M[1][2];
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 上界保护 */
@@ -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];
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; 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_new[i][j];
}
}
/* 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);
@@ -525,11 +587,13 @@ void CorridorEKF_UpdateIMUYaw(float imu_yaw_rad, float imu_yaw_ref_rad, bool val
{
if (!s_initialized || !valid) return;
/* 观测值: IMU 相对于走廊参考方向的航向偏差 */
float z_eth_imu = imu_yaw_rad - imu_yaw_ref_rad;
/* 观测值: IMU 相对于走廊参考方向的航向偏差
* [改进] 角度归一化防止 ±π 跨越时新息突变 */
float z_eth_imu = wrap_angle(imu_yaw_rad - imu_yaw_ref_rad);
/* 新息: y = z - h(x), h(x) = e_th = x[1] */
float y_imu = z_eth_imu - s_state.x[1];
/* 新息: 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;
@@ -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
* H = [0, 1, 0], 所以 KH[i][j] = K[i] * H[j] = K[i] * δ(j,1) */
float P_new[3][3];
for (int i = 0; i < 3; i++) {
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];
}
}
/* 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);

1
App/est/corridor_ekf.h
View File

@@ -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; // 横向观测噪声方差 (侧墙)

7
App/est/corridor_filter.c
View File

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