NLOS场景下带加权非高斯噪声的卡尔曼滤波实现方法咨询（DecaWave+IMU）

首先明确：你之前把NLOS下的测量协方差矩阵R设为0的操作完全错误。R代表测量噪声的协方差，设为0相当于告诉滤波算法“这次测量完全没有误差，绝对可靠”，这和NLOS下测量数据极不可靠的实际情况完全相反，反而会让滤波过度信任错误的测量值，导致定位状态严重发散。

下面是针对你的场景的正确实现方案：

1. 基于QF参数动态调整测量协方差R #

利用DecaWave的质量因子（QF）作为环境状态的直接依据，动态调整R的大小：

• LOS环境：使用DecaWave标称的高斯误差对应的R值（比如根据手册给出的测距误差σ，设置R = σ²，如果是多维度测量则对应对角矩阵）。
• NLOS环境：根据QF的大小成比例放大R——QF越小，说明测量可靠性越低，R的放大倍数越高。比如可以定义一个映射函数：

  scale_factor = max(10, 100 / QF_current)  # 确保最小放大10倍，QF为10时放大10倍，QF为5时放大20倍
  R = R_LOS * scale_factor
  

  当QF低于设定的阈值（比如20）时，直接使用预设的最大R_NLOS（比如100 * R_LOS），让卡尔曼增益K变得极小，从而大幅降低不可靠测量对状态更新的影响。

2. 引入鲁棒卡尔曼滤波变种适配非高斯噪声 #

标准卡尔曼滤波假设噪声为高斯分布，NLOS下的非高斯异常值会严重影响滤波效果，结合QF参数切换到鲁棒滤波模式：

• Huber滤波：修改代价函数，当残差（创新值）在阈值内时用高斯代价，超过阈值时用线性代价，减少异常值的权重。可以用QF调整阈值：QF越低，阈值越严格，对残差的惩罚越重。
• 自适应卡尔曼滤波：实时根据残差的统计特性调整R，同时结合QF作为先验信息——当QF较低时，强制增大R的初始调整幅度，避免滤波被异常值带偏。

3. 极端NLOS场景的测量值加权/跳过策略 #

当QF极低（比如接近0）时，完全可以跳过本次测量更新步骤，仅依靠IMU进行状态预测：

• 检测到QF低于极端阈值（比如5）时，不执行卡尔曼滤波的测量更新环节，直接使用IMU的预测结果作为当前状态。
• 若不想完全丢弃测量，可以给测量值设置极低的权重，比如在计算卡尔曼增益时乘以一个基于QF的权重系数（weight = QF_current / QF_max），进一步削弱不可靠测量的影响。

伪代码示例 #

# 初始化参数
R_LOS = np.diag([0.01, 0.01])  # LOS下的测距协方差矩阵，根据实际误差调整
QF_THRESH = 20
QF_EXTREME_THRESH = 5

# 获取当前DecaWave数据和QF
z = decawave.get_measurement()
QF_current = decawave.get_quality_factor()

# 状态预测（IMU部分）
x_pred = imu.predict(x_prev, dt)
P_pred = F * P_prev * F.T + Q  # F是状态转移矩阵，Q是IMU过程噪声协方差

# 根据QF调整测量协方差和更新策略
if QF_current >= QF_THRESH:
    # LOS环境，正常卡尔曼更新
    R = R_LOS
    innovation = z - H @ x_pred
    innovation_cov = H @ P_pred @ H.T + R
    K = P_pred @ H.T @ np.linalg.inv(innovation_cov)
    x = x_pred + K @ innovation
    P = (np.eye(len(x)) - K @ H) @ P_pred
elif QF_current >= QF_EXTREME_THRESH:
    # 轻度NLOS，动态放大R+鲁棒更新
    scale_factor = max(10, 100 / QF_current)
    R = R_LOS * scale_factor
    innovation = z - H @ x_pred
    innovation_cov = H @ P_pred @ H.T + R
    # Huber阈值调整
    huber_thresh = 1 * np.sqrt(np.trace(innovation_cov))
    if np.linalg.norm(innovation) <= huber_thresh:
        K = P_pred @ H.T @ np.linalg.inv(innovation_cov)
    else:
        weight = huber_thresh / np.linalg.norm(innovation)
        K = P_pred @ H.T @ np.linalg.inv(innovation_cov) * weight
    x = x_pred + K @ innovation
    P = (np.eye(len(x)) - K @ H) @ P_pred
else:
    # 极端NLOS，仅用IMU预测
    x = x_pred
    P = P_pred

# 更新状态
x_prev = x
P_prev = P

内容的提问来源于stack exchange，提问作者greg kuhn