机器人未知测量噪声下的扩展卡尔曼滤波 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



