1. 为什么我要从零手写线性卡尔曼滤波而不是直接调库很多人第一次接触卡尔曼滤波是在做传感器数据融合或者目标跟踪的时候。打开搜索引擎一搜满屏都是矩阵公式和协方差推导看得人头大。更常见的做法是直接找一个现成的库把参数往里一塞跑通就行。我早期也是这么干的直到有一次一个简单的温度采集项目出了岔子——滤波后的曲线在真实值附近来回振荡调了半天参数也没搞明白问题出在哪。那次之后我下定决心把线性卡尔曼滤波从公式到代码彻底啃了一遍。这篇内容就是那次啃完之后整理的实战笔记。核心是用Python把线性卡尔曼滤波Linear Kalman Filter简称LKF从数学公式一步步翻译成可运行的代码并且用模拟数据验证它的行为。适合两类人看一类是已经会用现成滤波库、但想搞清楚内部到底在算什么的人另一类是刚学完概率论和线性代数、想把理论落到代码上的学生或者转行者。整篇不依赖任何高级滤波库只用NumPy所有矩阵运算都自己写保证你能看清每一步在干什么。线性卡尔曼滤波解决的是一个很具体的问题在一个线性系统里状态随时间演化同时我们通过带噪声的观测去估计这个状态。它的最优性建立在两个假设上——系统是线性的噪声是高斯的。这两个假设一旦成立卡尔曼滤波就是在最小均方误差意义下的最优估计器。理解这一点很关键因为它决定了你什么时候该用它、什么时候该换扩展卡尔曼或者粒子滤波。我下面会先讲清楚状态空间模型怎么建再讲预测和更新两步的矩阵推导然后给出完整可运行的Python实现最后用一组模拟数据做验证并分享几个我踩过的坑。整个过程你会看到卡尔曼滤波的核心其实就五个公式剩下的都是工程细节。2. 把物理问题翻译成状态空间模型2.1 状态向量到底该放什么卡尔曼滤波的第一步永远不是写代码而是确定状态向量。状态向量就是你想要估计的那些量的集合。举个最经典的例子一个物体在一维直线上运动我们想估计它的位置和速度。那么状态向量就是x [position, velocity]^T为什么速度也要放进去因为位置的变化率就是速度如果我们只估计位置那系统模型就没法描述位置怎么随时间变化。把速度纳入状态系统就变成了一个二阶系统可以用简单的运动学关系描述。这里有个经验状态向量里放什么取决于你的系统模型能不能把这些量之间的关系写清楚。如果你放了一个量进去但它的演化规律你完全不知道那放进去只会增加不确定性。我见过有人在状态里塞了七八个量结果观测根本覆盖不到滤波器直接发散。状态向量的维度记作 n。上面这个例子里 n2。2.2 状态转移矩阵F的构造逻辑状态转移矩阵 F 描述的是在没有观测的情况下状态从上一时刻到这一时刻怎么变。对于匀速直线运动模型position_new position_old velocity_old * dt velocity_new velocity_old写成矩阵形式F [[1, dt], [0, 1]]dt 是采样时间间隔。这个矩阵的物理含义很直白位置按速度累积速度保持不变。这就是所谓的“匀速模型”Constant Velocity Model。如果你做的是更复杂的系统比如带加速度的那状态里要加加速度F 也要相应扩展。但注意每加一个状态量计算量是平方级增长的因为协方差矩阵是 n×n 的。所以状态向量不是越大越好够用就行。2.3 观测矩阵H从状态到观测的映射观测矩阵 H 描述的是我们实际测量到的量和状态向量之间是什么关系。还是上面那个例子假设我们只有一个传感器能测位置测不到速度。那么z position [1, 0] * [position, velocity]^T所以 H [1, 0]维度是 1×2。观测维度记作 m这里 m1。如果传感器同时能测位置和速度那 H 就是 2×2 的单位矩阵。H 的构造完全取决于你的传感器配置这一点在写代码之前必须想清楚。我踩过的一个坑就是H 写错了把测不到的量也映射进去了结果滤波器“以为”自己观测到了速度实际上没有导致估计严重偏离。2.4 过程噪声Q和观测噪声R的物理意义这两个矩阵是卡尔曼滤波里最需要“调”的部分也是最容易出问题的地方。过程噪声 Q 描述的是你的状态转移模型有多不靠谱。比如匀速模型假设速度不变但实际物体可能会加速、减速这个模型误差就体现在 Q 里。Q 越大滤波器越不相信模型预测越依赖观测。观测噪声 R 描述的是你的传感器有多不准。R 越大滤波器越不相信观测越依赖模型预测。这两个矩阵的比值 Q/R 决定了滤波器的“性格”。Q/R 大滤波器反应快但噪声大Q/R 小滤波器平滑但滞后。我后面会专门讲怎么调这两个参数。对于上面的匀速模型Q 的常见构造是Q [[dt^4/4, dt^3/2], [dt^3/2, dt^2]] * sigma_a^2sigma_a^2 是加速度的方差代表模型的不确定程度。这个公式的推导涉及连续时间系统的离散化这里不展开你只需要知道它是从“加速度是白噪声”这个假设推出来的。R 通常是对角矩阵对角线上的值就是各个传感器测量噪声的方差。如果你不知道具体值可以先估一个后面再调。3. 预测与更新五个公式的矩阵推导3.1 预测步用模型往前推预测步做两件事把状态往前推一步把不确定性也往前推一步。状态预测x_pred F x_prev这很直观就是用状态转移矩阵把上一时刻的估计值推到当前时刻。协方差预测P_pred F P_prev F.T Q这个公式是卡尔曼滤波的精髓之一。P 是状态估计的协方差矩阵代表我们对估计值的不确定程度。F P F.T 表示不确定性通过系统模型传播再加上 Q 表示模型本身带来的新不确定性。为什么是 F P F.T这是协方差传播定律的直接应用。如果 x 的协方差是 P那么 A x 的协方差就是 A P A.T。这是线性变换下协方差的基本性质记住就行。3.2 更新步用观测修正预测更新步是卡尔曼滤波真正“滤波”的地方。它用实际观测来修正预测值。首先算新息innovation也就是观测残差y z - H x_predz 是实际观测值H x_pred 是根据预测状态“应该”观测到的值。两者之差就是新息代表观测带来的新信息。然后算新息协方差S H P_pred H.T R这个 S 代表新息的不确定性由预测不确定性和观测不确定性共同决定。接着算卡尔曼增益K P_pred H.T inv(S)卡尔曼增益是整个过程的核心。它决定了我们应该在多大程度上相信观测。如果 R 很小观测很准K 就会接近 H 的伪逆滤波器几乎完全信任观测如果 P_pred 很小预测很准K 就会接近零滤波器几乎不理会观测。最后更新状态和协方差x_new x_pred K y P_new (I - K H) P_predx_new 是在预测值基础上按卡尔曼增益加权的新息修正。P_new 是修正后的不确定性通常会比 P_pred 小因为观测带来了信息。3.3 为什么这五个公式是最优的这五个公式不是拍脑袋想出来的它们可以从最小均方误差准则推导出来。简单说卡尔曼增益 K 的那个形式正好是让估计误差协方差的迹最小的解。这是一个凸优化问题有解析解就是上面那个 K。另一个理解角度是贝叶斯推断。预测步对应先验更新更新步对应后验计算。在高斯假设下先验和后验都是高斯分布而高斯分布只需要均值和协方差就能完全描述。所以卡尔曼滤波本质上是在高斯假设下做贝叶斯推断的闭式解。这也是为什么卡尔曼滤波要求噪声是高斯的。如果噪声不是高斯卡尔曼滤波仍然是最优的线性估计器但不再是最优的非线性估计器。这一点在做实际项目时很重要因为很多传感器的噪声并不是严格高斯的。3.4 协方差矩阵的对称性与正定性维护理论上P 矩阵永远是对称正定的。但在数值计算中浮点误差可能会让它失去对称性甚至变成非正定导致滤波器崩溃。我踩过这个坑跑了几千步之后P 矩阵出现了很小的负特征值然后 inv(S) 算出来的结果完全不对滤波器直接发散。解决办法有两个。一是每次更新后强制对称化P (P P.T) / 2二是用 Joseph 形式更新协方差P_new (I - K H) P_pred (I - K H).T K R K.T这个形式在数值上更稳定虽然计算量稍大。我在长时间运行的系统里都会用 Joseph 形式。4. 用NumPy手写一个可运行的线性卡尔曼滤波4.1 类的结构设计我习惯把卡尔曼滤波封装成一个类这样状态和参数都在一起调用也方便。核心方法就两个predict 和 update。import numpy as np class LinearKalmanFilter: def __init__(self, F, H, Q, R, x0, P0): self.F np.array(F, dtypefloat) self.H np.array(H, dtypefloat) self.Q np.array(Q, dtypefloat) self.R np.array(R, dtypefloat) self.x np.array(x0, dtypefloat).reshape(-1, 1) self.P np.array(P0, dtypefloat) self.n self.x.shape[0] self.I np.eye(self.n) def predict(self): self.x self.F self.x self.P self.F self.P self.F.T self.Q self.P (self.P self.P.T) / 2 def update(self, z): z np.array(z, dtypefloat).reshape(-1, 1) y z - self.H self.x S self.H self.P self.H.T self.R K self.P self.H.T np.linalg.inv(S) self.x self.x K y I_KH self.I - K self.H self.P I_KH self.P I_KH.T K self.R K.T self.P (self.P self.P.T) / 2注意几个细节。x 我用列向量存储这样矩阵乘法不用转置来转置去。P 的更新用了 Joseph 形式并且每次更新后强制对称化。这些都是在实际运行中总结出来的习惯。4.2 参数初始化的经验值x0 通常设成第一次观测值或者一个粗略的猜测。P0 表示你对 x0 的不确定程度。如果你对初始状态完全没把握P0 可以设大一点比如 1000 * I。这样滤波器会快速收敛到真实值附近。Q 和 R 的初始化比较讲究。R 可以从传感器的数据手册里找噪声方差如果没有就采一段静止数据算方差。Q 通常需要调我一般先设一个很小的值比如 1e-5 * I然后根据滤波效果调整。有一个经验公式可以参考Q 的对角线元素大致等于“模型误差的标准差的平方”。比如匀速模型里如果你估计物体最大加速度是 1 m/s²那 sigma_a 就取 1然后按前面的公式算 Q。4.3 一个完整的匀速运动跟踪示例下面用模拟数据跑一遍。物体从位置 0 出发速度 1 m/s采样间隔 0.1 秒观测噪声标准差 0.5 米。np.random.seed(42) dt 0.1 n_steps 200 true_velocity 1.0 true_position 0.0 F [[1, dt], [0, 1]] H [[1, 0]] Q [[dt**4/4, dt**3/2], [dt**3/2, dt**2]] * 0.01 R [[0.25]] x0 [0, 0] P0 [[1000, 0], [0, 1000]] kf LinearKalmanFilter(F, H, Q, R, x0, P0) true_positions [] observations [] estimates [] for i in range(n_steps): true_position true_velocity * dt z true_position np.random.randn() * 0.5 true_positions.append(true_position) observations.append(z) kf.predict() kf.update([z]) estimates.append(kf.x[0, 0])跑完之后你可以把 true_positions、observations、estimates 三条曲线画在一起。你会看到估计曲线明显比观测曲线平滑而且紧贴真实值。这就是卡尔曼滤波的效果。4.4 怎么判断滤波器是否正常工作看三个东西。第一新息 y 应该接近零均值白噪声。如果 y 有明显的趋势或者周期性说明模型有问题。第二新息协方差 S 应该和实际新息的方差匹配。如果实际方差远大于 S说明 R 设小了。第三P 矩阵应该收敛到一个稳定值。如果 P 一直增大说明 Q 设太大了。我一般会画新息的直方图和自相关图。如果直方图接近正态分布自相关在滞后非零处接近零那滤波器基本没问题。5. 调参实战Q和R到底怎么设5.1 Q/R比值决定滤波器的“信任分配”这是最核心的一条经验。卡尔曼滤波的行为完全由 Q 和 R 的相对大小决定。你把 Q 和 R 同时放大十倍滤波结果完全一样。所以不要单独调 Q 或 R要调它们的比值。Q/R 大滤波器更相信观测响应快但噪声抑制差。Q/R 小滤波器更相信模型平滑但滞后。我通常先固定 R因为 R 有物理意义可以从传感器数据估然后调 Q。5.2 从传感器数据估R的实操方法把传感器放在一个静止的、已知的状态下采几百个点算方差。这就是 R 的对角线元素。如果传感器有多个轴每个轴单独算。如果传感器在运动没法静止采集那就用一段匀速运动的数据先拟合一个多项式把残差的方差当作 R 的估计。这个方法我用了很多次比拍脑袋设值靠谱得多。5.3 Q的调参策略从小往大调Q 的初始值我一般设得很小比如 1e-6 倍的单位矩阵。然后看滤波效果。如果估计曲线明显滞后于真实值在真实值变化时跟不上就增大 Q。如果估计曲线噪声太大就减小 Q。有一个更系统的做法把 Q 参数化成一个标量 sigma_a^2然后扫描这个标量看哪个值让新息的统计特性最接近白噪声。这个方法稍微费时间但能找到比较优的值。5.4 模型失配时会发生什么如果真实系统的运动和你的模型不一致比如真实物体在加速但你用的是匀速模型那新息会出现系统性偏差。这时候滤波器会“努力”用观测去修正但修正总是滞后。解决办法有两个。一是增大 Q让滤波器更相信观测代价是噪声变大。二是改模型把加速度纳入状态。后者更根本但计算量更大。我一般先试增大 Q如果效果不够再改模型。6. 那些让我调试到凌晨的坑6.1 矩阵维度不匹配最常见也最烦人卡尔曼滤波里矩阵乘法很多维度错一个就报错或者算出莫名其妙的结果。我的习惯是在类的初始化里就把所有矩阵的维度打印出来检查一遍。F 是 n×nH 是 m×nQ 是 n×nR 是 m×mx 是 n×1z 是 m×1P 是 n×n。这几个维度必须自洽。我见过有人把 H 写成 n×m结果 update 的时候没报错但算出来的 K 维度不对最后 x 变成了一个奇怪的形状。6.2 观测更新顺序写反导致发散predict 和 update 的顺序不能乱。必须先 predict 再 update。如果你先 update 再 predict相当于用当前观测去修正上一时刻的状态时间上就错了。更隐蔽的一种情况是在循环里先 update 再 predict但初始的 x 和 P 没有初始化好第一步 update 就把状态带偏了。我的做法是第一步只 predict 不 update从第二步开始正常循环。6.3 数值不稳定P矩阵失去正定性前面提过长时间运行后 P 可能失去正定性。除了对称化和 Joseph 形式还有一个办法是每隔一段时间重新初始化 P。但这会丢失历史信息不到万不得已不用。另一个数值问题是 inv(S) 的计算。如果 S 接近奇异inv 会出问题。可以用 np.linalg.solve 代替 inv或者用伪逆 np.linalg.pinv。我在 S 的条件数很大时会用 pinv。6.4 观测丢失或异常值处理实际系统里观测可能会丢失或者出现异常值。丢失的时候跳过 update只做 predict。异常值可以用新息的门限来检测如果 y 的范数超过某个阈值比如 3 倍 sqrt(S)就认为这个观测不可信跳过 update。这个技巧在传感器偶尔跳变的时候特别有用。我做过一个项目传感器每隔几百步会出一个明显偏离的值加了门限检测之后滤波曲线就干净了。7. 从线性到非线性什么时候该换滤波器线性卡尔曼滤波的适用边界很明确系统是线性的噪声是高斯的。一旦这两个条件不满足它就不是最优的了。常见的非线性场景包括雷达测距测角观测方程是非线性的、机器人位姿估计状态转移涉及三角函数、目标跟踪中的转弯运动模型本身是非线性的。这时候有两条路。一是扩展卡尔曼滤波EKF在当前估计点对非线性函数做一阶泰勒展开把它线性化然后套用线性卡尔曼滤波的框架。EKF 的优点是计算量小缺点是线性化误差大尤其是在非线性强的时候。二是无迹卡尔曼滤波UKF用一组确定性采样点sigma点来传播均值和协方差不需要求导。UKF 的精度通常比 EKF 高计算量也更大。我的经验是如果非线性程度不高EKF 够用如果非线性强或者函数不可导直接上 UKF。粒子滤波适合更复杂的场景但计算量是另一个量级一般不在考虑范围内。不过不管你后面用什么非线性滤波器线性卡尔曼滤波都是基础。它的五个公式、预测更新框架、Q/R调参逻辑在 EKF 和 UKF 里都是一样的。把线性版本吃透后面换滤波器只是换一层皮。8. 我个人的几个使用习惯最后分享几个我在实际项目里养成的习惯不一定对所有人适用但至少帮我省了很多调试时间。第一永远先用模拟数据验证。在接真实传感器之前用已知的真实轨迹加噪声跑一遍确认滤波器能收敛、能跟踪。这一步能排除掉大部分参数和维度问题。第二把新息和卡尔曼增益记录下来。这两个量是滤波器状态的“体温计”。新息异常说明模型或观测有问题增益异常说明 Q/R 比例不对。我一般会在调试阶段把这两个量画出来看。第三参数不要硬编码。把 Q、R、P0 这些做成配置项方便快速切换和对比。我见过太多项目把参数写死在代码里换一个场景就要改代码重新编译。第四注意单位。位置用米、速度用米每秒、时间用秒这些单位要统一。我踩过一次坑dt 用了毫秒但速度按米每秒算结果状态转移矩阵完全错了滤波器输出离谱到没法看。第五别迷信“最优”。卡尔曼滤波在假设成立时是最优的但实际系统总有各种不满足假设的地方。有时候一个简单的滑动平均或者低通滤波效果反而更好。工具是拿来用的不是拿来供的。