1. 为什么我也开始啃卡尔曼滤波1.1 从Dr_can的视频说起我很早以前就听过Kalman Filter的大名真正下定决心把它当成一个需要啃下来的课题是在一个移动目标位置估计的小项目里。当时手头只有一组噪声很大的位置测量值速度信息需要从差分里硬算出来结果差分之后的噪声大到完全没法用曲线抖得像心电图。折腾了两周阈值、滑动窗口和最小二乘拟合之后我才老老实实承认这种问题就应该用状态估计的思路来做于是找来网上评价很高的Dr_can卡尔曼滤波系列视频一边看一边做笔记。Dr_can的视频我前后刷了三遍。第一遍开二倍速扫概念第二遍跟着推导五个公式第三遍是合上视频自己拿纸笔默写整个流程再对比。三遍下来我才真正理解卡尔曼滤波为什么能工作、公式是怎么一环扣一环闭环的。这个系列笔记就是想把我的学习过程、踩坑记录和推导心得整理出来。第一篇先解决最基础也最核心的问题卡尔曼滤波到底在解决什么问题五个黄金公式为什么长成那个样子这篇笔记适合的人群很明确刚接触卡尔曼滤波、被一堆矩阵和协方差搞得晕头转向的初学者以及已经会用现成库调用、但想真正搞懂内部原理的人。如果你是做自动驾驶、机器人、无人机、组合导航或者目标追踪相关工作的相信这篇笔记能帮你省下不少自己摸索的时间。1.2 卡尔曼滤波到底在解决什么问题用一句大白话概括卡尔曼滤波是一种在含有噪声的系统中把“不精确的模型预测”和“不精确的传感器测量”融合起来得到比单独使用任何一种都更精确的状态估计的算法。这句话里有三个关键词值得拆开说。第一个关键词是“不精确的模型预测”。几乎所有物理系统都可以用状态空间方程来描述比如一个匀速运动的物体它的位置和速度随时间的变化关系可以用公式表达。但现实中没有哪个模型是完美的有没建模的摩擦力、有外部扰动、有参数标定误差因此模型推出来的结果肯定有误差。第二个关键词是“不精确的传感器测量”。传感器读数同样存在噪声无论是温度计、GPS、雷达还是IMU测量值都是“真实值噪声”的混合体。噪声的统计特性我们可能知道但噪声的具体数值无法预知。第三个关键词是“融合”。当预测和测量都不准的时候最自然的想法是取加权平均但关键问题是权重怎么取。如果测量噪声很大、模型误差很小那就应该多信模型反过来如果模型误差很大、测量相对精确那就应该多信测量。卡尔曼滤波干的事情就是根据两者不确定性的实时变化动态计算出一套最优权重然后加权融合。我用一个生活化的类比来帮助理解。想象你在一个黑乎乎的房间里走想判断自己和墙壁之间的距离。闭着眼睛走时你能根据自己迈了多少步、每步大概多长来估计距离这是“模型预测”问题是步长忽长忽短估计不准。你偶尔睁眼瞄一下能隐约看到墙的轮廓这是“测量数据”但光线太暗看到的位置也模模糊糊。如果你睁开眼看到的墙比你闭眼估算的远你会怎么处理卡尔曼滤波给出的答案是不完全相信任何一种而是看哪种方式更“靠谱”然后动态调整信任比例。从数学角度看这背后是典型的贝叶斯思想用模型的预测作为先验分布用传感器的测量作为观测似然计算后验分布。卡尔曼滤波之所以有简洁漂亮的解析解是因为它做了两个关键假设所有噪声都服从高斯分布系统模型和观测模型都是线性的。在这两个假设下后验分布仍然是高斯分布于是整个滤波问题就化简为“更新高斯分布的均值和方差”五个公式就足以完成全部递推计算。理解了这一层后面所有公式就不再是死记硬背而是可以自己推出来的。2. 拆解卡尔曼滤波的核心机制2.1 预测让模型先走一步卡尔曼滤波的整个工作流程可以拆成两半预测Predict和更新Update。预测这步的目标是“用系统的动态模型把状态估计从上一时刻推进到当前时刻”。比如一个匀速直线运动的物体在上一时刻我们知道它的大概位置和大概速度那么这一时刻位置应该等于上一时刻位置加上速度乘以时间步长速度如果假设不变就维持原值。这个过程用状态空间表达式写出来就是x̂_k⁻ F x̂_{k-1}⁺ B u_k这里的上标“−”表示先验估计意思是“还没有用当前时刻测量值修正过的估计”上标“”表示后验估计意思是“用了当前时刻测量值修正之后的估计”。F是状态转移矩阵描述系统的动态规律B是控制输入矩阵u_k是控制量。在Dr_can的视频里这个公式被反复强调为一个“把状态往前推”的步骤。但我在学习时留意到一个容易忽略的点预测时不仅要推状态向量x̂还要把误差协方差矩阵P一起推过去。第二个预测公式是P_k⁻ F P_{k-1}⁺ Fᵀ Q这个公式的含义是“不确定性如何随模型往前传播”。Q是过程噪声协方差矩阵它描述的是模型本身的不确定性如果Q很大说明你认为模型预测是粗糙的即使上一时刻的状态很确定推进到当前时刻后不确定性也会明显变大如果Q很小说明你认为模型很可靠预测值的不确定性增长得少。我第一次学习时犯过一个错误只把状态向量往后推了完全把P矩阵抛在脑后。后来才意识到P矩阵是整个滤波器的“信心指标”如果没有它后面根本无法计算卡尔曼增益。预测和更新两个模块通过P矩阵串联起来形成一个完整的闭环。2.2 更新用观测数据修正方向当传感器返回一个新的观测值z_k时第二个信息来源出现了。但传感器读数不能直接和状态向量比大小因为测量得到的量往往和状态不在同一个空间。比如状态是物体的位置和速度而传感器是激光雷达返回的是距离或者状态是温度传感器返回的是电压。这时候需要用观测矩阵H把状态映射到测量空间写成z_k H x_k v_k其中v_k是测量噪声它的协方差矩阵用R表示。R描述了传感器读数的不稳定程度这个值通常可以从传感器标定文档或者一段静态测量数据的方差中估计出来。更新步骤要回答的核心问题只有一个模型预测值和传感器测量值之间我到底该信谁多一点答案就藏在两者的不确定性里。如果P_k⁻很大而R很小说明预测很不可靠、测量很可靠加权时应该偏向测量如果P_k⁻很小而R很大则相反应该偏向预测。这个动态的偏向程度用一个量来刻画那就是卡尔曼增益K_kK_k P_k⁻ Hᵀ (H P_k⁻ Hᵀ R)⁻¹只看公式会觉得抽象但把它想成“预测不确定性与总不确定性预测测量的比值”就很直观增益越大说明预测的不确定性占比越高应该更多地采纳测量值增益越小说明测量噪声占比高应该保持预测结果。得到增益之后就可以修正先验估计了x̂_k⁺ x̂_k⁻ K_k (z_k − H x̂_k⁻)括号里的z_k − H x̂_k⁻在滤波领域叫“新息”或者“残差”含义是“测量值相对于预测值的偏差”。卡尔曼滤波的更新过程本质上就是按照增益K_k的大小把一部分残差加到先验估计上得到后验估计。当K接近1时估计值几乎完全跟着测量走当K接近0时估计值几乎保持预测不变。更新步骤同时还要修正误差协方差P_k⁺ (I − K_k H) P_k⁻这个公式反映的是在吸收了测量信息之后状态的不确定性应当缩小。因为多了一个信息源你对状态的把握应该更大了方差自然要下降。这一点在物理意义上非常直观。2.3 协方差矩阵不确定性如何流动很多初学者看到P矩阵就头疼。如果状态只有一个量P就是一个标量方差很容易理解但实际工程中状态往往是多维的比如位置和速度、横滚角和角速度等这些量之间不是孤立的而是存在相关性。协方差矩阵正是用来描述这种相关性的。对角线元素是每个状态的方差非对角线元素是状态之间的协方差。在预测和更新过程中不仅各个状态的方差在变化它们之间的相关性也在变化。如果忽略了相关性滤波器的性能会明显下降。举一个常见的例子用位置和速度两个状态去跟踪一辆匀速行驶的车。假如某一时刻测量显示位置偏了由于位置和速度之间存在物理上的强相关性速度的估计也应当被联动修正。卡尔曼滤波通过P矩阵的非对角元素自动实现了这种联动不需要额外写任何逻辑。这也是“矩阵版卡尔曼滤波”比几个标量加权平均更强大的根本原因。它不只是把两个数做加权平均而是在一个多维、变量之间相互耦合的状态空间里进行最优估计。一个比较自然的学习路径是先把一维标量案例彻底搞懂此时矩阵全部退化为数字公式直观得多然后再把矩阵版本看作“同时处理多个标量、且变量之间耦合”的推广。我自己就是这样一步步走过来的第4节会专门手算一个一维例子。3. 五大公式的推导与直观理解3.1 先建立一套统一的符号约定要真正理解卡尔曼滤波第一步是统一符号。不同教材和视频用的符号略有差异如果不提前统一很容易看糊涂。我这里采用Dr_can视频里比较常用的一套符号和多数工程资料也能对上x_kk时刻系统的真实状态实际中永远无法确切知道x̂_k⁻k时刻的先验估计即只用模型预测得到的估计值x̂_k⁺k时刻的后验估计即融合了测量值后的估计值P_k⁻、P_k⁺对应的误差协方差F状态转移矩阵描述上一时刻状态如何影响当前时刻状态B控制输入矩阵u_k是控制量H观测矩阵把状态空间映射到测量空间z_kk时刻的传感器测量值Q过程噪声协方差矩阵R测量噪声协方差矩阵K_k卡尔曼增益这套符号记起来有个小诀窍带“−”上标的都是预测阶段的结果带“”上标的都是更新阶段的结果P矩阵跟着估计值走估计值在哪里更新P就在哪里更新。把这条主线抓住五个公式其实只是在轮流更新x̂和P。3.2 从状态方程推导预测公式预测公式的第一个看起来很好理解x̂_k⁻ F x̂_{k-1}⁺ B u_k它就是状态方程的期望形式。假设系统的真实动态是x_k F x_{k-1} B u_k w_k其中w_k是过程噪声。对两边取期望由于过程噪声的期望为0得到E[x_k] F E[x_{k-1}] B u_k。把期望替换成估计值就得到了预测公式。第二个预测公式稍微复杂P_k⁻ F P_{k-1}⁺ Fᵀ Q它表达的是不确定性如何传播。真实估计误差可以写成e_k x_k − x̂_k⁻代入状态方程整理后会发现误差通过F矩阵被放大或缩小同时还叠加了新的过程噪声。根据协方差传播的性质对于一个线性变换加独立噪声的情况Var(F e w) F Var(e) Fᵀ Var(w)。这就是第二个预测公式的来源。这里有一个我容易写错的地方P矩阵传播时要写成F P Fᵀ而不是F P F。原因是协方差矩阵要保持对称性而且维度才能对齐。F乘在左边转置乘在右边相当于对矩阵做了一次“二次型变换”。记住这个口诀协方差传播左边乘原矩阵右边乘转置。3.3 从条件高斯推导更新公式与卡尔曼增益更新公式的推导可以从两个方向理解。一个是直接套用高斯变量的条件分布公式另一个是从“新息”的角度来理解。我个人觉得条件分布的方向更本质。在预测阶段结束后我们手里有两个和真实状态相关的量一是先验估计x̂_k⁻它围绕着真实状态有一个分布方差就是P_k⁻二是测量残差z_k − H x̂_k⁻它的均值是0方差是H P_k⁻ Hᵀ R。在模型和噪声都是高斯的假设下这两个量是联合高斯的。这里用到概率论里的一个经典结论对于联合高斯分布的两个变量a和b在已知b的条件下a的条件均值可以写成E[a|b] E[a] Cov(a, b) Var(b)⁻¹ (b − E[b])把a取为真实状态x_kb取为测量残差经过整理后条件均值恰好就是更新公式x̂_k⁺ x̂_k⁻ P_k⁻ Hᵀ (H P_k⁻ Hᵀ R)⁻¹ (z_k − H x̂_k⁻)于是卡尔曼增益K_k P_k⁻ Hᵀ (H P_k⁻ Hᵀ R)⁻¹的直观意义就清楚了它是预测误差与测量残差之间的协方差除以测量残差的总方差。增益越大说明预测误差与测量残差的相关性越强就越应该用残差去修正预测。用同样的条件方差公式可以得到P_k⁺的更新式P_k⁺ P_k⁻ − K_k H P_k⁻ (I − K_k H) P_k⁻这个过程让我对五大公式的理解上了一个台阶卡尔曼滤波本质上每一步都在做“在已知观测值条件下求状态的条件高斯分布计算”。五个公式只是这个条件分布计算的显式结果。一旦理解到这一层就算忘了某个公式的具体细节也能现场把核心推导写出来。4. 手把手推演一个一维温度估计例子4.1 问题设定与参数选择用一个和Dr_can视频类似的经典一维例子来实际走一遍流程。假设你在一个房间里测量温度真实温度恒定不变是25摄氏度。温度计每秒返回一个读数读数的噪声服从均值为0、方差为4的高斯分布也就是说R 4。系统状态x_k是当前温度。因为温度恒定所以状态转移矩阵F 1没有控制量B 0观测矩阵H 1因为温度计直接测温度。过程噪声Q设为一个比较小的值比如0.01表示“我们相信房间温度不会突变但允许有一点极小的扰动”。初始化时我们不知道真实温度随便猜一个初始估计x̂_0⁺ 20摄氏度初始误差方差P_0⁺ 10。这个P_0取得比较大说明“我对初值猜得很没信心”。4.2 前两步迭代的完整手算先做第一步预测x̂_1⁻ 1 × 20 0 20 P_1⁻ 1 × 10 × 1 0.01 10.01计算卡尔曼增益K_1 P_1⁻ H / (H P_1⁻ H R) 10.01 / (10.01 4) ≈ 0.7146假设第一次测量读数是z_1 26更新x̂_1⁺ 20 0.7146 × (26 − 20) ≈ 24.2876 P_1⁺ (1 − 0.7146) × 10.01 ≈ 2.857可以看到仅仅一次更新估计值就从20跳到了24.29方差也从10降到了2.86。由于测量噪声方差R 4比较大估计值并没有完全跳到26而是往测量方向拉了一部分停在了一个合理的中间位置。接着做第二次迭代。假设第二次测量z_2 25.5。先预测x̂_2⁻ 24.2876 P_2⁻ 2.857 0.01 2.867再算增益K_2 2.867 / (2.867 4) ≈ 0.4176然后更新x̂_2⁺ 24.2876 0.4176 × (25.5 − 24.2876) ≈ 24.7948 P_2⁺ (1 − 0.4176) × 2.867 ≈ 1.6696继续迭代下去你会看到两个规律。第一估计值逐渐逼近真实的25P也逐渐变小说明对状态的把握越来越足。第二卡尔曼增益K从0.7146逐渐下降并趋于一个稳定值因为随着滤波进行P⁻变小R的相对比重大了滤波器越来越依赖自己的预测而不是单次测量。这个例子特别适合新手自己动手算几遍因为所有公式都退化成数字每一步算出来都能和直觉对得上。4.3 用Python验证手算结果手算容易出错我用Python中的numpy把同样的过程写了一遍代码非常简短import numpy as np F np.array([[1.0]]) H np.array([[1.0]]) Q np.array([[0.01]]) R np.array([[4.0]]) x np.array([[20.0]]) P np.array([[10.0]]) measurements [26.0, 25.5, 24.8, 25.2, 25.1] for z in measurements: # 预测步骤 x_pred F x P_pred F P F.T Q # 更新步骤 K P_pred H.T np.linalg.inv(H P_pred H.T R) x x_pred K (z - H x_pred) P (np.eye(1) - K H) P_pred print(fz{z:.1f} | x{x.item():.4f} | P{P.item():.6f} | K{K.item():.4f})运行之后前两步的输出和手算结果完全一致。从第三步开始估计值会越来越接近25P继续逐步下降。我建议初学者把上面代码中的Q和R改一改比如把R从4改成100再运行一次观察估计值对测量的响应变化这个实验能非常直观地体会到“R越大越信任预测”的含义。5. 新手最容易踩的坑与排查方法5.1 Q和R不是随手调的超参数很多刚接触卡尔曼滤波的人会把Q和R当成机器学习里的超参数随便调几组看看哪个效果好但对它们背后的物理含义没有概念。这是一个很常见、也很危险的误区因为如果Q和R的比值与实际系统严重不符滤波器给出的“最优估计”根本不是真正的最优。Q描述的是“你对模型动态的信任程度”。如果Q调得过大滤波器会认为模型完全不可信估计值会剧烈跟随测量噪声被放大如果Q调得过小滤波器会过度相信模型测量数据几乎不起作用一旦模型存在偏差估计值就跟不上真实状态。R描述的是“你对传感器的信任程度”R过小会让滤波器对测量噪声过度敏感R过大会让滤波器反应迟钝。正确的做法是先根据工程实际给R一个合理估计比如直接使用传感器标定文件里的噪声方差Q则根据你对未建模动态的经验来估计。我调参的习惯是先固定R从一个较小的Q开始观察估计轨迹的平滑度再逐步增大Q直到平滑度和跟踪响应速度达到让人满意的平衡点。整个过程要结合具体的应用场景来判断而不是盲目追求曲线好看。5.2 初始值P_0和x̂_0怎么定初始状态估计x̂_0的设定确实有一定自由度因为卡尔曼滤波对初值并不那么敏感。如果你对初值毫无头绪那不用纠结随手设一个合理范围内的值即可。真正需要留意的是P_0的设置P_0越大初始卡尔曼增益K就越大滤波器会在早期自动给测量更高的权重从而快速把x̂_0拉到真实值附近。反过来如果P_0设得很小滤波器一开始就表现出很强的信心那么即便测量数据在疯狂暗示“你猜错了”估计值也会固执地停留在原地收敛会变得非常慢。我在第一次做仿真时把P_0设成了0.1结果前几十步几乎没有任何更新还以为代码写错了。后来把P_0调大到10滤波器的收敛速度快了很多。所以在不知道该信任谁的时候宁可把P_0调大一些让滤波器自己从测量中学习。5.3 模型失配会引发滤波发散状态转移矩阵F是对真实物理系统的简化描述不可能100%精确。比如在温度估计的例子中如果房间温度并非严格恒定而是会受空调影响慢速波动那么F1、Q0.01的模型就低估了过程不确定性导致估计方差偏小。这样算出来的置信区间会比真实情况窄让人产生一种“估计得很准”的错觉但实际上是对误差的错误自信。模型失配严重时可能导致一个专业术语叫“滤波发散”的现象长时间运行后P矩阵被压到极小卡尔曼增益几乎等于0新的测量无法再修正估计。此时如果系统状态突然发生变化滤波器就像睡着了一样反应不过来。防止发散有一个实用技巧人为给Q设置一个下限不让P无限趋近于0。另一个技巧是在系统启动阶段使用较大的Q等观测一段时间、模型逐渐校准后再降低Q。我在实际调试时还发现一个有用的做法定期计算残差新息的均值与方差看看它是否与理论值一致。如果残差均值长期偏离0或者残差方差明显大于HP⁻H^TR说明模型或者噪声参数与实际情况不符应该回头检查F、H、Q、R的设置。5.4 状态估计代码调试的几个经验调试卡尔曼滤波代码最让我受益的是三个习惯。第一写代码时把每个矩阵的维度标注出来。卡尔曼滤波的矩阵运算比较多维度不匹配是最高频的错误。我在草稿纸上习惯这样写P是(2,2)F是(2,2)F P F.T是(2,2)Q是(2,2)H是(1,2)K是(2,1)。维度对上了公式才不容易写错。第二用已知结果做交叉验证。我推荐用filterpy库来对照手写代码如果手写实现和标准库的结果偏差超过浮点误差那一定是自己的实现有问题。这种对照法比盯着屏幕看半天更高效。第三先跑最简场景。在调试复杂的二维或三维滤波器之前先把矩阵退化成一维的简单场景跑通确认逻辑无误后再逐步增加维度。这样能把问题定位在“逻辑错误”还是“维度错误”上省下大量排查时间。6. 从学习笔记到实际问题6.1 标准KF在实际项目中的经典应用场景学完卡尔曼滤波的五大公式之后很多人的下一个问题自然会是这东西到底用在哪里最直接的场景是目标跟踪。雷达或者激光雷达返回带有噪声的目标位置同时目标可能在做近似匀速或匀加速运动这时候卡尔曼滤波可以实时输出平滑的位置和速度估计。自动驾驶中的车辆状态估计、安防监控中的行人轨迹追踪本质上都是这个套路。第二个经典场景是导航与组合惯导。GPS的更新频率低且容易受遮挡影响惯性测量单元IMU的短时间精度高但会持续漂移。卡尔曼滤波可以把GPS观测和IMU预测融合起来在GPS信号短暂丢失时依靠IMU预测继续维持一段时间的位置估计这就是“组合导航”的核心思想。很多无人机、机器人和手机导航方案里都有它的身影。第三个场景是传感器融合。当系统里同时有多个传感器比如摄像头、毫米波雷达和超声波雷达测量各自的噪声特性不同、观测的物理量也不同用卡尔曼滤波可以把它们的信息统一融合到一个状态估计中。这也是当前智能驾驶领域多传感器融合的基础框架之一。6.2 下一步路线与自测练习题标准卡尔曼滤波学完之后接下来比较自然的学习路线是扩展卡尔曼滤波EKF和无迹卡尔曼滤波UKF。当系统是非线性的时候比如雷达通过距离和角度计算位置时观测方程就不是线性的此时需要对非线性做处理。EKF通过泰勒展开把非线性系统线性化UKF通过采样点传播不确定性两者在实际工程中都非常常用。如果你想把基础打牢我建议先完成下面三个自测练习。第一个练习写一个二维匀速运动目标跟踪demo。状态设为[x, y, vx, vy]观测是带噪声的位置测量用矩阵版KF完成跟踪画出真实轨迹、测量轨迹和滤波轨迹观察误差变化。第二个练习在固定R的情况下把Q从很小调到很大画出同样的跟踪曲线观察估计轨迹是变得更平滑还是更容易跟丢。这个练习会让你对Q的物理含义有非常直观的体会。第三个练习把一维温度例子改成温度缓慢变化的情况比如真实温度是30 0.1k的线性上升趋势但你的模型仍然是F1的恒定模型。观察估计值会出现什么现象再思考如何改进模型或调整Q。做完这三个练习你对卡尔曼滤波的掌握程度会比只看视频深得多。6.3 一点学习体会我在实际学习中发现卡尔曼滤波给人最大的门槛其实不是数学本身而是“直觉的建立”。一开始看矩阵公式会觉得抽象很容易陷入机械套公式的循环。真正让我开窍的是两件事一是把一维例子用笔一步步算了几十遍直到闭着眼睛都能说出“预测—更新—增益”这三个动作的含义二是把协方差矩阵想象成“对估计的信心程度”而不是只把它当数学概念。卡尔曼滤波最迷人的地方在于它把“不确定”变成了一个可以计算、可以传播、可以更新的量。做状态估计的人每天面对的都是不完美的模型和不完美的传感器但有了这套框架你可以在不完美里找到统计意义下最优的那个折中点。这个思想不仅适用于工程也会改变你看待噪声和误差的方式。最后再分享一个小技巧学习时一定要把每一步的矩阵维度写出来顺手在纸上标记清楚这个方法能避免百分之八十的书写错误。我自己吃过很多次维度不匹配的亏养成了这个习惯之后推导新公式的效率明显高了很多。希望这篇笔记对你也有同样的帮助。