传感器观测数据自融合技术3——误差的不确定性传播理论

C方案中不确定性传播的挑战

问题核心：非线性映射的不确定性失真

在C方案中：

z1, z2 → f_nonlinear(z1, z2) → x_current → 状态递推融合

关键问题：非线性函数 f_nonlinear 会严重扭曲不确定性结构：

# 简单例子：距离-角度到笛卡尔坐标
def polar_to_cartesian(r, theta):
    x = r * np.cos(theta)
    y = r * np.sin(theta)
    return x, y

# 假设测量不确定性
r = 10 ± 0.1      # 高斯分布
theta = 30° ± 1°  # 高斯分布

# 经过非线性变换后：
x, y = polar_to_cartesian(r, theta)
# x, y 的分布不再是高斯分布！
# 不确定性椭圆被严重扭曲

具体的不确定性传播问题

1. 分布形状改变

   • 输入：高斯噪声
   • 输出：非高斯分布（经过非线性映射）
   • 您的状态递推模型可能假设高斯分布，导致模型失配

2. 相关性丢失

   • 原始测量 z1, z2 可能有已知的相关性
   • 经过非线性映射后，状态分量间的相关性难以追踪

3. 不确定性低估

   • 一阶近似（如EKF的雅可比矩阵）可能严重低估实际不确定性
   • 导致状态递推融合中给当前估计过高权重

改进的C方案：显式处理不确定性传播

方案C+：完整不确定性传播

def 改进的状态估计_带不确定性传播():
    # 第一步：必要观测预处理 + 不确定性建模
    z1, P_z1 = 去除异常值_带不确定性(z1_raw, R1)
    z2, P_z2 = 去除异常值_带不确定性(z2_raw, R2)
    
    # 第二步：非线性映射 + 正确的不确定性传播
    if 系统非线性较弱:
        # 使用一阶近似（EKF风格）
        x_current, P_current = ekf_style_mapping(z1, z2, P_z1, P_z2, f_nonlinear)
    else:
        # 使用无迹变换（UKF风格）保持高阶矩
        x_current, P_current = ukf_style_mapping(z1, z2, P_z1, P_z2, f_nonlinear)
    
    # 第三步：状态递推建模 + 不确定性感知融合
    # 基于历史状态序列及其不确定性
    x_pred, P_pred = 状态递推模型_带不确定性(历史状态序列, 历史不确定性)
    
    # 第四步：不确定性感知的融合
    x_fused, P_fused = 不确定性感知融合(x_pred, P_pred, x_current, P_current)
    
    return x_fused, P_fused  # 返回状态和完整不确定性

关键改进：不确定性传播方法

方法1：一阶泰勒展开（EKF风格）

def ekf_style_mapping(z1, z2, P_z1, P_z2, f_nonlinear):
    # 计算雅可比矩阵
    J = jacobian(f_nonlinear, [z1, z2])
    
    # 合并观测不确定性
    P_z = block_diag(P_z1, P_z2)
    
    # 不确定性传播
    x_current = f_nonlinear(z1, z2)
    P_current = J @ P_z @ J.T
    
    return x_current, P_current

方法2：无迹变换（UKF风格）- 推荐用于强非线性

def ukf_style_mapping(z1, z2, P_z1, P_z2, f_nonlinear):
    # 创建sigma点
    mean_z = np.concatenate([z1, z2])
    P_z = block_diag(P_z1, P_z2)
    
    # 通过非线性函数传播sigma点
    sigma_points = compute_sigma_points(mean_z, P_z)
    transformed_points = [f_nonlinear(*point) for point in sigma_points]
    
    # 计算变换后的均值和协方差
    x_current, P_current = unscented_transform(transformed_points)
    
    return x_current, P_current

方法3：蒙特卡洛采样 - 最准确但计算量大

def monte_carlo_mapping(z1, z2, P_z1, P_z2, f_nonlinear, n_samples=1000):
    # 从观测分布采样
    samples_z1 = np.random.multivariate_normal(z1, P_z1, n_samples)
    samples_z2 = np.random.multivariate_normal(z2, P_z2, n_samples)
    
    # 通过非线性映射传播
    samples_x = [f_nonlinear(z1_i, z2_i) for z1_i, z2_i in zip(samples_z1, samples_z2)]
    
    # 计算经验均值和协方差
    x_current = np.mean(samples_x, axis=0)
    P_current = np.cov(samples_x, rowvar=False)
    
    return x_current, P_current

状态递推建模的不确定性处理

您的状态递推模型也需要正确处理不确定性：

def 状态递推模型_带不确定性(历史状态序列, 历史不确定性):
    # 基于历史状态和不确定性建立预测模型
    # 例如：带不确定性的自回归模型
    
    # 状态预测
    x_pred = AR_model.predict(历史状态序列)
    
    # 不确定性预测 = 模型不确定性 + 过程噪声
    P_pred = AR_model.predict_uncertainty(历史不确定性) + Q_process
    
    return x_pred, P_pred

不确定性感知的融合

def 不确定性感知融合(x_pred, P_pred, x_current, P_current):
    # 标准卡尔曼增益融合
    S = P_pred + P_current
    K = P_pred @ np.linalg.inv(S)
    
    # 状态更新
    x_fused = x_pred + K @ (x_current - x_pred)
    
    # 不确定性更新（Joseph形式，数值稳定）
    I = np.eye(len(x_pred))
    P_fused = (I - K) @ P_pred @ (I - K).T + K @ P_current @ K.T
    
    return x_fused, P_fused

各方案的不确定性处理能力对比

维度	原C方案	C+方案（改进）
非线性误差传播	忽略	显式处理（EKF/UKF/MC）
分布形状保持	差	优（UKF/MC）
相关性保持	无	完整保持
计算复杂度	低	中-高
理论一致性	差	优

实际工程建议

对于计算资源有限的系统：

# 简化但合理的不确定性处理
def 简化不确定性传播(z1, z2, f_nonlinear):
    # 使用一阶近似但保守估计
    x_current = f_nonlinear(z1, z2)
    
    # 保守的不确定性估计：放大理论值
    J = jacobian(f_nonlinear, [z1, z2])  # 数值或自动微分
    P_conservative = 1.5 * (J @ block_diag(R1, R2) @ J.T)  # 1.5倍安全系数
    
    return x_current, P_conservative

关键检查点：

1. 一致性检查：使用NEES（归一化估计误差平方）检验滤波器一致性
2. 不确定性校准：确保估计的不确定性与实际误差匹配
3. 故障检测：基于不确定性设置创新检测门限

结论

感谢您指出这个关键问题！ 不考虑不确定性传播的C方案确实有问题。改进的C+方案通过：

1. 显式的不确定性传播（EKF/UKF/MC）
2. 完整的协方差跟踪
3. 不确定性感知的融合

能够正确处理多传感器非线性组合中的不确定性，使您的创新递推融合方法建立在坚实的理论基础上。

推荐使用C+方案中的无迹变换（UKF风格）方法，它在准确性和计算复杂度间提供了良好平衡，特别适合处理您描述的非线性传感器融合场景。