Python卡尔曼滤波实现求助：滤波状态发散问题排查

核心错误分析 #

• 协方差计算逻辑错误：你每次循环都用当前单步误差的外积重新计算状态协方差P和噪声协方差R，但P是需要迭代更新的状态估计不确定性，R是基于整个数据集统计的固定噪声协方差，而非单步瞬时值。
• 矩阵运算误用：卡尔曼增益计算时用了逐元素乘法*和逐元素逆**-1，这完全不符合矩阵运算规则，必须用矩阵乘法@和矩阵逆np.linalg.inv()。
• 残差与更新逻辑错误：状态更新时的残差选择错误，且协方差更新公式冗余且偏离标准卡尔曼滤波逻辑，导致状态估计被错误放大。

解决方案：静态/无转移矩阵场景的融合方案 #

由于你没有状态转移矩阵，核心需求是融合测量（ms）和预测（ps）两个6维数据集，可采用静态卡尔曼融合实现，以下是修正后的方案：

步骤说明 #

1. 预先计算两个数据集的噪声协方差：用样本协方差反映数据的波动特性。
2. 初始化状态估计和协方差：用测量数据的均值和协方差作为初始值。
3. 迭代执行融合：将每个时刻的预测值作为先验估计，测量值作为观测，执行卡尔曼更新步。

修正代码 #

import numpy as np

# 数据参数
nn = 81036
ns = 6

# 导入数据（替换为实际文件路径）
ps = np.genfromtxt('predictions.csv', delimiter=',')
ms = np.genfromtxt('measurements.csv', delimiter=',')

# 预先计算噪声协方差（无偏样本协方差，rowvar=False表示每行是一个样本）
R = np.cov(ms, rowvar=False)  # 测量数据的噪声协方差
Q = np.cov(ps, rowvar=False)  # 预测数据的噪声协方差

# 初始化状态估计和协方差
xi = np.mean(ms, axis=0)  # 用测量均值初始化状态
P = R.copy()  # 初始协方差用测量数据的协方差

# 存储每步的误差协方差
error_covs = []

for i in range(nn):
    current_meas = ms[i]
    current_pred = ps[i]
    
    # 计算残差：测量值与预测值的偏差
    residual = current_meas - current_pred
    
    # 融合后的总协方差
    total_cov = Q + R
    
    # 计算卡尔曼增益（矩阵运算）
    K = Q @ np.linalg.inv(total_cov)
    
    # 更新状态估计
    xi = current_pred + K @ residual
    
    # 更新状态协方差（静态场景下的简化公式）
    P = (np.eye(ns) - K) @ Q
    
    # 记录当前误差协方差
    error_covs.append(P)

# 最终结果
final_state = xi
mean_error_cov = np.mean(error_covs, axis=0)

print("最终状态估计：", final_state)
print("平均误差协方差：", mean_error_cov)

可选：状态缓慢变化的场景 #

如果状态随时间有微小变化，但你没有转移矩阵，可以引入极小的过程噪声，模拟状态的缓慢漂移：

# 加入过程噪声的动态融合
process_noise = 1e-6 * np.eye(ns)  # 自定义过程噪声强度
P = R.copy()
xi = np.mean(ms, axis=0)
error_covs = []

for i in range(nn):
    current_meas = ms[i]
    
    # 预测步：无转移矩阵，状态保持不变，加入过程噪声
    pred_state = xi
    pred_cov = P + process_noise
    
    # 更新步
    residual = current_meas - pred_state
    total_cov = pred_cov + R
    K = pred_cov @ np.linalg.inv(total_cov)
    
    xi = pred_state + K @ residual
    P = (np.eye(ns) - K) @ pred_cov
    error_covs.append(P)

原代码发散原因 #

你每次循环重置P为单步误差的外积，且误用逐元素运算计算增益，导致增益矩阵完全偏离最优权重，状态更新被错误的残差持续放大，最终数值发散。

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