卡尔曼滤波 Python 3.12 实现：从 2 传感器数据融合到 5 个核心公式代码化

卡尔曼滤波 Python 3.12 实现：从双传感器数据融合到核心公式代码化

在工程实践中，我们常常需要处理来自多个传感器的数据，这些数据往往存在噪声和不确定性。如何有效地融合这些数据，获得更准确的状态估计？卡尔曼滤波提供了一种优雅的解决方案。本文将带你从零开始实现一个完整的卡尔曼滤波器，使用Python 3.12将数学公式转化为可运行的代码。

1. 卡尔曼滤波基础概念

卡尔曼滤波是一种递归算法，它通过融合系统模型预测和实际测量值来估计系统状态。想象你正在开发一个自动驾驶系统，GPS提供位置信息但更新频率低，IMU（惯性测量单元）提供高频的运动数据但存在漂移。卡尔曼滤波能完美融合这两者：

class KalmanFilter: def __init__(self, initial_state, initial_covariance): self.state = initial_state # 初始状态估计 self.covariance = initial_covariance # 初始协方差矩阵

卡尔曼滤波的核心在于它处理不确定性的方式。它维护两个关键量：

• 状态估计：对系统当前状态的"最佳猜测"
• 协方差矩阵：表示我们对状态估计的不确定性程度

提示：卡尔曼滤波假设系统和测量噪声都是高斯分布的白噪声，这在许多实际应用中是一个合理的近似。

2. 卡尔曼滤波的五大核心方程

让我们分解卡尔曼滤波的五个核心方程，并理解它们的Python实现：

2.1 预测步骤

预测步骤根据系统模型预测下一时刻的状态：

def predict(self, transition_matrix, process_noise): # 状态预测 self.state = transition_matrix @ self.state # 协方差预测 self.covariance = (transition_matrix @ self.covariance @ transition_matrix.T) + process_noise return self.state, self.covariance

这里的关键参数：

• transition_matrix：描述系统如何从上一状态转移到当前状态
• process_noise：过程噪声协方差，表示系统模型的不确定性

2.2 更新步骤

当获得新的测量值时，我们更新状态估计：

def update(self, measurement, measurement_matrix, measurement_noise): # 计算卡尔曼增益 S = measurement_matrix @ self.covariance @ measurement_matrix.T + measurement_noise K = self.covariance @ measurement_matrix.T @ np.linalg.inv(S) # 状态更新 innovation = measurement - measurement_matrix @ self.state self.state = self.state + K @ innovation # 协方差更新 I = np.eye(self.state.shape[0]) self.covariance = (I - K @ measurement_matrix) @ self.covariance return self.state, self.covariance

更新步骤的关键点：

1. 计算卡尔曼增益K，决定我们更信任预测还是测量
2. 用测量值修正预测状态
3. 更新协方差矩阵反映新的不确定性

3. 双传感器融合实战：GPS与IMU

让我们构建一个具体示例，融合GPS和IMU数据来估计车辆位置：

3.1 系统建模

假设我们跟踪车辆的1维位置和速度：

# 状态向量: [位置, 速度] initial_state = np.array([[0.0], [0.0]]) initial_covariance = np.diag([100.0, 10.0]) # 初始不确定性 # 状态转移矩阵 (假设恒定速度模型) dt = 0.1 # 时间步长 transition_matrix = np.array([[1, dt], [0, 1]]) # 过程噪声 (模型不确定性) process_noise = np.diag([0.1, 0.01])

3.2 传感器模型

GPS和IMU提供不同特性的测量：

# GPS测量矩阵 (只测量位置) gps_matrix = np.array([[1, 0]]) gps_noise = np.array([[1.0]]) # GPS噪声较大 # IMU测量矩阵 (测量加速度，可推导速度变化) imu_matrix = np.array([[0, 1]]) imu_noise = np.array([[0.1]]) # IMU更精确但会漂移

3.3 融合实现

def simulate_and_filter(): kf = KalmanFilter(initial_state, initial_covariance) true_states = [] estimates = [] for t in np.arange(0, 10, dt): # 真实状态演化 (模拟) true_state = transition_matrix @ true_states[-1] if true_states else initial_state true_states.append(true_state) # 预测步骤 kf.predict(transition_matrix, process_noise) # 模拟传感器测量 if t % 0.5 == 0: # GPS低频更新 gps_measurement = gps_matrix @ true_state + np.random.normal(0, gps_noise[0,0]) kf.update(gps_measurement, gps_matrix, gps_noise) if t % 0.1 == 0: # IMU高频更新 imu_measurement = imu_matrix @ true_state + np.random.normal(0, imu_noise[0,0]) kf.update(imu_measurement, imu_matrix, imu_noise) estimates.append(kf.state.copy()) return true_states, estimates

4. 性能评估与可视化

实现完成后，我们需要评估滤波器性能：

import matplotlib.pyplot as plt def plot_results(true_states, estimates): times = np.arange(0, 10, dt) true_pos = [s[0,0] for s in true_states] est_pos = [e[0,0] for e in estimates] plt.figure(figsize=(12,6)) plt.plot(times, true_pos, 'g-', label='真实位置') plt.plot(times, est_pos, 'b--', label='估计位置') plt.xlabel('时间 (s)') plt.ylabel('位置 (m)') plt.legend() plt.title('卡尔曼滤波性能') plt.grid(True) plt.show()

典型输出会显示：

• 绿色曲线：真实位置
• 蓝色虚线：卡尔曼滤波估计
• 滤波器能有效平滑噪声，即使传感器更新频率不同

5. 高级话题与优化

5.1 参数调优

卡尔曼滤波性能很大程度上取决于噪声参数的设置：

5.2 非线性系统：扩展卡尔曼滤波

当系统非线性时，我们需要扩展卡尔曼滤波(EKF)：

def ekf_predict(f, F, Q): self.state = f(self.state) # 非线性状态转移 self.covariance = F @ self.covariance @ F.T + Q def ekf_update(h, H, z, R): y = z - h(self.state) # 非线性测量残差 S = H @ self.covariance @ H.T + R K = self.covariance @ H.T @ np.linalg.inv(S) self.state = self.state + K @ y self.covariance = (np.eye(len(self.state)) - K @ H) @ self.covariance

5.3 多传感器融合架构

对于多个传感器，可以采用以下架构：

1. 集中式融合：所有传感器数据直接输入单个滤波器
2. 分布式融合：每个传感器有独立滤波器，结果再融合
3. 混合架构：结合两者优点

# 分布式融合示例 def distributed_fusion(filters, weights): fused_state = np.zeros_like(filters[0].state) fused_cov = np.zeros_like(filters[0].covariance) for flt, w in zip(filters, weights): fused_state += w * flt.state inv_cov = np.linalg.inv(flt.covariance) fused_cov += w * inv_cov fused_cov = np.linalg.inv(fused_cov) return fused_state, fused_cov

在实际项目中，我发现卡尔曼滤波的实现难点不在于代码本身，而在于正确建模系统动力学和噪声特性。通过合理设置参数和多次实验验证，才能获得理想的滤波效果。