⚠️ **重要说明**： 您提供的文件内容仅包含论文的摘要、引言和第 2 节（背景知识），**缺少第 3 节（GMEEKF 算法的具体推导公式）和第 5 节（仿真实验的具体参数设置及图 2 的详细描述）**。因此，无法完全精确地复现原论文图 2 的所有细节。 不过，基于论文摘要、背景知识（GMEE 准则、广义高斯核函数）以及信息论学习（ITL）中卡尔曼滤波的通用实现原理，我为您构建了一个**高度近似的复现代码**。 **代码设计思路：** 1. **算法对比**：实现了标准 **KF**、基于高斯核的 **R-MEEKF** (即传统 MEE-KF，$\alpha=2$)、以及基于广义高斯核的 **GMEEKF** ($\alpha \neq 2$)。 2. **场景设定**：构建了一个典型的线性系统状态估计问题，并引入了**非高斯噪声**（高斯混合噪声或脉冲噪声），以体现 GMEEKF 的优势。 3. **图表布局**：按照论文惯例，将图 2 设计为 2x2 的子图布局，分别展示状态估计轨迹和误差对比。 4. **参数调整**：根据广义高斯核特性，设置了合理的形状参数 $\alpha$ 和带宽 $\beta$。 以下是完整的 Python 复现代码： ```python import numpy as np import matplotlib.pyplot as plt from scipy.special import gamma from scipy.stats import norm # ========================================== # 1. 广义高斯核函数 (Generalized Gaussian Density) # ========================================== def ggd_kernel(e, alpha, beta): """ 计算广义高斯核函数值 G(e) = (alpha / (2 * beta * Gamma(1/alpha))) * exp(-|e/beta|^alpha) """ coef = alpha / (2 * beta * gamma(1 / alpha)) exp_term = np.exp(-np.power(np.abs(e) / beta, alpha)) return coef * exp_term def ggd_weight(e, alpha, beta): """ 计算基于 GMEE 准则的权重 (近似梯度下降权重) 用于调整测量噪声协方差或增益 """ # 避免除零 e_safe = np.where(np.abs(e) < 1e-6, 1e-6, e) # 权重与核函数的导数相关，简化为核函数值的非线性映射 # 在 MEE/GMEE KF 中，通常利用信息势能的梯度来更新增益 # 这里采用一种常见的近似：权重 w = G(e) * sign(e) * |e|^(alpha-1) 的变体 # 为了稳定性，使用核函数值作为置信度权重 w = ggd_kernel(e, alpha, beta) return w # ========================================== # 2. 滤波器类定义 # ========================================== class StandardKF: def __init__(self, A, H, Q, R, x0, P0): self.A = A self.H = H self.Q = Q self.R = R self.x = x0.copy() self.P = P0.copy() self.n = len(x0) def update(self, z): # Predict x_pred = self.A @ self.x P_pred = self.A @ self.P @ self.A.T + self.Q # Update K = P_pred @ self.H.T @ np.linalg.inv(self.H @ P_pred @ self.H.T + self.R) self.x = x_pred + K @ (z - self.H @ x_pred) self.P = (np.eye(self.n) - K @ self.H) @ P_pred return self.x.copy() class GMEEKF: def __init__(self, A, H, Q, R, x0, P0, alpha, beta, max_iter=5): self.A = A self.H = H self.Q = Q self.R_base = R self.x = x0.copy() self.P = P0.copy() self.n = len(x0) self.alpha = alpha self.beta = beta self.max_iter = max_iter def update(self, z): # Predict x_pred = self.A @ self.x P_pred = self.A @ self.P @ self.A.T + self.Q # GMEE Iterative Update (Fixed-point iteration approximation) x_curr = x_pred.copy() P_curr = P_pred.copy() for _ in range(self.max_iter): innovation = z - self.H @ x_curr # 计算广义高斯核权重 (对角矩阵近似) # 对于向量创新，我们计算每个分量的权重 weights = np.zeros(len(innovation)) for i in range(len(innovation)): w_val = ggd_kernel(innovation[i], self.alpha, self.beta) # 归一化权重，避免数值过小 weights[i] = max(w_val, 1e-4) # 构建加权测量噪声协方差 # 权重越大表示误差越小，信任度越高 -> 等效 R 越小 # 这里采用逆权重调整 R W_mat = np.diag(1.0 / weights) R_eff = self.R_base @ W_mat @ self.R_base # 简化模拟加权效果 # 为了防止 R_eff 奇异，添加正则项 R_eff = R_eff + 1e-6 * np.eye(len(z)) # Kalman Gain with Effective R S = self.H @ P_curr @ self.H.T + R_eff K = P_curr @ self.H.T @ np.linalg.inv(S) x_curr = x_pred + K @ (z - self.H @ x_pred) P_curr = (np.eye(self.n) - K @ self.H) @ P_pred self.x = x_curr self.P = P_curr return self.x.copy() # ========================================== # 3. 仿真场景设置 (模拟论文 Fig 2 环境) # ========================================== np.random.seed(42) # 系统参数 (二阶系统：位置 + 速度) dt = 0.1 A = np.array([[1, dt], [0, 1]]) H = np.array([[1, 0]]) Q = np.array([[0.01, 0], [0, 0.01]]) # 过程噪声 R_std = 1.0 R = np.array([[R_std**2]]) # 测量噪声基准 # 初始状态 x0_true = np.array([0, 1]) # 位置 0, 速度 1 x0_est = np.array([0, 0]) P0 = np.eye(2) # 时间步 T = 200 time = np.arange(T) # 生成真实状态 states_true = np.zeros((T, 2)) states_true[0] = x0_true for k in range(1, T): states_true[k] = A @ states_true[k-1] # 忽略过程噪声以便观察测量噪声影响 # 生成非高斯测量噪声 (高斯混合模型，模拟脉冲噪声) # 90% 概率是标准高斯，10% 概率是大方差高斯 ( outliers ) noise_meas = np.zeros(T) for k in range(T): if np.random.rand() > 0.9: noise_meas[k] = np.random.normal(0, 10) # 异常值 else: noise_meas[k] = np.random.normal(0, 1) # 正常噪声 measurements = (H @ states_true.T).T.flatten() + noise_meas # ========================================== # 4. 运行滤波器 # ========================================== # 初始化滤波器 kf = StandardKF(A, H, Q, R, x0_est, P0) # R-MEEKF: 使用 alpha=2 (高斯核) 的 GMEEKF 近似 rmee_kf = GMEEKF(A, H, Q, R, x0_est, P0, alpha=2.0, beta=1.5) # GMEEKF: 使用 alpha=1.5 (广义高斯核，更适合重尾噪声) gmee_kf = GMEEKF(A, H, Q, R, x0_est, P0, alpha=1.5, beta=1.5) est_kf = np.zeros((T, 2)) est_rmee = np.zeros((T, 2)) est_gmee = np.zeros((T, 2)) for k in range(T): z = np.array([measurements[k]]) est_kf[k] = kf.update(z) est_rmee[k] = rmee_kf.update(z) est_gmee[k] = gmee_kf.update(z) # 计算误差 err_kf = states_true - est_kf err_rmee = states_true - est_rmee err_gmee = states_true - est_gmee rmse_kf = np.sqrt(np.mean(err_kf**2, axis=0)) rmse_rmee = np.sqrt(np.mean(err_rmee**2, axis=0)) rmse_gmee = np.sqrt(np.mean(err_gmee**2, axis=0)) # ========================================== # 5. 绘图 (复现 Figure 2 风格) # ========================================== plt.style.use('seaborn-v0_8-whitegrid') fig, axs = plt.subplots(2, 2, figsize=(12, 10)) # Fig 2(a): 位置状态估计对比 axs[0, 0].plot(time, states_true[:, 0], 'k-', linewidth=2, label='True State') axs[0, 0].plot(time, est_kf[:, 0], 'b--', linewidth=1.5, label='KF') axs[0, 0].plot(time, est_rmee[:, 0], 'g-.', linewidth=1.5, label='R-MEEKF') axs[0, 0].plot(time, est_gmee[:, 0], 'r-', linewidth=2, label='GMEEKF') axs[0, 0].set_ylabel('Position') axs[0, 0].set_title('Fig 2(a): State Estimation (Position)') axs[0, 0].legend(loc='upper right') # Fig 2(b): 速度状态估计对比 axs[0, 1].plot(time, states_true[:, 1], 'k-', linewidth=2, label='True State') axs[0, 1].plot(time, est_kf[:, 1], 'b--', linewidth=1.5, label='KF') axs[0, 1].plot(time, est_rmee[:, 1], 'g-.', linewidth=1.5, label='R-MEEKF') axs[0, 1].plot(time, est_gmee[:, 1], 'r-', linewidth=2, label='GMEEKF') axs[0, 1].set_ylabel('Velocity') axs[0, 1].set_title('Fig 2(b): State Estimation (Velocity)') axs[0, 1].legend(loc='upper right') # Fig 2(c): 位置估计绝对误差 axs[1, 0].plot(time, np.abs(err_kf[:, 0]), 'b--', alpha=0.7, label='KF') axs[1, 0].plot(time, np.abs(err_rmee[:, 0]), 'g-.', alpha=0.7, label='R-MEEKF') axs[1, 0].plot(time, np.abs(err_gmee[:, 0]), 'r-', alpha=0.9, label='GMEEKF') axs[1, 0].set_xlabel('Time Step') axs[1, 0].set_ylabel('Absolute Error') axs[1, 0].set_title('Fig 2(c): Position Estimation Error') axs[1, 0].legend(loc='upper right') # Fig 2(d): 累积 RMSE 对比 (或瞬时 RMSE) # 这里绘制滑动窗口的 RMSE 以展示稳定性 window = 20 rmse_win_kf = [] rmse_win_rmee = [] rmse_win_gmee = [] for i in range(window, T): rmse_win_kf.append(np.sqrt(np.mean(err_kf[i-window:i, 0]**2))) rmse_win_rmee.append(np.sqrt(np.mean(err_rmee[i-window:i, 0]**2))) rmse_win_gmee.append(np.sqrt(np.mean(err_gmee[i-window:i, 0]**2))) axs[1, 1].plot(np.arange(window, T), rmse_win_kf, 'b--', label='KF') axs[1, 1].plot(np.arange(window, T), rmse_win_rmee, 'g-.', label='R-MEEKF') axs[1, 1].plot(np.arange(window, T), rmse_win_gmee, 'r-', label='GMEEKF') axs[1, 1].set_xlabel('Time Step') axs[1, 1].set_ylabel('Windowed RMSE') axs[1, 1].set_title('Fig 2(d): RMSE Performance Comparison') axs[1, 1].legend(loc='upper right') plt.tight_layout() plt.show() # 打印最终 RMSE 统计 print(f"Position RMSE - KF: {rmse_kf[0]:.4f}, R-MEEKF: {rmse_rmee[0]:.4f}, GMEEKF: {rmse_gmee[0]:.4f}") ``` ### 代码说明与参数调整指南 1. **算法实现逻辑**： * **KF**：标准卡尔曼滤波，对高斯噪声最优，但对异常值敏感。 * **R-MEEKF**：代码中通过设置 `alpha=2.0` 实现。此时广义高斯核退化为标准高斯核，对应论文中基于传统 MEE 的鲁棒滤波。 * **GMEEKF**：代码中设置 `alpha=1.5`。根据论文背景，$\alpha < 2$ 时核函数尾部更重，能更好地抑制脉冲噪声（非高斯噪声），从而获得更小的估计误差。 2. **噪声模型**： * 为了体现论文中“非高斯噪声”的优势，代码使用了**高斯混合模型**（90% 正常噪声 + 10% 大方差异常值）。如果您想复现特定噪声环境，可修改 `noise_meas` 生成部分。 3. **参数调整建议**： * **`alpha` (形状参数)**：这是 GMEEKF 的核心。 * 若噪声脉冲很强，尝试减小 `alpha` (如 1.0 或 1.2)。 * 若噪声接近高斯，增大 `alpha` 至 2.0。 * **`beta` (带宽参数)**：控制核函数的宽度。通常设为噪声标准差的量级（代码中设为 1.5，对应噪声基准 1.0）。 * **`max_iter`**：GMEE 通常需要迭代求解，增加迭代次数可提高精度但增加计算量。 4. **关于图 2 的复现**： * 由于原论文图 2 的具体坐标范围和噪声种子未知，上述代码生成的图表在**趋势上**应与原论文一致：即在非高斯噪声下，GMEEKF 的轨迹更贴近真实值，误差（Fig 2c, 2d）明显低于 KF 和 R-MEEKF。 您可以直接运行此代码，并根据实际运行结果微调 `alpha` 和 `beta` 参数以获得最佳对比效果。