别再死记硬背公式了！用Python手撸一个卡尔曼滤波器，从传感器数据融合开始

用Python从零实现卡尔曼滤波传感器数据融合实战指南卡尔曼滤波就像一位经验丰富的航海家在充满噪声的数据海洋中为你指引方向。想象一下当GPS信号在城市峡谷中漂移IMU的加速度计因车辆震动而失真时如何获得可靠的定位这正是卡尔曼滤波大显身手的时刻。1. 环境准备与基础概念1.1 工具链配置工欲善其事必先利其器。我们需要以下Python包import numpy as np import matplotlib.pyplot as plt from scipy.linalg import inv对于传感器数据模拟可以创建一个简单的虚拟传感器类class VirtualSensor: def __init__(self, true_value, noise_std): self.true_value true_value self.noise_std noise_std def read(self): return self.true_value np.random.randn() * self.noise_std1.2 卡尔曼滤波核心思想卡尔曼滤波本质上是在做两件事预测基于系统模型预测下一个状态更新用实际测量值修正预测这种预测-修正的循环就像不断调整瞄准镜的狙击手每次射击后都根据弹着点修正下一次的瞄准。2. 一维卡尔曼滤波实现2.1 基础实现让我们从最简单的一维情况开始——估计一个恒定值。假设我们要测量室温温度计有±0.5°C的误差。class SimpleKalmanFilter: def __init__(self, initial_state, process_variance, measurement_variance): self.state initial_state self.process_variance process_variance self.measurement_variance measurement_variance self.estimate_variance 1.0 # 初始估计方差 def update(self, measurement): # 预测步骤 predicted_variance self.estimate_variance self.process_variance # 更新步骤 kalman_gain predicted_variance / (predicted_variance self.measurement_variance) self.state self.state kalman_gain * (measurement - self.state) self.estimate_variance (1 - kalman_gain) * predicted_variance return self.state2.2 实际测试让我们模拟一个温度测量场景true_temp 25.0 sensor VirtualSensor(true_temp, 0.5) kf SimpleKalmanFilter(20.0, 0.01, 0.25) measurements [] estimates [] for _ in range(100): z sensor.read() x kf.update(z) measurements.append(z) estimates.append(x)绘制结果对比plt.figure(figsize(10,6)) plt.plot(measurements, r., labelMeasurements) plt.plot(estimates, b-, labelKalman Filter) plt.axhline(true_temp, colorg, linestyle--, labelTrue Value) plt.legend() plt.show()3. 多维卡尔曼滤波位置与速度追踪3.1 状态空间模型现在我们来处理更实际的场景——同时估计位置和速度。状态向量包含位置和速度x [position] [velocity]状态转移矩阵A和观测矩阵H定义为dt 0.1 # 时间步长 A np.array([[1, dt], [0, 1]]) H np.array([[1, 0]]) # 只能观测到位置3.2 完整实现class MultiKalmanFilter: def __init__(self, initial_state, initial_covariance, process_noise, measurement_noise): self.state initial_state self.covariance initial_covariance self.Q process_noise # 过程噪声协方差 self.R measurement_noise # 测量噪声协方差 def predict(self): self.state A self.state self.covariance A self.covariance A.T self.Q return self.state def update(self, measurement): K self.covariance H.T inv(H self.covariance H.T self.R) self.state self.state K (measurement - H self.state) self.covariance (np.eye(2) - K H) self.covariance return self.state3.3 运动物体跟踪示例模拟一个匀加速运动的物体# 初始化 true_pos 0 true_vel 1 accel 0.1 kf MultiKalmanFilter( initial_statenp.array([0, 1]).reshape(-1,1), initial_covariancenp.eye(2), process_noisenp.array([[0.01, 0], [0, 0.01]]), measurement_noisenp.array([[0.1]]) ) positions [] estimates [] for t in range(100): # 真实运动 true_pos true_vel * dt true_vel accel * dt # 模拟测量 z true_pos np.random.randn() * 0.5 # 卡尔曼滤波 kf.predict() x kf.update(z) positions.append(true_pos) estimates.append(x[0,0])4. 传感器数据融合实战4.1 IMU与GPS融合现在我们来解决实际问题融合IMU加速度计和GPS数据。IMU提供高频但会漂移的加速度数据GPS提供低频但绝对的位置参考。状态向量扩展为x [position] [velocity] [acceleration]状态转移矩阵需要相应调整dt 0.1 A np.array([ [1, dt, 0.5*dt**2], [0, 1, dt], [0, 0, 1] ])4.2 实现细节class SensorFusionKalman: def __init__(self): self.state np.zeros((3,1)) self.covariance np.eye(3) self.Q np.diag([0.01, 0.01, 0.1]) # 过程噪声 self.R_gps np.array([[1.0]]) # GPS噪声 self.R_imu np.array([[0.1]]) # IMU噪声 def predict(self, accel_input, dt): # 更新状态转移矩阵 A np.array([ [1, dt, 0.5*dt**2], [0, 1, dt], [0, 0, 1] ]) B np.array([[0], [0], [1]]) self.state A self.state B * accel_input self.covariance A self.covariance A.T self.Q return self.state def update_gps(self, position): H np.array([[1, 0, 0]]) K self.covariance H.T inv(H self.covariance H.T self.R_gps) self.state self.state K (position - H self.state) self.covariance (np.eye(3) - K H) self.covariance return self.state def update_imu(self, accel): H np.array([[0, 0, 1]]) K self.covariance H.T inv(H self.covariance H.T self.R_imu) self.state self.state K (accel - H self.state) self.covariance (np.eye(3) - K H) self.covariance return self.state4.3 调参经验卡尔曼滤波的性能很大程度上取决于Q和R的选择过程噪声Q表示你对模型的信任程度Q越大滤波器对测量的响应越快Q越小滤波器越相信模型预测测量噪声R表示你对传感器的信任程度R越大滤波器对测量的权重越低R越小滤波器越相信测量值一个实用的调试方法是# 初始猜测 Q np.diag([0.1, 0.1, 1.0]) R np.array([[1.0]]) # 根据实际表现调整 if filter_response_too_slow: Q * 2 # 增加过程噪声 elif filter_too_jittery: R * 2 # 增加测量噪声