1. 从一节课程笔记说起卡尔曼滤波器到底在解决什么问题如果你正在学机器人软件工程或者做自动驾驶、无人机、ROS相关的项目卡尔曼滤波器这个名字你一定绕不过去。我当初学它的时候最大的困惑不是公式看不懂而是不知道它到底在哪儿用、为什么非得用它。后来做项目多了才明白卡尔曼滤波器的本质就一句话在传感器有噪声、系统有误差的情况下尽可能准确地估计出系统真实的状态。举个生活化的例子。你闭着眼睛在房间里走路心里大概知道自己走了几步、朝哪个方向这是“预测”。但你偶尔睁眼瞄一下看到墙的位置这是“观测”。预测和观测都不准但你根据两者的可信程度加权平均一下就能得到一个比单独用任何一个都更靠谱的位置估计。卡尔曼滤波器干的就是这件事只不过它用数学把“加权平均”做到了最优。这篇笔记覆盖的内容从一维卡尔曼滤波器讲起然后扩展到多维最后落到拓展卡尔曼滤波器EKF。这个递进顺序非常合理因为一维情况下所有公式都退化成标量理解起来没有矩阵运算的负担能把核心思想吃透。等你把一维的预测-更新循环搞明白了多维只是把标量换成矩阵EKF则是处理非线性系统的必要手段。适合谁看如果你刚接触机器人状态估计或者上过课但一到写代码就懵这篇内容应该能帮到你。我会把每个公式背后的直觉讲清楚把参数怎么调、代码怎么写、坑在哪里都摊开来说。不是纯理论推导而是“我实际做项目时是怎么理解和使用的”。2. 一维卡尔曼滤波器用最少的数学把核心思想吃透2.1 一维情况下的五个核心公式一维卡尔曼滤波器是整个知识体系的基石。很多人一上来就看多维矩阵公式结果被矩阵求逆、协方差传播搞得晕头转向反而忽略了最本质的东西。我的建议是先把一维的五个公式手推一遍用数值算一遍你就能理解卡尔曼滤波器的灵魂。一维情况下系统模型和观测模型都是标量预测步骤状态预测x_pred A * x_prev B * u协方差预测P_pred A * P_prev * A Q更新步骤卡尔曼增益K P_pred * H / (H * P_pred * H R)状态更新x_new x_pred K * (z - H * x_pred)协方差更新P_new (1 - K * H) * P_pred这里A是状态转移系数一维匀速运动模型里A1B是控制输入系数u是控制量H是观测系数通常为1Q是过程噪声协方差R是观测噪声协方差z是实际观测值。我第一次看到这组公式的时候觉得最反直觉的是卡尔曼增益K。它到底在干什么后来我想明白了K就是一个“信任分配器”。当R很大传感器很吵K就变小滤波器更相信预测当P_pred很大预测很不确定K就变大滤波器更相信观测。这个直觉在多维情况下同样成立只是变成了矩阵形式。2.2 用一个小车例子把公式跑一遍假设有一辆小车在直线上匀速运动状态就是位置。初始位置x0初始不确定度P1。过程噪声Q0.01观测噪声R0.1。第一轮预测x_pred 0P_pred 1 0.01 1.01假设观测到位置z1.2K 1.01 / (1.01 0.1) 0.910x_new 0 0.910 * (1.2 - 0) 1.092P_new (1 - 0.910) * 1.01 0.091可以看到经过一次更新不确定度从1降到了0.091下降非常明显。这就是卡尔曼滤波器“融合”信息的威力。第二轮的时候P_prev已经很小了K会相应变小滤波器会更信任自己的预测。注意Q和R的比值决定了滤波器的“性格”。Q/R越大滤波器越信任观测响应快但抖动大Q/R越小滤波器越信任预测平滑但可能滞后。这个比值没有标准答案要根据你的传感器实际噪声和系统实际动态来调。2.3 一维代码实现与参数调试心得用Python写一维卡尔曼滤波器非常简单核心代码不超过20行class KalmanFilter1D: def __init__(self, A1.0, B0.0, H1.0, Q0.01, R0.1, x00.0, P01.0): self.A A self.B B self.H H self.Q Q self.R R self.x x0 self.P P0 def predict(self, u0.0): self.x self.A * self.x self.B * u self.P self.A * self.P * self.A self.Q def update(self, z): K self.P * self.H / (self.H * self.P * self.H self.R) self.x self.x K * (z - self.H * self.x) self.P (1 - K * self.H) * self.P return self.x, self.P这段代码我用了很多次实测下来很稳。但有几个坑要提醒第一个坑是初始P0的设置。P0设得太小滤波器一开始会过度自信观测值拉不回来P0设得太大前几次更新会剧烈跳动。我的经验是P0设成你初始估计误差的平方比如你估计初始位置误差在1米以内P0就设1。第二个坑是Q和R的量纲。Q的单位是状态量的平方R的单位是观测量的平方。如果状态是位置米观测也是位置米那Q和R单位一致直接比较大小就行。但如果状态是位置观测是速度就要注意H矩阵的量纲转换作用。第三个坑是数值稳定性。一维情况下P_new (1-KH)P_pred这个公式在K接近1的时候会出现浮点精度问题更稳定的写法是P_new P_pred - KHP_pred或者用Joseph形式。不过一维情况下问题不大多维情况下就要注意了。3. 多维卡尔曼滤波器矩阵形式与协方差传播3.1 从标量到矩阵的思维跃迁一维到多维最大的变化不是公式变复杂了而是你要开始用“协方差矩阵”来思考问题。一维情况下P是一个数表示不确定度的大小。多维情况下P是一个矩阵对角线元素表示各个状态分量的方差非对角线元素表示状态分量之间的相关性。这个“相关性”非常关键。举个例子如果你估计一个物体的位置和速度位置估计偏大时速度估计往往偏小因为观测到的位移是两者共同作用的结果这种负相关就体现在P矩阵的非对角线元素上。卡尔曼滤波器在更新时会自动利用这种相关性让位置和速度的修正互相影响最终得到比单独修正每个分量更优的结果。多维卡尔曼滤波器的五个公式形式上是预测x_pred F * x_prev B * uP_pred F * P_prev * F^T Q更新K P_pred * H^T * (H * P_pred * H^T R)^(-1)x_new x_pred K * (z - H * x_pred)P_new (I - K * H) * P_pred注意这里F就是状态转移矩阵对应一维的A。H是观测矩阵把状态空间映射到观测空间。R是观测噪声协方差矩阵Q是过程噪声协方差矩阵。3.2 状态转移矩阵F和观测矩阵H的构建方法F矩阵的构建是很多人卡住的地方。我的经验是F矩阵的每一行描述一个状态分量如何从上一时刻的状态演化而来。以匀速直线运动为例状态向量是[x, v]^T位置和速度。位置的变化等于位置加上速度乘以时间间隔dt速度不变。所以F [[1, dt], [0, 1]]如果加上加速度a状态变成[x, v, a]^T假设加速度恒定那么F [[1, dt, 0.5*dt^2], [0, 1, dt], [0, 0, 1]]这个0.5dt^2来自位置对加速度的积分。我一开始总是记不住这个系数后来想明白了加速度对位置的贡献是二次的所以是0.5dt^2。H矩阵的构建相对简单H矩阵的每一行描述一个观测值如何由状态分量线性组合而成。如果只观测位置H [1, 0]如果同时观测位置和速度H [[1,0],[0,1]]。提示F和H的维度必须匹配。如果状态是n维观测是m维那么F是n×nH是m×nQ是n×nR是m×mK是n×m。3.3 过程噪声Q和观测噪声R的物理意义与调参Q和R是多维卡尔曼滤波器调参的核心。很多人直接把Q设成单位矩阵乘以一个小数R设成单位矩阵乘以另一个小数然后就开始调。这样做能跑但不够好。Q矩阵的物理意义是“模型不准确的程度”。如果你的运动模型是匀速直线但实际物体有随机加速度那么Q就应该反映这个加速度的方差。对于[x, v]状态常见的Q矩阵形式是Q q * [[0.25dt^4, 0.5dt^3], [0.5*dt^3, dt^2]]这个形式来自“加速度是白噪声”的假设q是加速度的功率谱密度。我实测下来这种Q矩阵比简单设对角矩阵效果好很多因为它正确反映了位置和速度噪声的相关性。R矩阵的物理意义是“传感器有多吵”。如果你用的是GPSR大概在几米到十几米如果是激光雷达R可能在几厘米。R的获取方法有两种一是查传感器手册二是静止状态下采集一批数据算方差。我一般用第二种更准。调参的时候我习惯先固定R因为R有物理依据然后调Q。Q调大滤波器响应快但噪声大Q调小平滑但可能跟不上真实变化。一个实用的技巧是先让滤波器在已知真值的数据上跑看估计误差的均方根然后微调Q使误差最小。3.4 多维实现中的矩阵运算陷阱多维卡尔曼滤波器用Python实现NumPy是标配。但有几个矩阵运算的坑我踩过第一个坑是矩阵求逆。K的计算里有一项(HPH^T R)^(-1)这个矩阵是m×m的m是观测维度。如果m很大求逆会很慢。但实际中m通常很小1到10所以问题不大。真正要注意的是这个矩阵可能接近奇异导致求逆不稳定。解决办法是用np.linalg.solve代替np.linalg.inv或者加一个很小的对角项。第二个坑是协方差矩阵的对称性。理论上P应该始终是对称的但浮点运算会让它慢慢变得不对称。我一般每次更新后强制P (P P.T) / 2这样能保持对称性避免数值问题。第三个坑是维度顺序。F、H、Q、R的维度必须严格匹配一旦搞错NumPy会广播出奇怪的结果而不是报错。我建议在代码里加assert检查维度比如assert F.shape (n, n)。4. 拓展卡尔曼滤波器EKF当系统不再线性4.1 为什么需要EKF非线性系统的挑战前面讲的卡尔曼滤波器有一个隐含假设系统模型和观测模型都是线性的。但现实世界几乎都是非线性的。比如你用一个雷达观测目标的距离和角度目标在笛卡尔坐标系下做匀速运动那么观测方程就是非线性的涉及sin和cos。再比如机器人用轮速计和陀螺仪做航迹推算角度积分也是非线性的。EKF的思路很直接既然非线性函数不好处理那就在当前估计点附近把它线性化。线性化的工具就是泰勒展开取一阶项。具体来说用雅可比矩阵代替原来的F和H矩阵。雅可比矩阵是什么简单说如果有一个非线性函数f(x)它的雅可比矩阵就是f对x的偏导数矩阵。对于向量函数雅可比矩阵的第i行第j列就是第i个输出对第j个输入的偏导数。EKF的五个公式变成预测x_pred f(x_prev, u)P_pred F_j * P_prev * F_j^T Q更新K P_pred * H_j^T * (H_j * P_pred * H_j^T R)^(-1)x_new x_pred K * (z - h(x_pred))P_new (I - K * H_j) * P_pred注意f和h是非线性函数F_j和H_j是它们在当前估计点的雅可比矩阵。这个“在当前估计点线性化”是EKF的核心也是它最大的局限性——如果非线性很强一阶近似误差会很大滤波器可能发散。4.2 雅可比矩阵的推导与代码实现雅可比矩阵的推导是EKF最烦人的部分但也是最不能偷懒的部分。我以雷达观测为例推导一遍。状态向量x [px, py, vx, vy]^T观测向量z [r, theta]^T其中r是距离theta是角度。观测函数h(x)r sqrt(px^2 py^2)theta atan2(py, px)雅可比矩阵H_j是2×4的H_j [[px/r, py/r, 0, 0], [-py/r^2, px/r^2, 0, 0]]推导过程r对px的偏导是px/r对py的偏导是py/r对vx和vy的偏导是0。theta对px的偏导是-py/(px^2py^2) -py/r^2对py的偏导是px/r^2。代码实现import numpy as np def h(x): px, py, vx, vy x r np.sqrt(px**2 py**2) theta np.arctan2(py, px) return np.array([r, theta]) def H_jacobian(x): px, py, vx, vy x r np.sqrt(px**2 py**2) r2 r**2 H np.array([[px/r, py/r, 0, 0], [-py/r2, px/r2, 0, 0]]) return H注意当r接近0的时候雅可比矩阵会出现除零问题。实际使用中要加一个极小值保护比如r max(r, 1e-6)。这个坑我在第一次做雷达跟踪的时候踩过目标靠近原点时滤波器直接输出了NaN。4.3 EKF的局限性线性化误差与发散问题EKF不是万能的。它的核心问题是线性化只在估计点附近有效如果真实状态离估计点很远线性化误差会很大导致滤波器发散。我遇到过的典型发散场景有两种第一种是初始估计误差太大。比如目标真实位置在(100, 100)但你初始估计在(0, 0)这时候雅可比矩阵在(0,0)处计算和真实位置处的雅可比矩阵差别巨大滤波器可能永远追不上。解决办法是做好初始化或者用无迹卡尔曼滤波器UKF代替。第二种是观测更新频率太低。如果两次观测之间隔了很久预测步骤的协方差P会变得很大这时候线性化点可能已经偏离真实状态很远了。解决办法是提高观测频率或者在预测步骤之间做多次小步预测。还有一个不太被提及但很重要的点*EKF的协方差更新公式P_new (I - K*H_j)P_pred在数值上不稳定。更好的写法是Joseph形式P_new (I - KH_j)P_pred(I - KH_j)^T KRK^T。这个形式能保证P始终正定我实测下来在高维情况下差别很明显。4.4 EKF在机器人状态估计中的典型应用场景EKF在机器人领域用得非常多我列举几个典型场景场景一IMU和轮速计融合。轮速计给出线速度IMU给出角速度和加速度。状态向量是[x, y, theta, v, omega]运动模型是非线性的因为theta的积分涉及旋转矩阵观测模型也是非线性的。EKF在这里几乎是标配。场景二视觉惯性里程计。相机给出特征点位置IMU给出加速度和角速度。状态向量可能包含几十个维度相机位姿、速度、偏置、特征点深度EKF的计算量是O(n^2)n太大就跑不动了。所以实际中常用滑动窗口或者MSCKF多状态约束卡尔曼滤波来降低维度。场景三目标跟踪。雷达或激光雷达观测目标状态是目标的位置和速度。观测模型是距离和角度非线性。EKF在这里的表现取决于目标机动性如果目标做剧烈机动EKF可能跟不上需要调大Q。提示EKF的状态维度不要超过20。超过20之后协方差矩阵的传播和求逆会变得很慢而且数值稳定性会下降。如果状态维度确实很高考虑用因子图优化或者UKF。5. 从理论到代码一个完整的EKF实现与调试记录5.1 项目背景与状态向量定义我拿一个模拟项目来说一个地面机器人做二维平面运动传感器有轮速计测线速度和陀螺仪测角速度另外有一个外部系统偶尔给出绝对位置观测。状态向量定义为[x, y, theta, v, omega]^T共5维。运动模型非线性x_new x v * cos(theta) * dty_new y v * sin(theta) * dttheta_new theta omega * dtv_new vomega_new omega观测模型轮速计观测vH_v [0, 0, 0, 1, 0]陀螺仪观测omegaH_omega [0, 0, 0, 0, 1]位置观测x和yH_pos [[1,0,0,0,0],[0,1,0,0,0]]5.2 雅可比矩阵的完整推导运动模型f对状态x的雅可比矩阵F_j是5×5F_j [[1, 0, -v*sin(theta)*dt, cos(theta)dt, 0], [0, 1, vcos(theta)*dt, sin(theta)*dt, 0], [0, 0, 1, 0, dt], [0, 0, 0, 1, 0], [0, 0, 0, 0, 1]]这个矩阵的推导关键是theta对x和y的偏导为0x对theta的偏导是-v*sin(theta)dty对theta的偏导是vcos(theta)*dt。其他项都是常数或者0。观测模型的雅可比矩阵很简单因为观测都是线性的H_j就是上面写的H_v、H_omega、H_pos。5.3 完整代码实现与逐行注释import numpy as np class EKF: def __init__(self, dt0.1): self.dt dt self.n 5 # 状态维度 self.x np.zeros(5) # 初始状态 self.P np.eye(5) * 1.0 # 初始协方差 self.Q np.diag([0.01, 0.01, 0.001, 0.1, 0.1]) # 过程噪声 self.R_v 0.1 # 轮速计噪声 self.R_omega 0.05 # 陀螺仪噪声 self.R_pos np.diag([0.5, 0.5]) # 位置观测噪声 def predict(self): x, y, theta, v, omega self.x dt self.dt # 非线性状态预测 self.x np.array([ x v * np.cos(theta) * dt, y v * np.sin(theta) * dt, theta omega * dt, v, omega ]) # 雅可比矩阵 F_j np.array([ [1, 0, -v*np.sin(theta)*dt, np.cos(theta)*dt, 0], [0, 1, v*np.cos(theta)*dt, np.sin(theta)*dt, 0], [0, 0, 1, 0, dt], [0, 0, 0, 1, 0], [0, 0, 0, 0, 1] ]) self.P F_j self.P F_j.T self.Q def update_velocity(self, z_v): H np.array([[0, 0, 0, 1, 0]]) R np.array([[self.R_v]]) self._update(z_v, H, R) def update_omega(self, z_omega): H np.array([[0, 0, 0, 0, 1]]) R np.array([[self.R_omega]]) self._update(z_omega, H, R) def update_position(self, z_pos): H np.array([[1, 0, 0, 0, 0], [0, 1, 0, 0, 0]]) R self.R_pos self._update(z_pos, H, R) def _update(self, z, H, R): z np.atleast_1d(z) y z - H self.x # 新息 S H self.P H.T R # 新息协方差 K self.P H.T np.linalg.inv(S) # 卡尔曼增益 self.x self.x K y # Joseph形式更新协方差 I np.eye(self.n) self.P (I - K H) self.P (I - K H).T K R K.T # 强制对称 self.P (self.P self.P.T) / 2这段代码我调试了大概两天主要时间花在雅可比矩阵的符号上。第一次写的时候把theta对x的偏导符号搞反了结果滤波器一开始还能跑跑了几十步之后位置估计直接飞了。后来用数值差分验证了雅可比矩阵才找到问题。5.4 调试过程中遇到的三个典型问题问题一滤波器发散位置估计越来越大。排查后发现是F_j矩阵中theta对x的偏导符号错了。数值差分验证方法用(f(xeps) - f(x-eps)) / (2*eps)计算数值雅可比和解析雅可比对比。这个方法我强烈推荐能省很多调试时间。问题二协方差矩阵出现负特征值。这是因为用了简单的P_new (I - K*H)*P_pred公式数值误差累积导致P不再正定。换成Joseph形式后问题消失。这个坑在多维情况下很常见一维情况下不明显。问题三角度跳变导致滤波器抖动。theta在-pi到pi之间跳变当真实角度接近pi时观测值可能突然从pi跳到-pi新息y会变成2pi导致滤波器剧烈修正。解决办法是在计算新息时做角度归一化y[2] (y[2] pi) % (2*pi) - pi。这个技巧在处理角度时非常实用。6. 常见问题与排查技巧实录6.1 卡尔曼滤波器调参速查表现象可能原因调整方向估计滞后于真实值Q太小过度信任预测增大Q估计抖动大R太小过度信任观测增大R滤波器发散模型非线性太强或初始误差大检查雅可比增大P0考虑UKF协方差矩阵非正定数值误差累积用Joseph形式强制对称角度估计跳变角度周期性问题新息做角度归一化计算太慢状态维度太高降维或改用因子图优化6.2 我踩过的五个坑与解决方案坑一Q和R的单位不匹配。有一次状态是位置米观测是像素我直接把R设成像素方差Q设成米方差结果滤波器完全不工作。后来把观测通过标定矩阵转换到米问题解决。教训是所有量纲必须统一到状态空间。坑二初始协方差P0设得太小。P0设成0.001结果滤波器前几十步完全不理会观测因为K太小了。后来改成1.0正常了。P0应该反映你初始估计的真实不确定度不要拍脑袋设。坑三观测更新顺序影响结果。当有多个观测源时先更新哪个后更新哪个会影响最终结果。理论上如果都是线性观测顺序不影响但EKF中因为线性化点不同顺序会有影响。我的做法是按观测精度从高到低更新精度高的先更新这样线性化点更准。坑四忘记做时间同步。传感器数据时间戳不一致直接用会导致预测和更新错位。解决办法是用时间戳对齐或者用插值。这个坑在实车上特别常见。坑五在EKF中用了太大的dt。dt太大线性化误差就大滤波器容易发散。我的经验是dt不要超过0.1秒如果传感器频率低中间做多次小步预测。6.3 数值稳定性技巧汇总数值稳定性是卡尔曼滤波器从理论到工程必须跨过的坎。我总结了几条实用技巧协方差更新用Joseph形式保证正定性每次更新后强制P对称P (P P.T) / 2矩阵求逆用np.linalg.solve而不是np.linalg.inv在矩阵求逆前加一个极小对角项S np.eye(m) * 1e-9用平方根滤波SR-UKF代替标准EKF数值稳定性更好但实现复杂定期检查P的特征值如果有负特征值说明数值问题已经严重了提示如果你的EKF在仿真里跑得好好的一到实车就发散大概率是数值稳定性问题。先检查P是否正定再检查雅可比矩阵是否正确。7. 从EKF再往前走什么时候该换方法EKF用了几年之后我逐渐摸清了它的边界。如果你的系统非线性很强比如无人机做大幅机动、机械臂做快速旋转EKF的一阶线性化误差会大到无法接受。这时候有几个选择无迹卡尔曼滤波器UKF用sigma点传播代替线性化能捕捉到二阶非线性。实现比EKF复杂但不需要推导雅可比矩阵对于复杂模型反而更省事。我实测下来UKF在强非线性场景下比EKF稳定很多。粒子滤波器用一堆粒子表示概率分布能处理任意非线性甚至非高斯噪声。但计算量大状态维度不能太高一般不超过10维。适合低维强非线性场景。因子图优化把状态估计问题转化成优化问题用滑动窗口或者增量式求解。精度比滤波方法高但计算量也大。适合有回环检测的场景比如SLAM。我的建议是先用EKF如果发散或者精度不够再考虑UKF。如果UKF也不行再上因子图。不要一上来就用最复杂的方法工程上够用就好。最后分享一个我常用的EKF调试流程先在仿真里用已知真值跑看估计误差是否收敛然后加入真实噪声模型看是否稳定最后上实车从低速小范围运动开始逐步加大运动幅度。每一步都记录数据出问题了好回溯。这个流程帮我省了很多现场调试的时间。