卡尔曼滤波详解——预测更新循环的直觉理解

上篇聊了传感器数据融合的基础——为什么要融合、融合的层次、集中式和分布式架构的区别。讲到融合的具体实现时我提了一嘴卡尔曼滤波说它是传感器融合中最核心的工具。这话真不是吹的。你去面试任何一家做移动机器人的公司面试官问你了解卡尔曼滤波吗这基本是送分题——你要答不上来后面聊SLAM、聊控制理论都没底气。但卡尔曼滤波这个东西教科书上的公式看着吓人网上博客讲得又太抽象。今天这篇我想换个方式——从直觉出发把预测-更新这个循环掰开了讲清楚让你面试的时候能用自己的话讲出来而不是背公式。从一个最蠢的例子说起想象你蒙着眼睛在一片空旷的平地上走路。你只知道两件事你每一步大概走多远但不太准可能走多了也可能走少了有个朋友在旁边看着你时不时告诉你你现在大概在哪个位置但他眼神也不太好你怎么确定自己的位置这就是卡尔曼滤波要解决的核心问题——结合我知道大概会怎样和我看到的测量值得到一个比两者都更准的估计。你的步伐信息就是预测朋友的提示就是更新。卡尔曼滤波做的事情说白了就是这个预测-更新循环的不断重复。预测步我觉得我会到哪每一步预测卡尔曼滤波干的事情就两件事预测状态根据上一步的最优估计和运动模型推算这一步的状态。比如上一步你估计自己在(3, 0)的位置速度是1m/s向东那0.1秒后你大概在(3.1, 0)。预测协方差这个估计有多不确定因为上一步的估计本身就有误差运动模型也不完美所以预测的不确定性一定会比上一步大。协方差矩阵就是描述这种不确定性的。用数学表达就是# 预测步 x_pred F x_prev B u # 状态预测 P_pred F P_prev F.T Q # 协方差预测这里F是状态转移矩阵描述系统怎么从一步变到下一步B u是控制输入比如你的加速度Q是过程噪声协方差模型有多不准。面试的时候面试官问你预测步的物理意义是什么你就说预测步就是拿上一步最好的估计往前推一步。但因为模型不完美、上一步估计也有误差所以预测结果的不确定性一定比上一步大——协方差矩阵变大了。更新步看到测量值后修正预测完了你拿到了一个新的传感器测量值。这个测量值准不准不一定。但它提供了新的信息你应该用它来修正你的预测。更新步也干两件事计算卡尔曼增益这个增益是个权重决定了我到底该更相信预测还是更相信测量。如果预测很准协方差小增益就小更相信预测。如果测量很准测量噪声小增益就大更相信测量。修正状态和协方差用测量值和预测值的差叫新息乘以卡尔曼增益修正预测状态。同时因为融合了新的测量信息不确定性减小了——协方差矩阵变小。# 更新步 K P_pred H.T np.linalg.inv(H P_pred H.T R) # 卡尔曼增益 x_est x_pred K (z - H x_pred) # 状态更新 P_est (np.eye(n) - K H) P_pred # 协方差更新这里H是观测矩阵把状态空间映射到测量空间R是测量噪声协方差z是实际测量值z - H x_pred就是新息。讲真卡尔曼增益的物理意义是面试高频考点。你就记住一句话卡尔曼增益是一个最优权重它在预测的不确定性和测量的不确定性之间做权衡使得最终估计的均方误差最小。为什么叫最优它有啥假设卡尔曼滤波被称为最优估计器但这个最优是有前提的系统是线性的状态转移和观测都必须是线性关系。如果不是线性的就得用扩展卡尔曼滤波EKF或无迹卡尔曼滤波UKF后面两篇会讲。噪声是高斯的过程噪声和测量噪声都服从高斯分布。这个假设在实际中通常近似成立中心极限定理嘛。噪声统计特性已知Q和R矩阵你得提前知道或者调出来。这个在工程上是个大坑——Q和R的调参直接影响滤波效果但很多时候只能靠经验或者反复试验。面试的时候如果面试官问卡尔曼滤波的局限性是什么你就说这三条。然后补充一句实际应用中最大的问题是Q和R的调参以及线性假设在很多系统中不成立。面试官会觉得你真的做过工程。一个完整的Python示例来看一个一维匀速运动跟踪的例子。假设一个目标沿直线匀速运动我们用位置传感器来跟踪它。import numpy as np dt 0.1 # 时间步长 F np.array([[1, dt], [0, 1]]) # 状态转移矩阵 H np.array([[1, 0]]) # 观测矩阵 Q np.array([[0.01, 0], [0, 0.01]]) # 过程噪声 R np.array([[0.5]]) # 测量噪声 x np.array([[0], [1]]) # 初始状态 [位置, 速度] P np.eye(2) # 初始协方差 for z_meas in measurements: # 预测 x_pred F x P_pred F P F.T Q # 更新 S H P_pred H.T R K P_pred H.T np.linalg.inv(S) x x_pred K (np.array([[z_meas]]) - H x_pred) P (np.eye(2) - K H) P_pred这个例子虽然简单但包含了卡尔曼滤波的全部核心逻辑。实际工程中比如机器人定位状态向量会变成[x, y, yaw, vx, vy, omega]六维F和H矩阵会复杂一些但预测-更新的框架完全一样。工程中Q和R怎么调讲真教科书上从不讲这个但这恰恰是工程中最头疼的问题。R测量噪声相对好搞。如果你用的是厂家给的传感器规格书上面通常会写精度比如GPS定位精度±2m你可以把这个精度当作标准差平方一下就得到R。如果条件允许做个静态实验——让传感器静止不动采几百个数据算方差就是R。Q过程噪声就麻烦了。它描述的是你的运动模型有多不准。如果你用匀速模型但实际运动有加速度变化那Q就得大一些来吸收这些未建模的加速度。经验做法是先把Q设小看滤波结果是否跟不上实际轨迹滞后如果滞后了就加大Q。反过来如果滤波结果跳变太厉害说明Q太大了测量噪声的影响太大减小Q。之前做AMR的时候我们在调EKF的Q矩阵上花了整整两周。最后发现与其用一个固定的Q不如根据机器人的运动状态动态调整——直线匀速时Q小一些转弯时Q大一些。这个技巧在面试中可以提会让面试官觉得你有实战经验。面试中怎么聊面试官问卡尔曼滤波按这个思路回答先用蒙眼走路的例子讲直觉预测-更新循环再说数学表达状态转移方程、观测方程、五个核心方程然后说假设条件线性、高斯、已知噪声统计最后说工程实践Q和R怎么调。如果面试官追问卡尔曼增益的推导过程你可以说核心思想是最小化后验估计误差的迹对误差协方差关于K求导令其为零就能解出最优K。具体推导复杂但直觉就是找一个权重让融合后不确定性最小。如果面试官追问卡尔曼滤波和互补滤波的区别你可以说互补滤波用固定权重融合预测和测量卡尔曼滤波的权重是动态调整的——根据当前不确定性自动决定更相信谁。互补滤波计算量小适合资源受限场景但精度不如卡尔曼滤波。如果面试官追问协方差矩阵P的物理意义P矩阵描述状态估计的不确定性。对角线元素是各状态分量的方差非对角线是不同分量间不确定性的关联。预测步P增大更新步P减小稳态下两者平衡P收敛到固定值。面试时候能讲清楚P矩阵的物理意义说明你不只是会套公式而是真正理解了卡尔曼滤波的数学本质。下一篇讲扩展卡尔曼滤波EKF——当系统不是线性的时候怎么把卡尔曼滤波推广到非线性系统。如果这篇文章对你有帮助欢迎点赞、在看、转发三连。 你的支持是我持续更新的最大动力。「机器人软件开发面试·从入门到精通」连载系列上一篇第165篇 传感器数据融合基础——面试翻车实录 下一篇预告第167篇 扩展卡尔曼滤波EKF——非线性系统的状态估计有任何问题欢迎评论区留言我会尽量回复。