机器人未知测量噪声下的扩展卡尔曼滤波 SLAM 实现

在机器人同时定位与地图绘制（SLAM）任务中，扩展卡尔曼滤波（EKF）是处理非线性系统的经典方案。然而，实际应用中测量噪声的统计特性往往难以预先精确获取。若噪声协方差设定不当，极易导致状态估计偏差甚至滤波器发散。本文将探讨如何在未知测量噪声环境下，通过自适应机制增强 EKF-SLAM 的鲁棒性。

核心挑战与方法

传统 EKF 假设过程噪声和测量噪声服从已知的高斯分布。当测量噪声 $R$ 未知时，我们需要引入在线估计策略：

噪声协方差在线估计 利用新息序列（Innovation Sequence）的统计特性来反推当前的噪声水平。通过滑动窗口或指数加权平均更新 $\hat{R}_k$，公式如下：

$$ \hat{R}k = \frac{1}{N}\sum{i=k-N+1}^k \nu_i \nu_i^T - H_k P_{k|k-1} H_k^T $$

其中 $\nu_i$ 为新息，$H_k$ 为观测矩阵，$P_{k|k-1}$ 为预测协方差。
鲁棒滤波设计 针对异常值干扰，可引入 Huber 或 Tukey 等鲁棒代价函数，降低大误差新息的权重，防止单次异常观测破坏整体轨迹。
多模型融合 在噪声统计特性剧烈变化的场景下，可采用交互多模型（IMM）框架，并行运行多个不同噪声假设的滤波器，动态加权融合输出。

算法实现步骤

完整的自适应 EKF 流程包含状态预测、观测更新与状态修正三个环节。在实际编码时，需注意雅可比矩阵的计算精度以及数值稳定性。

1. 状态预测

根据运动模型预测下一时刻的位姿及地图特征：

$$ \hat{x}{k|k-1} = f(x{k-1}, u_k) $$ $$ P_{k|k-1} = F_k P_{k-1} F_k^T + Q_k $$

这里 $F_k$ 是状态转移雅可比矩阵，$Q_k$ 为过程噪声协方差。

2. 观测更新与噪声自适应

若噪声协方差未知，先基于历史新息估计 $\hat{R}_k$，再计算卡尔曼增益：

$$ K_k = P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T + \hat{R}_k)^{-1} $$

3. 状态修正

结合实际观测值 $z_k$ 修正状态估计：

$$ \hat{x}k = \hat{x}{k|k-1} + K_k (z_k - h(\hat{x}{k|k-1})) $$ $$ P_k = (I - K_k H_k) P{k|k-1} $$

MATLAB 代码示例

下面给出一个简化的自适应 EKF 核心逻辑实现。相比固定噪声，这里增加了基于滑动窗口的 $R$ 矩阵在线更新逻辑。

function [x_new, P_new, R_adapted] = adaptive_ekf_slam(x_prev, P_prev, u, z, Q, R_initial, window_size)
    % 状态预测
    x_pred = motion_model(x_prev, u);
    F = jacobian_motion(x_prev, u);
    P_pred = F * P_prev * F' + Q;
    
    % 观测预测与新息计算
    z_pred = observation_model(x_pred);
    H = jacobian_observation(x_pred);
    nu = z - z_pred;
    
    % 滑动窗口估计噪声协方差
    persistent innovation_buffer;
    if isempty(innovation_buffer)
        innovation_buffer = zeros(size(nu, 1), window_size);
    end
    
    innovation_buffer = [innovation_buffer(:, 2:end), nu'];
    if size(innovation_buffer, 2) >= window_size
        R_adapted = cov(innovation_buffer') - H * P_pred * H';
        % 确保正定性
        R_adapted = max(R_adapted, eye(size(R_adapted)) * 1e-6);
    else
        R_adapted = R_initial;
    end
    
    % 卡尔曼增益与更新
    S = H * P_pred * H' + R_adapted;
    K = P_pred * H' / S;
    x_new = x_pred + K * nu;
    P_new = (eye(length(x_pred)) - K * H) * P_pred;
end

机器人未知测量噪声下的扩展卡尔曼滤波 SLAM 实现