用Python和NumPy手把手实现一个卡尔曼滤波器（附完整代码与可视化）

用Python和NumPy手把手实现一个卡尔曼滤波器（附完整代码与可视化）

卡尔曼滤波器在机器人导航、自动驾驶和金融预测等领域有着广泛应用。但对于初学者来说，那些复杂的矩阵运算和概率公式常常让人望而却步。今天我们就用Python和NumPy，从零开始构建一个完整的卡尔曼滤波器，通过可视化让你直观感受它的强大之处。

我们将模拟一个匀速运动的小车，它的真实位置会被噪声干扰，而卡尔曼滤波器的任务就是从带有噪声的观测数据中，尽可能准确地估计出小车的真实位置。这个例子虽然简单，但包含了卡尔曼滤波的所有核心要素。

1. 环境准备与问题建模

首先确保你的Python环境已经安装了必要的库：

pip install numpy matplotlib

我们模拟的场景是：一个小车在直线上以恒定速度运动，但由于各种因素，我们无法直接获得它的精确位置，只能通过带有噪声的传感器进行观测。卡尔曼滤波器的目标就是结合运动模型和观测数据，给出最优的位置估计。

定义系统的基本参数：

import numpy as np import matplotlib.pyplot as plt # 时间参数 dt = 0.1 # 时间步长 T = 10 # 总时长 steps = int(T/dt) # 总步数 # 系统参数 true_velocity = 2.0 # 小车的真实速度 process_noise = 0.1 # 过程噪声强度 measure_noise = 0.5 # 测量噪声强度

2. 卡尔曼滤波器核心实现

卡尔曼滤波器由两个主要步骤组成：预测和更新。下面我们分别实现这两个步骤。

2.1 定义系统模型

首先定义卡尔曼滤波器所需的矩阵：

# 状态转移矩阵 (假设匀速运动) A = np.array([[1, dt], [0, 1]]) # 控制输入矩阵 (本例中没有外部控制输入) B = np.zeros((2, 1)) # 观测矩阵 (我们只能观测到位置) H = np.array([[1, 0]]) # 过程噪声协方差矩阵 Q = np.array([[dt**4/4, dt**3/2], [dt**3/2, dt**2]]) * process_noise**2 # 测量噪声协方差矩阵 R = np.array([[measure_noise**2]]) # 初始状态和协方差 x = np.zeros((2, 1)) # [位置, 速度] P = np.eye(2) * 1000 # 初始不确定性较大

2.2 预测步骤实现

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

def predict(x, P): # 状态预测 x = A @ x # 协方差预测 P = A @ P @ A.T + Q return x, P

2.3 更新步骤实现

更新步骤结合观测数据修正预测：

def update(x, P, z): # 计算卡尔曼增益 S = H @ P @ H.T + R K = P @ H.T @ np.linalg.inv(S) # 状态更新 y = z - H @ x x = x + K @ y # 协方差更新 P = (np.eye(2) - K @ H) @ P return x, P

3. 模拟数据生成与滤波

现在我们来生成模拟数据并应用卡尔曼滤波：

# 生成真实轨迹 true_positions = np.array([i*dt*true_velocity for i in range(steps)]) true_velocities = np.ones(steps) * true_velocity # 生成带噪声的观测 np.random.seed(42) measurements = true_positions + np.random.normal(0, measure_noise, steps) # 存储滤波结果 filtered_positions = np.zeros(steps) filtered_velocities = np.zeros(steps) # 初始化状态 x[0, 0] = 0 # 初始位置 x[1, 0] = 2.0 # 初始速度估计 # 运行卡尔曼滤波 for i in range(steps): # 预测 x, P = predict(x, P) # 更新 z = measurements[i] x, P = update(x, P, z) # 存储结果 filtered_positions[i] = x[0, 0] filtered_velocities[i] = x[1, 0]

4. 结果可视化与分析

让我们将真实轨迹、观测数据和滤波结果可视化：

plt.figure(figsize=(12, 6)) # 绘制真实轨迹 plt.plot(np.arange(0, T, dt), true_positions, label='真实位置', color='g', linewidth=2) # 绘制观测数据 plt.scatter(np.arange(0, T, dt), measurements, label='观测值', color='r', s=10) # 绘制滤波结果 plt.plot(np.arange(0, T, dt), filtered_positions, label='卡尔曼滤波', color='b', linewidth=2) plt.xlabel('时间 (s)') plt.ylabel('位置 (m)') plt.title('卡尔曼滤波效果对比') plt.legend() plt.grid(True) plt.show()

从可视化结果中，你可以清楚地看到：

• 红色散点代表带有噪声的观测数据
• 绿色线代表小车的真实运动轨迹
• 蓝色线是卡尔曼滤波器的估计结果

关键观察：

1. 滤波结果明显比原始观测数据平滑
2. 滤波轨迹紧密跟随真实轨迹，有效减少了噪声影响
3. 在观测数据波动较大时，滤波结果仍能保持合理的估计

5. 滤波器参数调优

卡尔曼滤波器的性能很大程度上取决于Q和R的选择。让我们看看不同参数设置下的滤波效果：

# 尝试不同的噪声参数组合 param_combinations = [ (0.1, 0.5), # 原始参数 (0.01, 0.5), # 过程噪声变小 (0.1, 0.1), # 测量噪声变小 (1.0, 0.5) # 过程噪声变大 ] plt.figure(figsize=(12, 8)) for i, (q, r) in enumerate(param_combinations, 1): # 调整噪声参数 Q = np.array([[dt**4/4, dt**3/2], [dt**3/2, dt**2]]) * q**2 R = np.array([[r**2]]) # 重置初始状态 x = np.zeros((2, 1)) P = np.eye(2) * 1000 x[0, 0] = 0 x[1, 0] = 2.0 # 运行滤波 temp_positions = np.zeros(steps) for j in range(steps): x, P = predict(x, P) x, P = update(x, P, measurements[j]) temp_positions[j] = x[0, 0] # 绘制结果 plt.subplot(2, 2, i) plt.plot(np.arange(0, T, dt), true_positions, 'g', label='真实位置') plt.scatter(np.arange(0, T, dt), measurements, color='r', s=5, label='观测值') plt.plot(np.arange(0, T, dt), temp_positions, 'b', label='滤波结果') plt.title(f'Q={q}, R={r}') plt.legend() plt.tight_layout() plt.show()

参数影响分析：

6. 卡尔曼滤波器的高级应用

虽然我们实现的是最简单的线性卡尔曼滤波器，但同样的原理可以扩展到更复杂的场景：

1. 非线性系统：使用扩展卡尔曼滤波(EKF)或无迹卡尔曼滤波(UKF)
2. 多传感器融合：结合GPS、IMU等多种传感器数据
3. 目标跟踪：在二维或三维空间中跟踪运动物体

下面是一个简单的二维扩展示例框架：

# 二维卡尔曼滤波器初始化 dt = 0.1 A = 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 = np.eye(4) * 0.1 # 过程噪声 R = np.eye(2) * 0.5 # 测量噪声

在实际项目中，你可能需要：

• 处理非线性运动模型
• 调整噪声参数以获得最佳性能
• 实现更复杂的数据关联逻辑（多目标跟踪时）