扩展卡尔曼滤波（EKF）是解决同时定位与地图绘制（SLAM）问题的经典方法。它把机器人位姿和地图特征放在同一个概率框架里，靠非线性系统的高斯近似来完成状态估计。问题在于，标准 EKF-SLAM 通常假设测量噪声的统计特性是已知的，比如协方差矩阵 R 固定不变。实际中这个假设经常不成立——传感器噪声会随环境变化，或者你根本拿不到准确的先验值。如果硬套一个错误的 R，估计就可能偏掉甚至发散。
处理未知测量噪声，大概有三类思路：在线估计 R、引入鲁棒机制、或者多模型并行。
在线更新噪声协方差
最直接的办法是拿新息（innovation）序列来推 R。新息就是实际观测和预测观测的差，理论上它应该只反映测量噪声。实际做法可以开一个滑动窗口，存最近 N 个新息向量，然后用它们的样本协方差减去状态预测不确定性的投影，得到对 R 的实时估计： [ \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 ] 这里 (H_k) 是观测矩阵，(P_{k|k-1}) 是预测协方差。这个估计直接放进卡尔曼增益公式里，替代原来的固定 R。窗口大小 N 是个需要权衡的参数——小了估计波动大，大了响应慢。
鲁棒代价函数
另一种思路是不去精确估计 R，而是让滤波器对异常测量不那么敏感。可以把标准卡尔曼滤波里的二次型代价换成鲁棒代价，比如 Huber 函数： [ \rho(\nu) = \begin{cases} \frac{1}{2}\nu^2 & |\nu| \leq c \ c|\nu| - \frac{1}{2}c^2 & |\nu| > c \end{cases} ] 这相当于给新息加了一个非线性权重，大偏差的测量会被压下。实现上要修改信息矩阵的更新，计算量会有增加，但能在噪声分布不清楚的时候稳住估计。
多模型滤波
如果噪声特性可能发生跳变，可以跑多个 EKF，每个假设一种不同的 R，然后用交互多模型（IMM）的框架来加权融合。哪个模型更匹配当前观测，它的权重就更高。这种方法重实时性，几个滤波器并行跑下来，对算力要求不低，适合噪声统计显著变化的场景。
实现起来，EKF 的流程还是那老几步。先按运动模型预测： [ \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) 是过程噪声协方差。然后进入观测更新，如果用了自适应 R 估计，先走一遍窗口更新的逻辑，再算增益： [ K_k = P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T + \hat{R}k)^{-1} ] 最后用实际观测修正： [ \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} ]
下面是一段 Python 实现，演示了用滑动窗口在线估计 R 的自适应 EKF-SLAM 一个迭代步。实际工程里 motion_model 和 observation_model 需要根据机器人模型补全。
import numpy as np
def adaptive_ekf_slam(x_prev, P_prev, u, z, Q, R_initial, window_size=10):
# 状态预测
x_pred = motion_model(x_prev, u)
F = jacobian_motion(x_prev, u)
P_pred = F @ P_prev @ F.T + Q
# 观测预测与新息计算
z_pred = observation_model(x_pred)
H = jacobian_observation(x_pred)
nu = z - z_pred
# 滑动窗口估计噪声协方差
if len(innovation_buffer) >= window_size:
innovation_buffer.pop(0)
innovation_buffer.append(nu)
R_adapted = np.cov(innovation_buffer, rowvar=False) - H @ P_pred @ H.T
# 卡尔曼增益与更新
S = H @ P_pred @ H.T + R_adapted
K = P_pred @ H.T @ np.linalg.inv(S)
x_new = x_pred + K @ nu
P_new = (np.eye(len(x_pred)) - K @ H) @ P_pred
return x_new, P_new, R_adapted
实际跑的时候要注意几个点。自适应估计的窗口大小得根据噪声变化的快慢来调，很多时候需要离线试几组参数。鲁棒方法虽然能抑制野值，但信息矩阵修正会引入额外计算，在嵌入式平台上可能要优化。多模型就更费资源了，如果场景里噪声就是慢漂，简单在线估计就够用。
仿真结果看图。算法可以在机器人跟踪特征点的同时，保持对位姿和地图的一致估计。
本实现参考 MATLAB 2019b 环境下的原程序，上述 Python 代码为等效转换。参考文献：
[1] 彭力，徐壮。带非线性约束的自适应高斯和卡尔曼滤波目标跟踪算法 [J].计算机测量与控制。2019
