卡尔曼滤波：让不确定的数据“融合”出真相

引言在现实生活中我们往往无法直接获得一个系统状态的精确值。例如无人驾驶汽车需要知道自己的实时位置——GPS信号有噪声惯性传感器的读数会漂移轮速计在打滑时失效。单独依赖任何一个传感器都会得到不完美的结果。有没有一种方法可以把这些“不准”的数据融合起来得到一个“更准”的估计卡尔曼滤波Kalman Filter正是这样一个优雅的数学工具。它由鲁道夫·卡尔曼在1960年提出通过预测—更新两个步骤的迭代利用系统的动态模型和带有噪声的观测递归地给出最优状态估计在噪声服从高斯分布、系统线性的假设下它是最小均方误差意义上的最优估计。如果把单次测量比作“雾里看花”把模型预测比作“刻舟求剑”那么卡尔曼滤波就是“让预测和观测互相纠错像炼金术士一样炼出最纯的金子”。前置知识状态空间模型用状态向量 xkxk​ 描述系统在时刻 kk 的“真实情况”如位置、速度。状态随时间演化并且我们能通过观测 zkzk​ 间接感知它。线性高斯系统状态转移方程xkFkxk−1Bkukwkxk​Fk​xk−1​Bk​uk​wk​其中 wk∼N(0,Qk)wk​∼N(0,Qk​) 是过程噪声。观测方程zkHkxkvkzk​Hk​xk​vk​其中 vk∼N(0,Rk)vk​∼N(0,Rk​) 是观测噪声。所有噪声均为零均值高斯白噪声且互不相关。协方差矩阵描述状态各分量之间不确定度的相关性。例如 PkCov(xk)Pk​Cov(xk​)。贝叶斯滤波卡尔曼滤波是贝叶斯在线滤波在高斯线性情况下的闭式解。矩阵运算基础转置、求逆、矩阵乘法和加法。核心思想卡尔曼滤波采用递归方式每个时刻它先根据上一时刻的估计值预测当前时刻的状态先验估计然后利用当前时刻的观测值更新这个预测后验估计。更新时按照卡尔曼增益来平衡预测的不确定度和观测的不确定度——谁的协方差小谁就更“可信”。可以用一个生活中的例子来理解你要估计房间的温度。你有一个温度模型比如关窗后温度变化很小这是预测。你还有一个温度计但读数有误差这是观测。卡尔曼滤波会结合两者如果温度计很准观测噪声小就更相信它如果模型很准过程噪声小就更相信预测。而且它会动态地调整这种信任程度。数学本质卡尔曼滤波推导出后验估计的均值和协方差的解析解结果就是先验估计 增益 × (观测 - 期望观测)其中增益由协方差计算得出。整个过程不需要保留历史数据仅需上一时刻的状态和协方差因此极其适合实时应用。卡尔曼滤波算法步骤标准形式假设系统是离散时间、线性的。每个周期包括两个阶段1. 预测阶段基于上一时刻的后验状态先验估计x^k−Fkx^k−1Bkukx^k−​Fk​x^k−1​Bk​uk​其中 x^k−1x^k−1​ 是上一时刻的后验估计ukuk​ 是控制输入如驱动力。先验协方差Pk−FkPk−1FkTQkPk−​Fk​Pk−1​FkT​Qk​这一步将上一时刻的不确定度传播到当前并加上过程噪声 QkQk​ 带来的新增不确定度。2. 更新阶段融合当前观测卡尔曼增益KkPk−HkT(HkPk−HkTRk)−1Kk​Pk−​HkT​(Hk​Pk−​HkT​Rk​)−1增益决定了观测对估计的修正程度。当观测噪声 RkRk​ 很大时KkKk​ 变小更相信预测当 Pk−Pk−​ 很大时KkKk​ 变大更相信观测。状态后验估计x^kx^k−Kk(zk−Hkx^k−)x^k​x^k−​Kk​(zk​−Hk​x^k−​)括号内称为新息innovation或残差即实际观测与预期观测之差。后验协方差更新两种等价形式Pk(I−KkHk)Pk−Pk​(I−Kk​Hk​)Pk−​这一步量化了融合后剩余的不确定性。以上公式中所有矩阵在 kk 时刻均可已知Fk,Bk,Hk,Qk,RkFk​,Bk​,Hk​,Qk​,Rk​ 可能是时变的但通常很多应用中是常数。算法流程图解text------------------- ------------------- | 上一时刻后验估计 | | 系统输入 | | x_hat_{k-1}, P_{k-1} | | u_k | ------------------- ------------------- │ │ ↓ ↓ ┌─────────────────────────────────────┐ │ 预测 │ │ x_hat_k^- F_k x_hat_{k-1} B_k u_k│ │ P_k^- F_k P_{k-1} F_k^T Q_k │ └─────────────────────────────────────┘ │ ↓ ┌─────────────────────────────────────┐ │ 等待观测 z_k │ └─────────────────────────────────────┘ │ ↓ ┌─────────────────────────────────────┐ │ 更新 │ │ K_k P_k^- H_k^T (H_k P_k^- H_k^T R_k)^{-1} │ │ x_hat_k x_hat_k^- K_k(z_k - H_k x_hat_k^-) │ │ P_k (I - K_k H_k) P_k^- │ └─────────────────────────────────────┘ │ ↓ ------------------- | 当前时刻后验估计 | | x_hat_k, P_k | ------------------- │ └──────────→ 下一时刻性质与关键理解性质说明最优性在系统为线性且噪声为高斯白噪声的条件下卡尔曼滤波提供最小均方误差MMSE估计等价于贝叶斯后验均值。递归只需保存上一时刻的状态和协方差计算量恒定适合嵌入式实时系统。无偏性如果初始估计无偏则所有后续估计都是无偏的E[x^k]E[xk]E[x^k​]E[xk​]。协方差收敛对于时不变系统固定F, H, Q, R协方差矩阵 PkPk​ 会收敛到一个稳态值此时卡尔曼增益也收敛。鲁棒性即便噪声不是严格高斯卡尔曼滤波往往也是线性系统下性能优良的次优滤波器。可观测性系统必须可观即通过观测能唯一确定状态否则滤波发散。关于卡尔曼增益的直观当观测噪声协方差 RR 很大观测不可信时KK 很小依赖预测。当状态先验协方差 P−P− 很大预测很迷茫时KK 很大更相信观测。当状态维度高时矩阵求逆HP−HTRHP−HTR 的逆是主要计算开销。拓展与变体扩展卡尔曼滤波EKF用于非线性系统。将状态转移函数和观测函数进行一阶泰勒展开线性化然后套用标准卡尔曼滤波框架。广泛用于无人机、机器人SLAM。无迹卡尔曼滤波UKF使用Sigma点近似非线性分布比EKF精度更高无需计算雅可比矩阵。容积卡尔曼滤波CKF基于球面径向积分规则高维系统性能更优。信息滤波协方差矩阵的逆信息矩阵递推适合多传感器融合。自适应卡尔曼滤波在线估计过程噪声 QQ 和观测噪声 RR应对时变环境。粒子滤波非高斯、强非线性场景下用随机采样的粒子近似后验分布替代卡尔曼滤波。常见误区澄清误区1卡尔曼滤波要求系统是线性的。→ 正确标准卡尔曼滤波确实要求线性但EKF/UKF能处理弱非线性。误区2卡尔曼滤波只能处理平稳过程。→ 不它可以跟踪时变系统只要模型参数 Fk,HkFk​,Hk​ 能随时间变化。误区3过程噪声和观测噪声必须已知且精确。→ 实际工程中往往需要调参tuning通过实验选择合理的 QQ 和 RR。误区4卡尔曼滤波的初值不重要。→ 实际上初值 x^0x^0​ 和 P0P0​ 会影响前期收敛速度但对稳态性能影响很小。总结卡尔曼滤波是将模型预测与数据观测进行最优融合的数学框架。它用优雅的递归公式将充满噪声的传感器数据和动力学模型相结合实时给出真实状态的最佳估计。虽然其背后的数学涉及概率论和矩阵运算但其核心思想极其朴素相信模型还是相信数据——看谁更确定。