用 filterpy 实现卡尔曼滤波：从原理到调参的工程实践指南

入行做数据融合和导航解算这些年，卡尔曼滤波我前前后后写了不下五遍——最早在C语言里一个矩阵一个矩阵手搓，后来在 MATLAB 里调现成工具箱，真正让我觉得“这套理论可以放下心直接交给工程”的，反而是 Python 生态里的 filterpy 这个库。它不是什么花哨的深度学习框架，就是一套干净利落的滤波器工具箱，把卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波都封装成了可以直接调用的类。你要是做传感器融合、目标跟踪、无人机姿态解算、自动驾驶状态估计，或者只是想在毕业设计里用最少的代码把卡尔曼滤波跑通，filterpy 都应该在你的工具清单里。

这篇文章我会直接从工程角度带你把它用起来。不会绕太多数学推导，但会讲明白每个矩阵是干什么的、每个参数应该怎么设、代码跑出来不对的时候怎么查。适合有一定 Python 基础、想快速把卡尔曼滤波落地到实际场景的读者。哪怕你之前只在课本上见过那五个公式，照着这篇文章的步骤走一遍，也能写出一个能用的滤波器。

1. 为什么是 filterpy：卡尔曼滤波在 Python 生态里的正确打开方式

1.1 卡尔曼滤波到底解决了什么问题

很多初学者第一次接触卡尔曼滤波，是被它的一大堆矩阵公式劝退的。其实抛开数学外壳，它干的事特别朴素：你有一个不太准的传感器，读数带噪声；你还有一个数学模型，能大致预测系统下一步会怎样，但模型本身也有误差。卡尔曼滤波就是要把这两个都不完美的信息来源融合起来，得到一个比单独任何一个都更准的估计。

举一个最简单的例子。你手里有一个温度传感器，测室温，每次读数都上下波动。如果你的数学模型说“温度基本不会突变”，那当传感器突然跳了 2 度时，更合理的做法不是完全相信它，而是把这次新信息按一定比例吸收进来。这个“比例”就是卡尔曼增益 K，它是根据你对模型有多信任、对传感器有多信任自动算出来的。模型噪声小、传感器噪声大，K 就小，滤波器偏向“平滑”；反过来 K 就大，滤波器偏向“快速跟随传感器”。

filterpy 的价值就是把这些矩阵运算、协方差递推、增益计算全部封装好，你只需要告诉它状态维度、观测维度、各个矩阵的值，然后循环调用 predict 和 update 就行。我不止一次在项目里看到有人要自己写卡尔曼滤波，写到最后矩阵维度对不上，出来的结果全是 NaN。真的没必要，工程场景下直接用成熟库，把精力花在调参和建模上，效率高得多。

1.2 为什么不自己手写：filterpy 与其他方案的取舍

有人会问，卡尔曼滤波公式也不长，自己用 numpy 写一遍也就几十行，为什么非要引入一个依赖？我的回答是：你自己写一遍，对理解原理确实有好处，但工程上要处理的细节远超那几个公式。比如矩阵维度校验、数值稳定性、Q 矩阵的离散化处理、R 矩阵的更新策略、残差和似然度的计算，这些 filterpy 都已经处理过了。

拿 Python 里的几个方案对比一下。scipy 没有现成的卡尔曼滤波接口，要写只能完全手写；pykalman 更适合批处理离线数据，实时在线滤波反而不顺手；filterpy 主打在线递推，接口设计非常贴近经典教材里的 predict/update 结构，而且内置了 Q_discrete_white_noise 这类生成标准过程噪声矩阵的工具函数，对工程场景非常友好。再加上这本书的作者就是《Kalman and Bayesian Filters in Python》的作者，文档和示例非常完整，遇到问题搜一下基本都有答案。

所以我的建议很直接：如果你不是为了学术研究非要从零实现，第一版系统直接用 filterpy，先把流程跑通、把数据留出来分析，之后真需要优化再考虑替换底层实现。

2. 动手前必须吃透的卡尔曼滤波基础

2.1 状态向量与状态转移矩阵：把物理规律写进方程

用 filterpy 之前，你首先要回答一个问题：系统状态是什么。状态向量可以是一维的标量，比如温度；也可以是一个多维数组，比如无人机的位置加速度。设计状态向量时要考虑两个约束：一是它要能完整描述系统当前情况，二是它要方便你写出状态转移矩阵 F，也就是“系统从上一时刻到这一时刻是怎么演化的”。

最常见的例子是匀加速或匀速运动模型。拿一个在直线上运动的目标来说，状态可以定义为位置 p 和速度 v，写成列向量 [p, v]^T。假设采样周期是 dt，那么下一时刻的位置等于当前位置加上速度乘以时间，下一时刻速度假设不变。把这个规律写成矩阵形式，F 就是 [[1, dt], [0, 1]]。filterpy 里只需要把 F 赋值给 kf.F，predict 的时候它就会自动完成这个外推。

这一步看起来简单，却是整个滤波器最容易出错的地方。很多人把 F 里的 dt 写成了固定值 1，结果采样频率一变，滤波出来的轨迹就对不上时间轴。我习惯把 dt 作为参数传进函数，每次滤波前动态更新 F。另外，如果系统里有控制量，比如车辆的加速度指令，你还需要定义 B 矩阵和 u 向量，filterpy 的 predict 方法支持 predict(u) 传入外部控制量，这个后面我会在例子里提到。

2.2 P、Q、R 三个矩阵：滤波器的性格由它们决定

很多新手拿到 filterpy 的例程，跑一遍发现结果也还挺好，但一换到自己的数据就完全不跟手，问题基本都出在 P、Q、R 三个矩阵的初始值上。

先说 P，也就是状态协方差矩阵的初始值。它表示你一开始对状态估计有多不确定。如果你完全不知道初始状态，可以把 P 的对角线设得大一些，比如 1000，这样滤波器在刚开始几个周期会快速把估计拉向真实状态。如果你对初始状态很有把握，P 就设小一点。注意 P 不能设成零矩阵，否则滤波器会觉得自己初值完全准确，后面传感器数据再多它也懒得修正。

R 是测量噪声协方差，它描述的是传感器读数有多可信。这个值最好从传感器标定或者实测数据里统计出来，比如对静止目标采集 100 组测量值，算一下方差。Q 是过程噪声协方差，它描述的是你对状态转移模型有多信任。模型不准确、目标有机动、车辆有颠簸，都需要通过增大 Q 来把“模型误差”吸收掉，否则滤波器会出现所谓“发散”，也就是估计结果越跑越偏。

Q、R 的绝对值大小不如它们之间的相对比例重要。实际调参你记住一句话：Q/R 越大，滤波器越相信测量、跟随越快但噪声也越大；Q/R 越小，滤波器越相信模型、输出越平滑但滞后越明显。这个比例就是滤波器的“性格开关”。

2.3 预测与更新：五个公式其实就干了两件事

教科书里卡尔曼滤波有五个公式，但如果给它们分组，其实就是两个阶段：预测和更新。预测阶段用状态转移矩阵 F 把当前状态外推一步，同时把协方差 P 也按不确定性的累积规律变大，代表“因为模型不完美，经过一步之后我更不确定了”。在 filterpy 里这就对应 kf.predict()。

更新阶段则是拿到新的观测 z 之后，算一下残差（测量值和预测值差多少），再计算卡尔曼增益 K，用 K 去决定把状态修正多少，同时把协方差 P 相应缩小。filterpy 里就对应 kf.update(z)。这两个方法交替调用，就是一个完整的在线滤波器循环。理解这个框架之后，你去看 filterpy 源码也会轻松很多，它内部无非就是把这几个公式翻译成了 numpy 代码。

需要提醒一下，predict 和 update 的调用顺序在实际场景里是有讲究的。如果测量和预测时刻是对齐的，那就先 predict 再 update。如果你在一个控制循环里，测量频率和预测频率不一样，那就要在预测之后直到有新的测量值才调用 update。这个时序问题很多人没注意，结果滤波结果相位不对，这锅还真不该 filterpy 背。

3. 实战：用 filterpy 实现一维与二维目标跟踪

3.1 环境准备：别急着写代码，先把依赖装明白

filterpy 的安装非常直接，pip install filterpy 一行命令就搞定。正常情况下它会自动把 numpy、scipy、matplotlib 这些依赖拉起来。如果装的时候遇到网络慢导致超时，可以指定 Python 版本后再试，比如先把 Python 升到 3.8 以上，因为 filterpy 对老版本的解释器支持并不好。

装好之后验证一下：

python复制from filterpy.kalman import KalmanFilter
kf = KalmanFilter(dim_x=2, dim_z=1)
print(kf)

如果能正常打印出对象信息，环境就没问题。我之前遇到过一种坑是机器上装了多个 Python 版本，pip 默认装到了旧版本解释器里，而 IDE 用的是新版本，结果 import filterpy 一直报 ModuleNotFoundError。这个问题的排查方法很机械，打开命令行执行 pip show filterpy 看安装路径，再在你的 Python 环境里执行 sys.executable 看当前解释器路径，两者对不上就说明装错环境了。

如果你是刚接触 Python 的读者，建议用 VSCode 配 Python 插件，新建一个虚拟环境再装依赖，这样不同项目之间的包不会互相打架。这个习惯虽然不复杂，但真的能帮你后续省下大量排障时间。

3.2 一维位置估计：从传感器读数里还原真实轨迹

我习惯拿一维位置估计当第一个完整 demo，因为它状态少，所有矩阵都是二维数组，新手也能一眼看明白。假设一个被测量的小车，你只知道它大概位置和速度，但传感器只提供位置观测，而且噪声比较大。

先定义一个二维状态 [位置, 速度]，dt 取 1 秒做单位简化：

python复制import numpy as np
from filterpy.kalman import KalmanFilter

kf = KalmanFilter(dim_x=2, dim_z=1)
kf.x = np.array([0., 0.])
kf.F = np.array([[1., 1.],
                 [0., 1.]])
kf.H = np.array([[1., 0.]])
kf.P = np.eye(2) * 1000
kf.R = np.array([[1.]])
kf.Q = np.array([[0.01, 0.],
                 [0., 0.01]])

这里 F 使用了 dt=1 的匀速模型，H 的意思是观测只取状态里的位置分量。R=1 表示测量噪声方差约为 1，Q 对角线上 0.01 表示模型误差比较小。注意 P 设了 1000，相当于告诉滤波器“我对初始位置和速度完全没底”，它会用前几步测量快速纠偏。

然后模拟一段测量数据并滤波：

python复制np.random.seed(42)
truth = np.linspace(0, 10, 100)
measurements = truth + np.random.randn(100)

positions = []
for z in measurements:
    kf.predict()
    kf.update(np.array([z]))
    positions.append(kf.x[0])

import matplotlib.pyplot as plt
plt.plot(truth, label='true')
plt.plot(measurements, 'o', alpha=0.4, label='measurement')
plt.plot(positions, label='filtered')
plt.legend()

跑完这个例子你会看到，滤波后的曲线明显比原始测量平滑，而且没有明显的滞后。这个 demo 虽小，但你已经走完了卡尔曼滤波的完整闭环：建模、初始化、预测、更新、结果分析。我建议你动手改一改 Q 和 R，观察曲线变化，这个经验比记住任何调参口诀都有用。

3.3 二维运动目标跟踪：一次完整的预测-更新循环

一维跑通之后，二维其实没有本质区别，就是状态向量从 2 维变成 4 维：x 方向位置、y 方向位置、x 方向速度、y 方向速度。观测仍然是二维位置，所以 H 矩阵变成一个 2x4 的矩阵。

模拟一个匀速直线运动的目标，采样周期 dt=0.1 秒，位置观测噪声标准差 0.5，代码如下：

python复制dt = 0.1
t = np.arange(0, 10, dt)
truth = []
measurements = []
px, py, vx, vy = 0., 0., 2., 1.
for _ in t:
    px += vx * dt
    py += vy * dt
    truth.append([px, py])
    measurements.append([px + np.random.randn() * 0.5,
                         py + np.random.randn() * 0.5])

对应的滤波器初始化和滤波循环：

python复制kf = KalmanFilter(dim_x=4, dim_z=2)
kf.x = np.array([0., 0., 2., 1.])
kf.F = np.array([[1, 0, dt, 0],
                 [0, 1, 0, dt],
                 [0, 0, 1, 0],
                 [0, 0, 0, 1]])
kf.H = np.array([[1, 0, 0, 0],
                 [0, 1, 0, 0]])
kf.P = np.eye(4) * 100
kf.R = np.eye(2) * 0.25
kf.Q = np.eye(4) * 0.01

results = []
for z in measurements:
    kf.predict()
    kf.update(np.array(z))
    results.append(kf.x[:2])

这里 Q 用 np.eye(4) * 0.01 其实是比较粗糙的写法，因为位置和速度的过程噪声强度应该不一样。更标准的做法是用 filterpy 自带的工具函数生成离散白噪声模型下的 Q 矩阵：

python复制from filterpy.common import Q_discrete_white_noise
q = Q_discrete_white_noise(dim=2, dt=dt, var=0.01)
kf.Q = np.block([[q, np.zeros((2, 2))],
                 [np.zeros((2, 2)), q]])

这个函数生成的是 2x2 的块矩阵，对应位置和速度维度的过程噪声。var 参数的含义是加速度噪声方差，取值越大代表目标机动越强。跑完之后画图，滤波轨迹基本能贴合真实轨迹，而原始测量点会明显散布在轨迹周围。这个例子里我把 Q 取得比较小，因为目标是匀速运动，模型准；如果你的目标会转弯、加减速，Q 就该调大，否则滤波器会固执地认为目标不可能转弯，结果越跟越偏。

注意：二维跟踪时最容易犯的错是把 R 矩阵写成对角元素相同的矩阵，也没问题，但如果两个方向传感器精度不一样，比如雷达测距精度高、测角精度低，就必须把 R 的两个对角线元素分开设置，否则滤波器会把精度高的方向也一起平滑过头。

4. 从标准卡尔曼到扩展卡尔曼：处理非线性系统

4.1 线性假设失效的场景：为什么直接套 KF 会翻车

标准卡尔曼滤波要求状态转移和观测方程都是线性的。但实际系统中大量场景不是线性的，最典型的就是雷达测距测角。一个目标在平面直角坐标系里运动，状态是 [x, y, vx, vy]，但传感器返回的是距离 r 和方位角 theta，这两个观测量和状态之间是平方根和反正切的关系，根本不是线性变换。

如果你硬把这种非线性观测近似成一个常数矩阵 H，滤波器的表现会很奇怪。因为卡尔曼增益是基于 H 算出来的，H 不准确，增益就不准，最终要么响应迟钝，要么直接发散。这时候就需要扩展卡尔曼滤波，也就是 EKF。

EKF 的核心思想是用泰勒展开把非线性函数在当前估计点附近线性化。对观测方程来说，就是求观测函数对状态的雅可比矩阵 H_jacobian，然后用这个随时间变化的矩阵替代标准卡尔曼滤波里固定的 H。filterpy 的 ExtendedKalmanFilter 类就是干这个的。

4.2 用 ExtendedKalmanFilter 实现距离-方位角跟踪

下面这个例子我经常用来给同事演示 EKF。假设雷达位于原点，目标是二维平面上的运动物体，状态为 [x, y, vx, vy]，观测是距离和方位角。先定义观测函数 hx 和雅可比矩阵 HJacobian：

python复制from filterpy.kalman import ExtendedKalmanFilter

def hx(x):
    px, py = x[0], x[1]
    r = np.sqrt(px**2 + py**2)
    theta = np.arctan2(py, px)
    return np.array([r, theta])

def HJacobian(x):
    px, py = x[0], x[1]
    r = np.sqrt(px**2 + py**2)
    H = np.zeros((2, 4))
    H[0, 0] = px / r
    H[0, 1] = py / r
    H[1, 0] = -py / (r**2)
    H[1, 1] = px / (r**2)
    return H

然后初始化滤波器，状态转移仍然用匀速模型的线性 F 矩阵：

python复制dt = 0.1
ekf = ExtendedKalmanFilter(dim_x=4, dim_z=2)
ekf.x = np.array([10., 0., 0., 5.])
ekf.F = np.array([[1, 0, dt, 0],
                  [0, 1, 0, dt],
                  [0, 0, 1, 0],
                  [0, 0, 0, 1]])
ekf.P = np.eye(4) * 100
ekf.R = np.diag([0.1, 0.01])
ekf.Q = np.eye(4) * 0.01

模拟生成一段真实轨迹和观测，然后循环滤波：

python复制truth = []
measurements = []
px, py, vx, vy = 10., 0., 0., 5.
sigma_r, sigma_theta = 0.1, 0.01

for _ in np.arange(0, 10, dt):
    px += vx * dt
    py += vy * dt
    truth.append([px, py])
    r_meas = np.sqrt(px**2 + py**2) + np.random.randn() * sigma_r
    theta_meas = np.arctan2(py, px) + np.random.randn() * sigma_theta
    measurements.append([r_meas, theta_meas])

results = []
for z in measurements:
    ekf.predict()
    ekf.update(np.array(z), HJacobian, hx, R=ekf.R)
    results.append(ekf.x[:2])

这里 update 时必须传入 HJacobian 和 hx 两个函数，R 也可以显式传，filterpy 内部会在当前状态处重新计算雅可比矩阵。运行完你会发现滤波轨迹基本能跟踪上真实轨迹。建议你做个小实验：把 hx 替换成线性近似，比如直接观测 [x, y]，但用相同的模拟数据，看看滤波精度差多少。做过这个对比之后，你对 EKF 为什么存在的理解会比看十遍公式都深刻。

提示：EKF 的雅可比矩阵推导容易出错，尤其是极坐标转到直角坐标这种组合。我自己有个习惯，先用 filterpy 自带的数值微分工具验证一下符号推导结果，确认无误再放到正式代码里。具体做法是拿当前状态的 x 微扰一下，看 HJacobian 和数值差分是否一致。这个步骤五分钟能搞定，但能省下后面大量调试时间。

5. 调参经验与常见问题排查实录

5.1 Q 和 R 的调参思路：先定 R 再调 Q

调卡尔曼滤波器的参数，我一直推荐“先定 R，再调 Q”的顺序。R 的物理意义很明确，就是传感器噪声方差，你可以离线采集一组静态数据直接算出来，这不算调参，算测量。R 定下来之后，剩下的工作就是调 Q。

Q 的初始值怎么给？我的经验是先给一个比较小的值，比如状态量的最小编码单位，然后观察滤波效果。如果输出噪声很大，说明滤波器太相信测量，要增大 Q；如果输出太平滑但滞后明显，说明滤波器太相信模型，要减小 Q。这个调节方向要记牢，不然容易越调越乱。

另外要提醒的是，Q 矩阵对角线上的每个元素是根据状态变量来设置的，不能一刀切。比如状态是 [位置, 速度] 时，位置的过程噪声通常远小于速度的，因为位置误差主要来自速度积分，直接给位置加一个与速度相同量级的噪声会破坏物理一致性。规范做法就是用 Q_discrete_white_noise 生成，它已经把位置、速度、加速度之间的耦合关系考虑进去了。

实际调参中还可以看 filterpy 的 likelihood 属性。每次 update 之后，kf.likelihood 是当前观测在该状态估计下的概率密度值。如果这个值长期异常低，说明测量值和模型预测差得离谱，通常就是某个矩阵设置失当。这个指标是免费的诊断工具，不要只看最终曲线，要学会看中间量。

5.2 常见报错与排查速查表

filterpy 整体比较成熟，但用的时候还是有几个高频坑，我整理成一张速查表，方便你在排查时对照。

其中矩阵维度问题占了我遇到的 filterpy 问题的六成以上。我的建议是，每次定义完 F、H 之后顺手打印它们的 shape，确认和 dim_x、dim_z 对得上，养成这个习惯能省很多时间。

还有一个隐蔽的坑是 dtype。如果你初始化 kf.x 或 kf.P 时用的是 np.array 纯整数，比如 np.array([0, 0])，后面很多矩阵运算会被迫把 dtype 提升为 float，但中间由于整数溢出或者除法精度问题，可能会导致诡异的结果。稳妥做法是全部写成浮点数，比如 np.array([0., 0.])，一劳永逸。

最后提一下数值稳定性。如果你的 P 矩阵在迭代过程中变得不是正定矩阵，滤波器会直接摆烂，输出 NaN。最典型的原因是 Q 设成了零矩阵，理论上系统模型是确定性的，但任何实际系统都有扰动，P 在预测阶段没有增加不确定性，更新阶段又不断缩小，几轮之后 P 就退化成了奇异矩阵。遇到这种情况，先给 Q 加上一个很小的对角值，比如 1e-6，然后再看结果。

6. 写在后面：filterpy 的定位与我的使用习惯

顺手整理几点我自己的使用心得，或许对你有参考价值。第一，filterpy 是“够用且好用”的库，但它不是万能药。如果你的系统有非常强的非线性，或者状态分布明显不是高斯分布，EKF 的线性化近似会失效，这时候你应该考虑 filterpy 里的无迹卡尔曼滤波（UKF）或者粒子滤波，而不是硬调 EKF。第二，滤波器的设计核心永远在建模和调参，filterpy 只是把公式实现的部分变成了填空题，真正见功夫的是你怎么填 F、Q、R 这几个空。

我个人的习惯是把滤波器封装成一个类，predict 和 update 放在独立的接口里，方便单元测试。初始化时把 Q、R、P 都做成可配置参数，配置文件里集中管理。这样换传感器、改采样周期、换目标模型，都不用改主业务流程。

最后再分享一个小技巧：调试滤波器的时候不要只盯着估计结果，把每一步的残差 y、卡尔曼增益 K、协方差 P 的迹都打出来看一眼。这几个量能直接告诉你滤波器是否健康。很多看起来玄学的问题，比如“为什么滤波后反而更抖了”，其实看一眼 K 的数值就能定位到是 R 太小还是 Q 太大。把 filterpy 当成一个透明的工具，而不是黑盒，调参的底气会完全不一样。