807 lines
24 KiB
C
807 lines
24 KiB
C
/**
|
||
* @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;
|
||
}
|
||
|
||
/** 对称矩阵拷贝 + 上三角 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];
|
||
}
|
||
|
||
/** 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;
|
||
}
|
||
}
|
||
|
||
/** 计算 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_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 (对角) */
|
||
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;
|
||
|
||
/* 协方差预测: 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][i] += Q[i][i]; // 加过程噪声
|
||
}
|
||
|
||
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
|
||
* ========================================================= */
|
||
int CorridorEKF_Update(const CorridorObs_t *obs, CorridorState_t *out_state)
|
||
{
|
||
if (!s_initialized) return 0;
|
||
|
||
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 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 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);
|
||
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);
|
||
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];
|
||
}
|
||
}
|
||
|
||
/* 协方差膨胀 (无观测时的信任衰减) */
|
||
s_state.P[0][0] += s_cfg.q_ey * 5.0f;
|
||
s_state.P[1][1] += s_cfg.q_eth * 5.0f;
|
||
protect_P(s_state.P);
|
||
return 0;
|
||
}
|
||
|
||
/* 横向观测: 两侧平均 */
|
||
if (valid_sides == 2) {
|
||
z_ey /= 2.0f;
|
||
}
|
||
|
||
/* ----------------------------------------------------
|
||
* 构建观测向量 z 和预测观测 h(x)
|
||
* ---------------------------------------------------- */
|
||
float e_y = s_state.x[0];
|
||
float e_th = s_state.x[1];
|
||
|
||
/* 预测观测 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
|
||
|
||
/* 新息向量 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++;
|
||
}
|
||
|
||
/* 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++;
|
||
}
|
||
|
||
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
|
||
}
|
||
|
||
/* 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
|
||
}
|
||
}
|
||
}
|
||
|
||
/* 检验 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
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
/* ----------------------------------------------------
|
||
* 计算卡尔曼增益 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};
|
||
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];
|
||
}
|
||
}
|
||
}
|
||
|
||
for (int i = 0; i < 3; i++) {
|
||
for (int j = 0; j < 3; j++) {
|
||
s_state.P[i][j] = P_tmp[i][j];
|
||
}
|
||
}
|
||
|
||
symmetrize(s_state.P);
|
||
protect_P(s_state.P);
|
||
|
||
updated_obs_count = used_obs;
|
||
|
||
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_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);
|
||
|
||
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 = 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];
|
||
|
||
/* 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;
|
||
|
||
/* 协方差更新: 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];
|
||
}
|
||
}
|
||
|
||
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);
|
||
}
|