在 Python 中运行卡尔曼滤波器：噪声、缺失数据和异常值

Contents — find the section you need

Change parameters and verify

Open the panel, then press Run to load Python. You can stop execution and reset parameters. Results are computed on this device. No Python installation is required.

Local execution steps below are optional for reproducing the source results; they are not required for the browser experiment.

The experiment controls are in English.

Open experiment panel in a new tab

Download reproduction source

卡尔曼滤波调谐用于表达运动模型及其观测结果中的不确定性。本实验首先估计位置和速度，然后比较缺失数据时的预测结果、对异常值的反应以及因过度自信而导致的拒绝。所有输入均为合成数据；这些数值并非硬件精度声明。

模型和单位

状态包含位置 [m] 和速度 [m/s]。仅观测位置，每 0.1 秒观测一次：

x_k=F x_{k-1}+w_k,\qquad F=\begin{bmatrix}1&0.1\\0&1\end{bmatrix},\qquad z_k=H x_k+v_k,\quad H=\begin{bmatrix}1&0\end{bmatrix}

观测标准差为 0.7 m，因此 R=0.49 m²。过程协方差为 q ggᵀ，其中 g=(dt²/2, dt)，表示每步独立的加速度扰动。此处 q=0.4 的单位为 (m/s²)²；它不是连续白噪声谱密度。未经确认其含义，请勿将此值应用于其他离散化方法。预测和更新的推导过程请参见卡尔曼滤波器简介。

理论部分请参见 Simo Särkkä (2013) 的贝叶斯滤波与平滑，第 4.3 节。此处的轨迹、门控和比较条件是我们自行设计的实验。

四种配置，输入相同

将实验脚本保存到空的输出目录并运行：

python3 engineering_labs.py

已使用 Python 3.12.3、NumPy 1.26.4 和 Matplotlib 3.6.3 进行执行测试。该文件还会生成其他相关的工程实验。它使用种子 42 和 300 个样本。真实速度在 12 秒时从 1 米/秒变化到 1.4 米/秒。 7.0 秒至 9.9 秒之间缺失 30 个观测值，16 秒时的观测值增加了 12 米。初始位置/速度分别为 0 米和 1 米/秒；初始协方差的对角线元素分别为 1 平方米和 1 (米/秒)²。每个配置接收到的观测值完全相同。

残差归一化

令 e 为校正前的观测残差，S 为其预测方差。当满足以下条件时，门会拒绝观测值：

e=z-H\hat{x}^{-},\qquad S=HP^{-}H^T+R,\qquad e^2/S>9

此阈值适用于标量归一化残差，而非任意应用于多维观测值。缺失或被拒绝的测量值会触发不进行校正的预测。该实现使用 Joseph 协方差更新并检查正半定性。由于数据缺失，不确定性会增加，因此这与以米为单位的固定拒绝距离不同。

结果揭示了过度自信

配置	位置 RMSE [米]	拒绝
q=0.4，R=0.49，无门控	0.262	0
q=0.4，R=0.49，门控	0.178	2
q=0.002，R=0.49，门控	0.393	4
q=0.4，R=0.0049，门控	11.251	237

为便于比较，垂直范围限制在 -3 至 4 米之间。12 米的异常值以及 R 值较小时的较大误差超出此范围。检查门控 CSV、小 R CSV 和完整汇总中未裁剪的值。CSV 中缺失的观测值是 NaN，不计入拒绝次数。

门控拒绝了两个观测值，尽管只注入了一个人为异常值。阈值无法仅识别故意篡改的样本。将观测方差低估 100 倍会导致 237 个拒绝：正常的离散度与滤波器的置信度不符。丢失的校正会阻止对速度变化的跟踪，使残差更大。单独添加门控并不能提高鲁棒性。

移至您自己的日志

首先检查时间戳、单位和帧。估计平稳时间间隔内的观测方差，然后检查运动期间的运动模型误差和残差。最后引入门控，并报告缺失数据率、拒绝率和不确定性以及均方根误差 (RMSE)。实际硬件需要独立的参考值来计算可比较的误差。

较小的 Q 值可以使曲线更平滑，但会增加延迟。增加 Q 值并非总是更好。这些排名仅适用于此轨迹、初始化、噪声和评估窗口。

后续检查

sigma_m 列是估计位置的标准差。绘制估计值加/减两倍 sigma 的曲线，并将缺失数据区间与速度变化进行比较。这可以将实际误差与估计器的自我评估区分开来。在更改协方差之前，请继续使用sensor-fusion debugging来诊断时序和时间函数 (TF)。

在本地运行相同的计算

解压完整的可运行源代码 ZIP，或将实验入口点、生成的内核和原始 PID 源代码保存在一个文件夹中。PID 使用 Python 标准库；Kalman 和 ICP 需要 NumPy。原始的图形生成命令仍然可用。

python3 -m pip install numpy==1.26.4
python3 experiment.py kalman --parameters '{"q":0.4,"r":0.49,"gate":false}' > result.json

切换门控以比较异常值响应，然后将 q 设置为 0.002 或 r 设置为 0.0049，并比较 RMSE 和拒绝计数。观测值在每次运行中均固定，取自保存的 CSV 文件。

Code, data and reproduction

The ZIP contains the same computation code as the browser, synthetic data and comparison cases. These are not physical measurements.

Download reproduction package · Instructions (EN/JA) · Files and experiment conditions (JSON)

Extract the ZIP into a new directory, then run:

python3 reproduce.py --experiment kalman

This Lab compares 4 cases. PID uses Python’s standard library; Kalman/ICP require NumPy. See the instructions for dependencies and tested versions.