用 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 的例程，跑一遍发现结果也还挺好，但一换到自己的数据就完全不