上周把Dr_can的Kalman Filter系列视频重新从头到尾刷了一遍这次看得比第一次细得多边看边推公式边写代码验证整理出了一份比较完整的笔记。卡尔曼滤波这个名字做机器人、导航、自动驾驶、信号处理甚至金融数据分析的朋友应该都不陌生——它本质上就是一套在充满噪声的传感器数据里把系统真实状态估准的算法框架。无人机在空中悬停的时候GPS给的位置可能偏好几米加速度计积分出来的速度又在慢慢漂任何单一传感器都没法直接给出高精度的位置和速度但卡尔曼滤波可以把这些不可靠的测量融合起来最终给出一组误差明显更小的估计值。这个算法从1960年提出到现在六十多年过去依然是工程领域的常青树从导弹制导、航天器轨道估计到手机里的陀螺仪与GPS融合都有它的身影。这篇笔记适合正在学控制、机器人、感知或者SLAM相关方向的朋友也适合单纯好奇手机里的传感器数据是怎么被融合成稳定输出的这类问题的读者。我会尽量不照着教科书念推导而是用直觉、图示化的理解外加一组具体的数值例子把卡尔曼滤波从建模到迭代计算的完整链路拆开讲清楚。你不需要很强的数学背景只要懂一点矩阵乘法和高斯分布的基本概念就能跟得上。看完这篇至少你能回答三个问题卡尔曼滤波解决什么问题、五个核心公式各自在干什么、实际调参的时候Q和R到底怎么给。这几件事弄明白了后续再看EKF、UKF或者工程里的各种变体都不会发怵。1. 卡尔曼滤波到底在解决什么问题从一个悬停场景说起1.1 一个最直观的例子无人机悬停假设你有一架四旋翼无人机正悬停在半空中你希望它像钉在天上一样保持不动。但真实情况是GPS给出的坐标在上下抖动民用级的误差可能有三到五米气压计测高度存在漂移IMU里的加速度计数据积分出来的速度随时间不断累积误差。现在你想知道无人机此刻到底在哪里、速度是多少却发现每个测量来源都有瑕疵。如果你只用GPS位置跳来跳去如果你只用IMU积分短期看起来平滑但时间一长位置就不知道漂到哪里去了。卡尔曼滤波的核心思路是把你对系统运动规律的了解和传感器的观测这两类信息结合起来各自取长补短最终得出一个比任何单个来源都可靠的状态估计。它干的事情不是简单的低通滤波而是真正意义上的最优状态估计。1.2 滤波为什么能成为最优估计很多人第一次听到卡尔曼滤波这个名字时会觉得它是一种频域滤波器类似低通或者高通那种。但实际上卡尔曼滤波的全称是线性二次最优估计它处理的是时域里的状态估计问题。它之所以称为最优基于三个前提假设系统模型是线性的也就是下一时刻的状态和当前状态、控制输入之间是线性关系过程噪声和测量噪声都服从高斯分布噪声的统计特性均值和协方差已知。在满足这三个假设的前提下卡尔曼滤波给出的是最小均方误差意义下的最优估计。这个最优不是自封的而是可以从数学上严格证明的。当然真实工程里完全线性的系统几乎不存在所以后来才有了扩展卡尔曼滤波EKF、无迹卡尔曼滤波UKF这些变体但它们的核心思想依然建立在标准卡尔曼滤波的骨架上。1.3 用一个通俗类比理解融合的意义可以把卡尔曼滤波类比成两个人合作称一杯水的重量。你凭手感先估了一下觉得大概是200克但你的手不是秤估计误差在±10克接着你又拿了一个不太精密的电子秤称了一下显示220克这个秤的说明书标称误差是±25克。现在你认为水的真实重量是多少直觉告诉你200克的这个估计可信度更高一些所以最终答案应该更靠近200克而不是220克。最简单的做法就是加权平均权重和误差方差成反比——你的手估计误差小就给更大权重电子秤误差大就给小权重。卡尔曼滤波的每一次更新其实就是在动态地做这种事情只不过它加权的对象是模型预测值和传感器测量值而权重就是那个大名鼎鼎的卡尔曼增益Kalman Gain。增益不是固定的它会根据当前系统的不确定性和传感器的噪声水平自动调整。2. 建模是第一步状态方程和观测方程怎么搭起来2.1 系统的状态是什么要使用卡尔曼滤波第一步不是写代码而是定义系统的状态。状态就是一组能够完整描述系统当前情况的变量在数学上用一个向量表示。以无人机悬停模型为例如果你关心的是位置和速度状态向量就是x [位置, 速度]^T很多新手会问能不能把加速度也放进状态里可以但会增加模型复杂度。在实际工程中加速度这类高阶量往往通过过程噪声来吸收因为如果你把加速度建进模型就必须知道它的演化规律而大多数情况下你对这个规律的了解并不比噪声猜得多。建模的哲学是能简化的地方绝不复杂化模型里只放你确信物理规律清晰的状态变量。2.2 状态转移方程系统如何随时间演化系统在离散时间下从k-1时刻到k时刻的演化过程用状态转移方程描述x_k A * x_{k-1} B * u_k w_k其中A叫状态转移矩阵它描述系统在没有任何外部输入的前提下如何从上一时刻的状态推导出当前时刻的状态B是控制矩阵u_k是控制输入比如飞机收到的油门指令或者转向指令w_k是过程噪声它代表你对系统模型的信任程度——模型再准也不可能完全刻画真实世界的所有细节这些没建模的差异就归入w_k。对于匀速运动模型如果采样间隔为dt状态向量是[位置, 速度]^T那么状态转移矩阵是A [[1, dt], [0, 1]]这个矩阵的含义很直观新位置 旧位置 旧速度 × dt新速度 旧速度因为是匀速模型。把这个矩阵和状态向量做乘法你会发现它其实就是把两条物理公式写成了矩阵形式。所以状态转移矩阵并不是什么神秘的东西它只是系统物理规律的浓缩表达。2.3 观测方程测量值和真实状态的关系光有模型还不够你还得描述传感器测量值和状态之间的关系这就是观测方程z_k H * x_k v_k其中H是观测矩阵v_k是测量噪声。拿无人机例子说如果GPS直接测量位置不直接测量速度那么观测方程就是z 位置H矩阵就是[1, 0]意思是观测值只取状态向量中的第一个分量。如果你有一个传感器能同时测位置和速度H矩阵就要相应地写成两行。H矩阵其实就是从状态空间到测量空间的一座桥。2.4 为什么高斯分布的假设如此重要高斯分布在卡尔曼滤波中的地位极其重要原因有两点大量独立微小因素叠加的结果会趋近高斯分布中心极限定理所以把没建模的噪声假设为高斯分布在很多场景下是合理的高斯分布经过线性变换后依然是高斯分布而且高斯分布完全由均值和协方差矩阵两个参数决定。第二点是整个卡尔曼滤波闭环能够运转的关键。状态估计的置信程度用协方差矩阵来表示预测和更新过程中我们永远只需要维护均值和协方差这两个量它们经过一轮又一轮的迭代依然保持高斯形式不需要额外参数。这就是为什么卡尔曼滤波能以如此简洁的递归形式运行几十年的根本原因。3. 五个核心公式每一个都配一个直觉理解3.1 预测步骤公式卡尔曼滤波的每个迭代周期可以清晰地分为两个阶段预测Predict和更新Update。预测阶段的第一个公式是先验状态估计x̂_k^- A * x̂_{k-1} B * u_k这个式子做的是根据上一时刻的最优估计x̂_{k-1}套用系统模型推算出当前时刻系统大概在哪里。左边那个减号上标表示这是利用模型得到的先验估计还没结合测量。所谓先验prior就是在观测数据到来之前预估的状态。预测阶段的第二个公式是先验协方差估计P_k^- A * P_{k-1} * A^T Q这个式子稍微抽象一点。P是状态协方差矩阵对角线上的元素表示各个状态变量的方差非对角线表示不同状态变量之间的相关性。这行公式做了两件事第一件A * P_{k-1} * A^T是把上一时刻的不确定性通过状态转移矩阵变换到当前时刻第二件加上Q是因为过程噪声w_k给系统注入了新的不确定性。可以这样理解即使上一时刻你对位置完全确定只要模型里加了噪声预测出来的位置就一定有不确定性这部分不确定性就是Q。3.2 更新步骤公式测量到来之后进入更新阶段。第三个公式是卡尔曼增益K_k P_k^- * H^T * (H * P_k^- * H^T R)^{-1}卡尔曼增益是整个算法里最核心的量它的作用是决定预测值和测量值各占多少权重。如果P_k^-很大意味着预测很不确定K会变大算法就更信任测量如果R很大意味着传感器噪声很大K会变小算法就更信任预测。这个增益不是拍脑袋给的它在每个时刻都根据最新的协方差矩阵和噪声参数自动计算出来所以卡尔曼滤波能够自适应地调节信谁多一点。第四个公式是后验状态估计x̂_k x̂_k^- K_k * (z_k - H * x̂_k^-)括号里那一项叫新息innovation即测量值减去预测值它代表模型预测和传感器观测之间的差异。如果差异为零说明预测和测量完全一致不需要任何修正如果差异很大说明预测可能偏离了就会乘上增益K来修正。K介于0和1之间所以修正量不会超过新息本身这一点保证了估计的平滑性。第五个公式是后验协方差估计P_k (I - K_k * H) * P_k^-测量更新阶段引入了新的信息所以不确定性通常会下降。这个公式计算出的P_k将作为下一轮迭代的P_{k-1}完成整个递归闭环。3.3 用一个简单数值验证直觉假设位置预测值x̂^- 10预测方差P^- 4测量噪声方差R 1实际测量值z 12。那么卡尔曼增益K 4 / (4 1) 0.8。后验估计x̂ 10 0.8 × (12 - 10) 11.6。注意这个结果P^- 4的平方根是2R 1的平方根是1预测的标准差是测量标准差的2倍说明预测更不确定所以最终估计往测量方向移动了80%从10移动到接近12的位置。如果反过来P^- 1而R 4K就变成1 / (1 4) 0.2估计值变成了10.4更靠近预测值。这个简单例子说明了一个关键规律卡尔曼滤波的融合方向永远朝更可信的一方倾斜。4. 完整跑一遍一维示例手把手算到收敛4.1 问题设定与初值选择用一个温度估计的例子来完整跑一遍算法。假设你要估计室温室温大约在25℃附近缓慢变化。用一个精度有限的温度计来测它的测量噪声方差R 4也就是标准差为2℃。因为室温变化很慢我们设置过程噪声方差Q 0.01。初始估计设为x̂_0 20℃初始协方差P_0 10。这个初始值意味着我们坦白承认自己对真实温度非常不确定——猜了20但方差10对应的标准差约3.16℃说明真实值大概率落在20±3.16℃的区间内。P_0设大一点在工程上是合理的因为后期的迭代会逐步矫正初始猜测的误差不需要对初值过分纠结。4.2 第一次迭代的完整计算预测阶段室内温度变化很小状态转移矩阵A 1没有控制输入x̂_1^- x̂_0 20P_1^- P_0 Q 10 0.01 10.01现在假设第一次测量值z_1 24℃开始更新K_1 P_1^- / (P_1^- R) 10.01 / (10.01 4) 0.7145x̂_1 20 0.7145 × (24 - 20) 22.86P_1 (1 - K_1) × P_1^- 0.2855 × 10.01 2.86经过第一次迭代估计值从20跳向24但没完全到达因为初始预测还有一定可信度。协方差从10骤降到2.86说明融合测量后不确定性大幅缩小。4.3 连续迭代观察收敛过程继续迭代几次每次在预测后加入一个新的测量值。假设后续的测量值分别在24.5、25、24.8、25.2℃附近经过五轮迭代后估计值x̂会稳定在25℃附近协方差P会逐步递减最终稳定在某个平衡值附近卡尔曼增益K也会随之变小说明随着估计越来越可信算法越来越不依赖单次测量。有意思的是P不会一直降到零。这是因为每一轮预测都会注入过程噪声Q哪怕很小所以稳态时预测带来的不确定性和测量减少的不确定性会达到一个动态平衡。这个稳态值可以用代数方程直接解出来在工程上很有用因为你可以提前算出这个滤波系统最终能把误差压到多少。4.4 初值敏感度P_0设大设小有什么影响如果你把P_0从10改成100甚至1000前几步的K会接近1估计值会迅速冲向测量值之后协方差很快收敛到正常水平。反过来如果你把P_0设成0.01也就是对初始猜测极度自信算法前几步就会非常依赖预测值需要多花几轮才能把估计拉回真实值附近。这就解释了一个常见疑问P_0到底怎么设工程经验是宁可设大也不要设小设大只影响前面几步收敛速度设小可能导致前期滤波结果偏差大。5. 从一维到多维矩阵形式和协方差矩阵的正确理解方式5.1 一维到多维的转变难点在哪一维例子很好理解但实际系统的状态往往包含多个变量比如无人机的位置、速度、加速度计偏置、陀螺仪偏置等。这些变量之间往往存在耦合位置和速度本身就通过运动学规律相互关联多个传感器的观测在统计上也常常不是独立的。一维形式完全无法处理这种耦合必须使用多维形式也就是矩阵和向量。多维形式下核心难点不是公式本身公式形式上和一维完全一样而是理解协方差矩阵P。很多初学者在把一维代码扩展到多维时最容易出问题的地方就是P矩阵的设置和迭代是否合理。5.2 协方差矩阵P的对角线与非对角线P矩阵是一个对称矩阵对角线元素是各状态变量的方差非对角线元素是不同状态变量之间的协方差。以状态向量[位置, 速度]^T为例P[0][0]是位置的方差P[1][1]是速度的方差P[0][1]和P[1][0]相等表示位置和速度的相关性。为什么这个相关性很重要想象一辆匀速运动的车如果你在某个时刻知道位置偏大那么因为速度估计也被模型关联着下一时刻位置与速度的相关性会传递到预测过程中。这就是A * P_{k-1} * A^T这步在背后做的事情——它不仅变换方差还保持并更新变量间的相关性。如果把P的非对角线元素全部清零当成对角阵处理虽然计算简化了但会丢失状态间耦合的重要信息在系统动态关联强的时候滤波性能会明显下降。5.3 控制输入和过程噪声在多维场景中的形态多维情况下控制输入B * u_k的形式不变只是矩阵B和向量u_k的维度变大了。比如一个能输出加速度的车辆模型控制输入是油门产生的加速度值B矩阵把加速度映射到速度变化和位置的二阶变化上。过程噪声协方差Q是一个n×n矩阵n是状态维度。初学者最常犯的错误是把Q设成全零矩阵——这等于告诉滤波器你的模型完美无缺、毫无噪声。实际系统中总存在未建模的动态比如风、摩擦、模型误差Q应该保留适当的值。最简单的Q设置方式是对角阵对角线元素表示各状态的过程噪声方差非对角线置零表示各噪声源独立。只有在确实知道噪声来源相关时才需要设置非对角线元素。5.4 EKF和非线性系统的入口你可能已经注意到所有公式都依赖A和H这两个线性变换矩阵。当系统变成非线性时比如机器人用里程计和激光雷达的SLAM场景状态转移和观测模型往往是非线性函数这时标准卡尔曼滤波就不能直接使用。扩展卡尔曼滤波EKF的做法是在当前状态估计点做一阶泰勒展开把非线性函数线性化也就是计算雅可比矩阵来代替A和H其余流程和标准卡尔曼滤波完全一致。理解标准卡尔曼滤波的矩阵形式是理解EKF的前提。很多同学直接上手EKF时一头雾水本质上是把标准卡尔曼滤波这块地基没有夯实。我的建议是先花时间把一个简单的两维位置-速度模型用标准KF实现清楚再去看EKF感受会完全不同。6. 最考验功力的环节Q和R的取值与调参经验6.1 R的初值从哪里来测量噪声协方差R的取值相对容易因为传感器的标称精度通常在数据手册里就能找到。比如温度计标称精度为±2℃如果你把±2理解为2倍标准差那么R 4。GPS的定位精度在开阔环境下一般是几米你就可以按这个量级设置R。但要注意一点数据手册的指标往往是在理想测试条件下测得的实际使用环境可能更差。一个工程上常用的方法是做一次静态实验把传感器放在已知真值的位置连续采集一段时间数据计算测量值相对真值的方差用这个实测值作为R的初始参考。这个方法虽然简单但比你拍脑袋给一个数可靠得多。6.2 Q的初值最费劲Q代表过程噪声协方差也就是你对自己系统模型的信任程度。它没有数据手册可以参考因为它刻画的是模型和现实之间的差距这个差距完全取决于你建模的好坏。我的经验是从小往大调。先用一个比较小的Q跑起来如果滤波结果出现明显的滞后或者过于平滑、跟不上真实变化就逐渐增大Q。过程噪声本质上意味着系统状态里存在你模型没描述的成分Q越大滤波越相信测量值估计结果越会跟着测量抖动Q越小滤波越相信模型估计结果越平滑但也越可能偏离真实状态。6.3 调参时的三条观察曲线调参不是盯着屏幕看热闹我一般看三类指标估计值与真实值的误差曲线如果有真值或者仿真环境看是否收敛、误差是否符合预期P矩阵对角线是否持续递减并趋于稳定如果P在某一步突然增大说明数值出问题了新息序列innovation sequence——即z_k - H * x̂_k^-这个序列应该接近零均值白噪声。如果新息序列出现明显的系统偏差或趋势比如连续多步为正或为负说明模型或参数有问题。第三条尤其有诊断价值。新息有偏往往意味着模型本身的系统误差而不只是噪声参数的问题。这时就算你调Q、R也救不回来该回头检查状态转移矩阵A和控制输入u了。6.4 一个经典调试技巧先仿真再上真机强烈建议任何卡尔曼滤波项目都先在仿真环境里把参数调通再上真实系统。仿真中你掌握真实状态可以精确计算误差能够快速判断参数选择是否合理。很多工程问题在仿真阶段就能暴露出来比如模型建错了、初值给得不合理、Q/R比例荒谬等。直接在真机上调参数被太多不确定因素干扰往往事倍功半。7. 常见问题排查与避坑实录7.1 滤波器发散估计值直接飘飞发散是指估计值和真实值的偏差越来越大最终失去参考意义。最常见的原因有三个Q设得太小模型被过度信任测量修正几乎不起作用R设得太小测量噪声被严重低估滤波器对测量过于敏感估计结果剧烈震荡H矩阵错误比如把观测方程中的状态映射关系写反导致测量信息被错误利用。排查方法是逐项检查先把Q、R设成量级合理的值用仿真验证再单独检查H矩阵把估计出来的状态通过H投影到测量空间和实际测量对比看看量纲和大小是否一致。7.2 滤波结果滞后于真实变化如果你发现估计曲线明显滞后于真实曲线比如机器人转向时位置估计跟不上实际位置大概率是Q太小了。Q太小意味着模型被认为很准确滤波器倾向于相信预测而忽略测量因此对突发的真实变化反应迟钝。适当增大Q可以提升滤波器的响应速度但代价是估计会变得不那么平滑。工程上要在这两者之间找平衡不能只看单一方面。7.3 新息序列不白说明模型有问题如果模型和参数都正确新息序列应当是零均值白噪声。如果发现新息序列连续多步同号或者有明显的周期性这就不是噪声参数问题而是模型本身存在系统偏差。比如A矩阵写错了控制输入u符号反了或者存在你完全没有建模的常值扰动。这种时候调Q、R是徒劳的需要回归物理模型本身。7.4 多传感器融合时的更新顺序问题很多实际系统都有多个传感器比如GPS、IMU、气压计它们的测量频率往往不同。最常见的融合方式是分时更新哪个传感器来数据了就触发一次测量更新不同传感器对应各自的H和R。IMU频率高可能100Hz更新一次GPS频率低可能10Hz更新一次这完全没问题。如果在同一时刻有多个传感器的数据处理方式有两种一种是按顺序依次更新每更新一次就更新一次后验状态和协方差另一种是构造一个组合观测向量把多个观测方程堆叠在一起一次性更新。两种方式在数学上等价但堆叠方式在编程上稍麻烦依次更新更直观。需要注意每用一次测量更新后P矩阵都会缩小所以同一时刻的第二次更新使用的P已经是更新后的了不要误解为一次更新只能用一次测量。7.5 数值稳定性P矩阵失去对称性在实际迭代计算中由于计算机浮点数舍入误差的累积P矩阵会逐渐失去对称性和半正定性最终可能导致滤波崩溃。解决办法有三个层次最简单也最常用每隔几步强制对称化即P (P P^T) / 2使用Joseph形式的协方差更新公式它在数学上比P (I - KH)P^-的数值更稳定使用平方根滤波如UD分解滤波直接对协方差的平方根做递推保证半正定性。工程中用得较多的是前两种方法平方根滤波适合对数值稳定性要求极高的嵌入式环境。7.6 一个容易被忽视的坑dt不一致别忘了状态转移矩阵A里含有采样时间dt。如果代码中的dt和实际传感器采样间隔不一致或者状态更新频率与测量更新频率混淆A矩阵就会失真滤波结果自然不对。一个常见的低级错误是预测阶段用100Hz的dt更新阶段却按10Hz的频率来结果状态转移矩阵里用的是0.01秒还是0.1秒都对不上整个估计就乱了。写过一遍代码的人应该都在这上面摔过跟头。我个人在实际使用中最大的体会是卡尔曼滤波的数学形式很优雅但真正让它在工程里跑得好的是你对背后物理系统的理解。A矩阵建得对不对、Q和R给得符不符合实际、初值设得是否合理这些问题比代码本身更考验功底也决定了滤波效果的上限。如果你刚开始接触卡尔曼滤波强烈建议不要急着调现成库先在Python或者MATLAB里把一个简单的一维例子从头到尾手写一遍每个中间量都打印出来亲眼看着协方差矩阵慢慢收敛、估计值逐步逼近真值。这个过程走通了再看矩阵形式、再上EKF整个思路就会通透明朗。下一篇我接着整理Dr_can这个系列的后半部分内容重点聊聊扩展卡尔曼滤波EKF和无迹卡尔曼滤波UKF的区别、各自适合什么场景以及实际项目中选型的一些思考。