★ FEATURED ARTICLE 1. 从一个朴素问题说起为什么需要卡尔曼滤波如果你做过传感器相关的项目大概率遇到过这样的场景手上有一个温度传感器读数一直在跳明明环境温度没变数值却在正负零点几度之间来回晃。你想着取个平均值吧结果响应又变慢了温度真变化的时候半天跟不上。再比如做小车定位编码器和IMU各说各话一个说走了1.2米另一个说走了1.35米你信谁这类问题的本质是我们手上有多个来源的信息每个都不完美怎么融合出一个最优估计卡尔曼滤波就是干这个的。它不是什么黑魔法本质上是一套“预测-校正”的递推算法用数学的方式告诉你在当前不确定性的情况下怎样组合预测值和观测值才能让估计结果的误差最小。我最初接触卡尔曼滤波是在一个姿态解算的小项目里当时用MPU6050读加速度和角速度加速度计噪声大但长期不漂陀螺仪短期准但积分会漂。试过互补滤波效果凑合但参数靠凑。后来换成卡尔曼虽然一开始被那一堆矩阵公式劝退但真正把代码跑通之后发现它的逻辑其实非常清晰而且可解释性极强。这篇内容适合谁看如果你正在学卡尔曼滤波被公式卡住不知道代码怎么写或者你已经会调库但不知道内部在干什么又或者你做过一些滤波但想系统梳理一遍线性卡尔曼的完整推导和实现那这篇应该能帮到你。我会从贝叶斯滤波的视角切入把线性卡尔曼滤波的来龙去脉讲清楚然后给出可以直接跑的Python实现最后分享一些我在实际项目中踩过的坑。2. 贝叶斯滤波框架卡尔曼滤波的“母体”2.1 从概率角度看状态估计卡尔曼滤波不是凭空冒出来的它是贝叶斯滤波在线性高斯系统下的一个特例。理解这一点非常关键因为很多人学卡尔曼的时候直接背五个公式背完也不知道为什么长那样。但如果你先理解贝叶斯滤波的通用框架再看卡尔曼就会发现那五个公式是自然而然的推导结果。贝叶斯滤波的核心思想可以用一句话概括用概率分布来描述状态的不确定性然后通过观测不断更新这个分布。具体来说我们有一个系统状态 $x$它随时间演化我们无法直接看到它只能通过观测 $z$ 来推断。贝叶斯滤波分两步走第一步是预测。根据系统模型从上一时刻的状态分布推出当前时刻的先验分布。用概率语言说就是$$p(x_k | z_{1:k-1}) \int p(x_k | x_{k-1}) \cdot p(x_{k-1} | z_{1:k-1}) , dx_{k-1}$$这个积分的意思是当前状态 $x_k$ 取决于上一时刻状态 $x_{k-1}$而我们对 $x_{k-1}$ 的认知是一个分布所以要把所有可能的 $x_{k-1}$ 都考虑进来加权求和。第二步是更新。拿到当前时刻的观测 $z_k$ 后用贝叶斯公式把先验分布修正为后验分布$$p(x_k | z_{1:k}) \frac{p(z_k | x_k) \cdot p(x_k | z_{1:k-1})}{p(z_k | z_{1:k-1})}$$其中 $p(z_k | x_k)$ 是观测似然表示“如果真实状态是 $x_k$观测到 $z_k$ 的概率有多大”。分母是一个归一化常数保证后验分布积分为1。这个框架在理论上很完美但实际用起来有个大问题那个积分一般情况下算不出来除非概率分布有特殊形式。而高斯分布恰好有这个好性质——高斯分布经过线性变换和乘积之后仍然是高斯分布。这就是卡尔曼滤波成立的数学基础。2.2 高斯假设带来的简化高斯分布有两个关键性质让贝叶斯滤波变得可计算性质一线性变换保持高斯性。如果 $x \sim \mathcal{N}(\mu, \Sigma)$那么 $Ax b \sim \mathcal{N}(A\mu b, A\Sigma A^T)$。这意味着预测步骤中如果上一时刻的后验是高斯的系统模型是线性的那么先验也是高斯的。性质二高斯乘积仍是高斯。两个高斯分布相乘结果还是高斯分布而且有闭式解。这意味着更新步骤中先验和似然都是高斯的后验也是高斯的。有了这两个性质我们就不需要维护整个概率分布了只需要维护均值和协方差两个量。卡尔曼滤波的五个公式本质上就是在高斯假设下把贝叶斯滤波的预测和更新两步写成均值和协方差的递推形式。我个人的理解是卡尔曼滤波 贝叶斯滤波 高斯假设 线性模型。这三个条件缺一不可。如果系统是非线性的那就得用扩展卡尔曼或者无迹卡尔曼如果噪声是非高斯的那就得用粒子滤波之类的办法。但线性高斯是最常见也最容易处理的情况所以线性卡尔曼滤波是入门的第一站。2.3 线性系统的状态空间模型在正式推导之前先把符号约定理清楚。线性卡尔曼滤波假设系统可以用以下状态空间模型描述$$x_k F_k x_{k-1} B_k u_k w_k$$$$z_k H_k x_k v_k$$其中$x_k \in \mathbb{R}^n$ 是 $k$ 时刻的系统状态向量$F_k \in \mathbb{R}^{n \times n}$ 是状态转移矩阵描述状态如何从上一时刻演化到当前时刻$B_k \in \mathbb{R}^{n \times m}$ 是控制输入矩阵$u_k \in \mathbb{R}^m$ 是控制输入向量$w_k \sim \mathcal{N}(0, Q_k)$ 是过程噪声服从零均值高斯分布协方差矩阵为 $Q_k$$z_k \in \mathbb{R}^p$ 是观测向量$H_k \in \mathbb{R}^{p \times n}$ 是观测矩阵描述状态如何映射到观测$v_k \sim \mathcal{N}(0, R_k)$ 是观测噪声协方差矩阵为 $R_k$这里有几个关键假设需要强调过程噪声和观测噪声都是零均值高斯白噪声且两者相互独立也与初始状态独立。这些假设在实际项目中不一定严格成立但很多情况下近似成立卡尔曼滤波依然能给出不错的结果。注意$Q$ 和 $R$ 的设定是卡尔曼滤波调参的核心。$Q$ 大表示你不太信任系统模型$R$ 大表示你不太信任观测。这两个矩阵的比值决定了滤波器的“性格”——是更相信预测还是更相信观测。3. 五个公式的推导从贝叶斯到卡尔曼3.1 预测步骤均值和协方差怎么传假设 $k-1$ 时刻的后验分布已知为 $x_{k-1} | z_{1:k-1} \sim \mathcal{N}(\hat{x}{k-1|k-1}, P{k-1|k-1})$。现在要预测 $k$ 时刻的先验分布。根据状态方程 $x_k F_k x_{k-1} B_k u_k w_k$对两边取期望$$\hat{x}{k|k-1} F_k \hat{x}{k-1|k-1} B_k u_k$$这一步很直观预测均值就是上一时刻均值经过状态转移再加上控制输入的影响。协方差的推导稍微绕一点。根据协方差的定义和线性变换性质$$P_{k|k-1} \text{Cov}(x_k) \text{Cov}(F_k x_{k-1} B_k u_k w_k)$$由于 $B_k u_k$ 是确定性的不影响协方差而 $w_k$ 与 $x_{k-1}$ 独立所以$$P_{k|k-1} F_k P_{k-1|k-1} F_k^T Q_k$$这就是预测步骤的两个公式。物理意义很清晰协方差经过状态转移矩阵“传播”后再加上过程噪声的贡献。如果状态转移矩阵 $F$ 的某些方向会放大不确定性那 $F P F^T$ 就会相应增大。3.2 更新步骤用观测修正预测更新步骤是卡尔曼滤波最精妙的部分。我们有一个先验 $x_k | z_{1:k-1} \sim \mathcal{N}(\hat{x}{k|k-1}, P{k|k-1})$还有一个观测 $z_k H_k x_k v_k$。现在要把两者融合。观测似然可以写成 $z_k | x_k \sim \mathcal{N}(H_k x_k, R_k)$。根据贝叶斯公式后验正比于先验乘以似然$$p(x_k | z_k) \propto \mathcal{N}(x_k; \hat{x}{k|k-1}, P{k|k-1}) \cdot \mathcal{N}(z_k; H_k x_k, R_k)$$两个高斯相乘结果仍然是高斯。经过一番代数运算这里省略中间步骤核心是配方法可以得到后验的均值和协方差$$\hat{x}{k|k} \hat{x}{k|k-1} K_k (z_k - H_k \hat{x}_{k|k-1})$$$$P_{k|k} (I - K_k H_k) P_{k|k-1}$$其中 $K_k$ 就是卡尔曼增益$$K_k P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T R_k)^{-1}$$这三个公式加上前面的两个预测公式就是经典的卡尔曼滤波五公式。3.3 卡尔曼增益的直观理解卡尔曼增益 $K_k$ 的维度是 $n \times p$它决定了观测残差 $(z_k - H_k \hat{x}_{k|k-1})$ 对状态修正的权重。理解 $K_k$ 的行为对调参至关重要。当 $R_k \to 0$ 时表示观测非常准确$K_k \to H_k^{-1}$如果 $H$ 可逆此时后验几乎完全信任观测。当 $P_{k|k-1} \to 0$ 时表示预测非常准确$K_k \to 0$此时后验几乎完全信任预测。更一般地$K_k$ 是在预测不确定性和观测不确定性之间做一个“加权平均”。预测协方差 $P_{k|k-1}$ 越大增益越大越信任观测观测噪声协方差 $R_k$ 越大增益越小越信任预测。这个权衡是自动完成的不需要手动干预这也是卡尔曼滤波相比固定权重的互补滤波的优势所在。实操心得很多人调卡尔曼的时候喜欢手动改 $K$这是不对的。$K$ 是自动计算的你真正应该调的是 $Q$ 和 $R$。把 $Q$ 调大滤波器会更信任观测把 $R$ 调大滤波器会更信任模型。4. 用Python实现一个线性卡尔曼滤波器4.1 代码结构设计网上很多卡尔曼滤波的示例代码写得比较随意变量命名混乱矩阵维度也不检查。我在实际项目中习惯把卡尔曼滤波器封装成一个类这样复用起来方便也便于调试。下面是我常用的一个实现框架import numpy as np class LinearKalmanFilter: def __init__(self, F, H, Q, R, BNone, x0None, P0None): F: 状态转移矩阵 (n x n) H: 观测矩阵 (p x n) Q: 过程噪声协方差 (n x n) R: 观测噪声协方差 (p x p) B: 控制输入矩阵 (n x m)可选 x0: 初始状态估计 (n,) P0: 初始协方差 (n x n) self.F np.array(F, dtypefloat) self.H np.array(H, dtypefloat) self.Q np.array(Q, dtypefloat) self.R np.array(R, dtypefloat) self.B np.array(B, dtypefloat) if B is not None else None self.n self.F.shape[0] self.p self.H.shape[0] self.x np.zeros(self.n) if x0 is None else np.array(x0, dtypefloat) self.P np.eye(self.n) if P0 is None else np.array(P0, dtypefloat) def predict(self, uNone): # 状态预测 self.x self.F self.x if self.B is not None and u is not None: self.x self.B np.array(u, dtypefloat) # 协方差预测 self.P self.F self.P self.F.T self.Q return self.x, self.P def update(self, z): z np.array(z, dtypefloat) # 计算卡尔曼增益 S self.H self.P self.H.T self.R K self.P self.H.T np.linalg.inv(S) # 状态更新 y z - self.H self.x # 新息 self.x self.x K y # 协方差更新Joseph形式数值更稳定 I np.eye(self.n) self.P (I - K self.H) self.P (I - K self.H).T K self.R K.T return self.x, self.P, K, y这个实现有几个细节值得说明。第一协方差更新我用了Joseph形式而不是简单的 $(I-KH)P$因为Joseph形式在数值上更稳定尤其是在 $K$ 接近奇异的时候。第二我把新息 $y$ 和增益 $K$ 也返回了方便后续做异常检测和调试。第三初始协方差 $P_0$ 如果不知道设为单位矩阵乘以一个较大的数就行滤波器会自己收敛。4.2 一个完整的例子匀速直线运动跟踪光看代码可能还是有点抽象用一个具体的例子来跑一遍。假设有一个物体在二维平面上做匀速直线运动我们用一个雷达测它的位置但测量有噪声。状态向量取 $x [p_x, p_y, v_x, v_y]^T$观测向量取 $z [p_x, p_y]^T$。import numpy as np import matplotlib.pyplot as plt # 时间设置 dt 0.1 T 10 N int(T / dt) # 状态转移矩阵匀速模型 F np.array([ [1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1] ]) # 观测矩阵只测位置 H np.array([ [1, 0, 0, 0], [0, 1, 0, 0] ]) # 噪声协方差 q 0.1 # 过程噪声强度 Q q * np.array([ [dt**4/4, 0, dt**3/2, 0], [0, dt**4/4, 0, dt**3/2], [dt**3/2, 0, dt**2, 0], [0, dt**3/2, 0, dt**2] ]) r 0.5 # 观测噪声强度 R r * np.eye(2) # 初始化滤波器 kf LinearKalmanFilter(F, H, Q, R, x0[0, 0, 1, 0.5], P0np.eye(4)*10) # 生成真实轨迹和观测 true_states [] observations [] x_true np.array([0, 0, 1, 0.5]) np.random.seed(42) for k in range(N): x_true F x_true np.random.multivariate_normal(np.zeros(4), Q) z H x_true np.random.multivariate_normal(np.zeros(2), R) true_states.append(x_true.copy()) observations.append(z.copy()) # 运行滤波器 estimates [] for z in observations: kf.predict() x_est, P_est, K, y kf.update(z) estimates.append(x_est.copy()) true_states np.array(true_states) observations np.array(observations) estimates np.array(estimates) # 画图 fig, axes plt.subplots(2, 1, figsize(10, 8)) axes[0].plot(true_states[:, 0], true_states[:, 1], g-, labelTrue) axes[0].plot(observations[:, 0], observations[:, 1], r., alpha0.5, labelObserved) axes[0].plot(estimates[:, 0], estimates[:, 1], b--, labelKalman) axes[0].legend() axes[0].set_title(Position Tracking) axes[0].set_xlabel(x) axes[0].set_ylabel(y) axes[1].plot(true_states[:, 2], g-, labelTrue vx) axes[1].plot(estimates[:, 2], b--, labelKalman vx) axes[1].legend() axes[1].set_title(Velocity Estimation) axes[1].set_xlabel(Time step) axes[1].set_ylabel(vx) plt.tight_layout() plt.show()跑完这段代码你会看到卡尔曼滤波的估计轨迹明显比原始观测平滑而且速度估计也能收敛到真值附近。这就是卡尔曼滤波的威力它不仅估计了你直接测量的量位置还估计了你没有直接测量的量速度而且速度估计的精度随着时间推移会越来越高。4.3 参数Q和R的调参方法$Q$ 和 $R$ 的设定是实际项目中最让人头疼的部分。理论上$Q$ 和 $R$ 应该反映真实的噪声统计特性但实际中往往不知道。我一般用以下几种方法来确定方法一从数据中估计。如果你有静止状态下的传感器数据可以直接算观测噪声的方差作为 $R$ 的对角元素。过程噪声 $Q$ 比较难直接测但可以根据系统模型的不确定性来估计。比如匀速模型如果物体可能有加速度那 $Q$ 就应该反映这个加速度的量级。方法二归一化调参。把 $Q$ 固定为单位矩阵只调 $R$ 的缩放系数。因为卡尔曼增益只取决于 $Q$ 和 $R$ 的相对大小所以固定一个调另一个就够了。我通常先把 $Q$ 设小一点然后逐渐增大 $R$观察滤波效果找到一个平滑度和响应速度的平衡点。方法三看新息序列。新息 $y_k z_k - H_k \hat{x}_{k|k-1}$ 理论上应该是零均值白噪声。如果新息有偏或者有自相关说明模型有问题。如果新息的幅值远大于 $\sqrt{H P H^T R}$说明 $Q$ 或 $R$ 设小了。这个方法比较专业但非常有效。注意$Q$ 和 $R$ 的调参没有万能公式不同应用场景差别很大。我的经验是先根据物理意义给一个粗略的值然后通过实验微调。不要指望一次就调好迭代几次是正常的。5. 常见问题与排查技巧实录5.1 滤波器发散怎么办滤波器发散是实际中最常见的问题表现为估计值越来越偏离真实值协方差矩阵 $P$ 不收敛甚至越来越大。造成发散的原因通常有以下几个模型不匹配。这是最常见的原因。比如物体实际在做加速运动你用的是匀速模型那滤波器就会持续滞后。解决办法是改进模型把加速度也纳入状态向量或者增大 $Q$ 让滤波器更信任观测。数值问题。协方差矩阵 $P$ 在迭代过程中可能失去正定性导致卡尔曼增益计算异常。解决办法是使用Joseph形式的协方差更新或者定期对 $P$ 做对称化处理$P (P P^T) / 2$。观测异常值。如果观测中有离群点卡尔曼滤波会被带偏。解决办法是加一个新息门限当新息超过一定阈值时认为观测异常跳过更新或者降低观测权重。下面是一个带异常检测的更新函数def update_with_gating(self, z, gate_threshold3.0): z np.array(z, dtypefloat) # 计算新息和新息协方差 y z - self.H self.x S self.H self.P self.H.T self.R # 马氏距离 mahalanobis np.sqrt(y.T np.linalg.inv(S) y) if mahalanobis gate_threshold: # 观测异常跳过更新 return self.x, self.P, None, y # 正常更新 K self.P self.H.T np.linalg.inv(S) self.x self.x K y I np.eye(self.n) self.P (I - K self.H) self.P (I - K self.H).T K self.R K.T return self.x, self.P, K, y5.2 初始值怎么设初始状态 $\hat{x}_0$ 和初始协方差 $P_0$ 的设定对滤波器的收敛速度有影响但对稳态性能影响不大。如果对初始状态完全不知道$\hat{x}_0$ 设为零向量$P_0$ 设为一个较大的对角矩阵比如 $100I$滤波器会在前几步快速收敛。但有一个坑需要注意如果 $P_0$ 设得过大前几步的卡尔曼增益会接近 $H^{-1}$导致估计值剧烈跳动。如果观测本身噪声很大这个跳动可能会让滤波器不稳定。我的做法是 $P_0$ 不要设得过分大一般取观测噪声方差的10到100倍就够了。5.3 观测矩阵H不可逆怎么办有些初学者会问如果 $H$ 不是方阵怎么求逆实际上卡尔曼滤波中根本不需要对 $H$ 求逆需要求逆的是 $S HPH^T R$这是一个 $p \times p$ 的矩阵$p$ 是观测维度。只要 $R$ 是正定的$S$ 就一定可逆。所以观测维度小于状态维度完全没问题卡尔曼滤波照样能工作只是有些状态分量没有直接观测需要靠模型推断。5.4 常见问题速查表问题现象可能原因排查方法解决方案估计值滞后于真实值Q设太小模型太“自信”观察新息是否持续同号增大Q估计值抖动大R设太小过度信任观测观察估计值与观测值的跟随程度增大R协方差矩阵P不收敛模型不匹配或数值问题打印P的迹随时间变化检查模型使用Joseph形式滤波器突然发散观测异常值检查新息的马氏距离加新息门限速度估计不准过程噪声模型不合理检查Q中速度相关项的设定调整Q的速度分量实操心得调试卡尔曼滤波的时候一定要把新息序列画出来。新息是滤波器“惊讶程度”的度量正常情况应该是零均值白噪声。如果新息有趋势、有周期性或者幅值异常那一定是哪里出了问题。这个习惯帮我省了很多瞎调参数的时间。6. 从线性到非线性下一步的方向线性卡尔曼滤波能处理的问题有限实际项目中的系统大多是非线性的。比如雷达测距测角观测方程里有余弦和正弦比如机器人运动学状态转移里有三角函数。这些情况下线性卡尔曼滤波就不够用了。最常见的扩展是扩展卡尔曼滤波它的思路是在当前估计点附近对非线性函数做一阶泰勒展开用雅可比矩阵代替线性卡尔曼中的 $F$ 和 $H$。这样做的好处是保留了卡尔曼滤波的框架计算量增加不大。缺点是如果非线性很强一阶近似误差会比较大可能导致滤波发散。另一个方向是无迹卡尔曼滤波它不线性化函数而是选取一组sigma点让这些点经过非线性变换后再重构均值和协方差。无迹变换能捕捉到二阶以上的统计特性精度比扩展卡尔曼高但计算量也更大。我个人的建议是先把线性卡尔曼滤波吃透理解预测-更新的本质理解Q和R的物理意义理解新息的作用。这些概念在非线性滤波中同样适用。等你把线性卡尔曼用熟了再去看扩展卡尔曼和无迹卡尔曼会发现它们只是在“如何传播均值和协方差”这一点上做了不同的处理核心思想是一脉相承的。最后分享一个我在实际项目中的体会卡尔曼滤波的效果七分靠建模三分靠调参。如果模型建得不对再怎么调Q和R都是治标不治本。花时间理解你的系统把状态方程和观测方程写对比什么都重要。我见过太多人一上来就调参数结果调了半天发现是状态向量少了一个分量。先把模型理清楚剩下的就是水到渠成的事。
阅读完成 · 觉得有帮助？
