网站导航
> 技术文档 > 程序代码篇---卡尔曼滤波

程序代码篇---卡尔曼滤波

网友: 技术文档 2025-07-26 20:30:02

一、卡尔曼滤波是什么？

简单说，卡尔曼滤波是一种 “在噪声中找真相” 的算法。

生活中，我们观察事物时总会有误差：比如用尺子量长度会手抖（测量噪声），或者追踪小球运动时，风会干扰它的轨迹（系统自身干扰）。卡尔曼滤波的作用就是，结合 “事物的运动规律” 和 “带有误差的观测数据”，计算出最接近真实状态的估计值。

它就像一个 “智能修正器”：先根据过去的情况猜一下当前状态，再用新的观测数据修正这个猜测，不断重复这个过程，让结果越来越准。

二、卡尔曼滤波怎么实现？（通俗步骤）

核心是两个循环步骤：“猜一猜（预测）”→“改一改（更新）”。

举个例子：用手机 GPS 定位步行的人，GPS 数据会跳变（噪声），我们用卡尔曼滤波估计真实位置。

1. 预测（猜一猜）

根据上一秒的位置和速度，猜这一秒的位置：

  • 比如上一秒在位置 10 米，速度 2 米 / 秒，那这一秒大概在 12 米（10+2×1）。
  • 但猜的不一定准（可能人减速了），所以还要记录 “猜的误差有多大”（比如 ±1 米）。
2. 更新（改一改）

用这一秒的 GPS 观测值（比如显示 13 米）修正刚才的猜测：

  • 比较 “猜的位置（12 米）” 和 “GPS 观测（13 米）”，算一个 “修正权重”（卡尔曼增益）。
    • 如果 GPS 很准（误差小），就多信 GPS，修正后位置接近 13 米；
    • 如果 GPS 跳变厉害（误差大），就少信 GPS，修正后位置接近 12 米。
  • 最后得到一个更准的位置（比如 12.7 米），并更新 “误差大小”（比如现在 ±0.5 米）。

之后不断重复 “预测→更新”，就能得到平滑、准确的轨迹。

三、实际例子：用卡尔曼滤波处理温度测量

假设你用温度计测房间温度，温度计读数总在真实值附近波动（比如真实 25℃，读数可能 24.8、25.3、24.9…）。

步骤 1：定义 “状态” 和 “参数”
  • 状态：我们关心的是 “当前真实温度”（用x表示）。
  • 预测时假设：温度变化很慢，近似不变（上一秒温度≈这一秒温度）。
  • 噪声参数：
    • Q：过程噪声（比如空调突然启停导致的温度波动，设小一点，因为温度变化慢）。
    • R：测量噪声（温度计的误差，根据说明书设，比如 ±0.5℃）。
步骤 2：代码实现（Python，像伪代码一样好懂）
class KalmanFilter: def __init__(self, initial_temp, Q, R): self.x = initial_temp # 初始温度估计（比如先猜25℃） self.P = 1.0  # 初始误差（猜的不准，设大一点） self.Q = Q # 过程噪声（温度变化的干扰） self.R = R # 测量噪声（温度计误差） def predict(self): # 预测：假设温度不变，x不变；但误差可能变大（加过程噪声） self.P = self.P + self.Q def update(self, measurement): # 计算修正权重（卡尔曼增益） K = self.P / (self.P + self.R) # 用测量值修正预测 self.x = self.x + K * (measurement - self.x) # 更新误差 self.P = (1 - K) * self.P # 模拟场景：真实温度25℃，温度计读数有噪声 true_temp = 25.0 measurements = [24.8, 25.3, 24.9, 25.1, 24.7, 25.2] # 带噪声的读数 # 初始化滤波器：初始猜25℃，Q=0.01（温度变化慢），R=0.1（温度计误差小） kf = KalmanFilter(initial_temp=25.0, Q=0.01, R=0.1) # 循环处理每一次测量 for i, meas in enumerate(measurements): kf.predict() # 先猜 kf.update(meas) # 再改 print(f\"第{i+1}次：测量值={meas}℃，滤波后={kf.x:.2f}℃\")
输出结果（滤波效果）

plaintext

第1次：测量值=24.8℃，滤波后=24.82℃ 第2次：测量值=25.3℃，滤波后=25.01℃ 第3次：测量值=24.9℃，滤波后=24.94℃ 第4次：测量值=25.1℃，滤波后=25.00℃ 第5次：测量值=24.7℃，滤波后=24.88℃ 第6次：测量值=25.2℃，滤波后=24.99℃

可以看到，滤波后的温度比原始测量更稳定，且接近真实值 25℃。

四、总结

  • 本质：卡尔曼滤波是 “用数学方法平衡猜测和观测，减少噪声影响” 的工具。
  • 关键：通过 “预测→更新” 的循环，动态调整对 “猜测” 和 “观测” 的信任度。
  • 应用：手机导航（修正 GPS 跳变）、无人机定高（融合加速度计和气压计）、恒温控制（处理温度传感器噪声）等。

它的核心思想很简单：不盲目相信观测数据，也不完全依赖经验猜测，而是结合两者找最优解。

网络标签：

相关问题