在自动驾驶和机器人领域，卡尔曼滤波（KF）是状态估计的基石。很多初学者看了无数遍数学推导，一到工程落地就抓瞎；或者只会调现成的库，根本不知道矩阵在底层是如何流转的。

import numpy as npimport matplotlib.pyplot as plt

=====================================

1. 滤波器类定义：纯手工打造的卡尔曼滤波器

=====================================

class KalmanFilter: def init(self, A, B, C, R, Q, initial_x, initial_P): """ 构造函数：初始化卡尔曼滤波器的5个核心矩阵和2个初始状态 """ self.A = np.array(A)

状态转移矩阵 (A)：描述系统如何随时间自行演变

self.B = np.array(B)

控制输入矩阵 (B)：描述踩油门/刹车对系统的影响

self.C = np.array(C)

观测矩阵 (C/H)：把车辆真实状态映射到传感器看得到的位置

self.R = np.array(R)

过程噪声协方差 (R)：地面滑移、风阻等物理模型的误差

self.Q = np.array(Q)

观测噪声协方差 (Q)：传感器硬件自带的随机电子噪声

self.x = np.array(initial_x)

最优状态估计值后验均值 μ (初始设为0)

self.P = np.array(initial_P)

状态不确定性后验协方差矩阵 Σ (初始置信度)

def predict(self, u): """ 第一步：预测步 (Prediction) ———— 闭着眼睛靠物理公式盲猜 """

公式 1：x̄t = A * x{t-1} + B * u_t (基于运动学规律推算当前位置和速度)

np.dot 是矩阵乘法。这里将上一时刻的状态乘以状态转移矩阵，再加上控制输入的贡献

self.x = np.dot(self.A, self.x) + np.dot(self.B, u)

公式 2：Σ̄t = A * Σ{t-1} * Aᵀ + R (由于多走了一秒，系统不确定性变大，再叠加过程噪声).T 代表矩阵转置。A * P * A.T 是高斯分布线性变换的经典方差推导结果

self.P = np.dot(np.dot(self.A, self.P), self.A.T) + self.R

return self.x, self.P

def update(self, z): """ 第二步：更新步 (Update/Correction) ———— 睁开眼看传感器，修正盲猜 """

公式 3：计算卡尔曼增益 K_t = Σ̄_t * Cᵀ * (C * Σ̄_t * Cᵀ + Q)⁻¹

S 矩阵代表“预测观测项的不确定性”，包含了理论预测方差和传感器的硬件噪声 Q

S = np.dot(np.dot(self.C, self.P), self.C.T) + self.Q

np.linalg.inv 用于求解矩阵的逆。K 矩阵就是最终用来调节权重的卡尔曼增益

K = np.dot(np.dot(self.P, self.C.T), np.linalg.inv(S))

公式 4：修正状态均值 x_t = x̄_t + K_t * (z_t - C * x̄_t)

(z - C * x) 叫做观测残差或创新（Innovation），代表“现实”与“理想盲猜”的差距

innovation = z - np.dot(self.C, self.x)

卡尔曼增益 K 决定了把多少百分比的“现实差距”补偿到盲猜的值里

self.x = self.x + np.dot(K, innovation)

公式 5：更新不确定性 Σ_t = (I - K_t * C) * Σ̄_t

eye(shape) 生成一个单位矩阵 I。因为融合了新传感器数据，后验不确定性 P 一定会变小

I = np.eye(self.A.shape[0]) self.P = np.dot((I - np.dot(K, self.C)), self.P)

return self.x, self.P

=====================================

2. 仿真场景设定：1D直线行驶的自动驾驶小车

=====================================

dt = 1.0

采样时间间隔为 1秒

物理矩阵构建

A = [[1.0, dt], [0.0, 1.0]]

位置 = 1位置 + dt速度； 速度 = 0位置 + 1速度

B = [[0.5 * dt ** 2], [dt]]

恒定加速度位移公式：s_加 = 0.5at²； v_加 = a*t

C = [[1.0, 0.0]]

[1, 0] 矩阵表示传感器（GPS）只能看清位置，速度维度的输出被抹为0

噪声大小设定

R = [[0.1, 0.0], [0.0, 0.1]]

过程噪声：假设位置和速度分别有 0.1 的物理扰动

Q = [[0.25]]

观测噪声：假设 GPS 的测距误差标准差为 0.5米（方差 = 0.5² = 0.25）

=====================================

3. 产生带有噪声的“真实世界”测试数据

=====================================

np.random.seed(42)

固定随机种子，确保读者在自己电脑上运行出来的图和我一模一样

true_x = np.array([[0.0], [0.0]])

车辆真实的初始状态 [初始位置=0, 初始速度=0]

u = np.array([[1.0]])

踩油门：给车辆输入恒定的 1 m/s² 加速度

true_positions = []

用于存放真实位置的历史记录

true_velocities = []

用于存放真实速度的历史记录

gps_measurements = []

用于存放被噪声污染的 GPS 历史记录

time_steps = 30

让小车跑 30秒

for _ in range(time_steps):

模拟物理世界的真实运动：x_true = Ax + Bu + 真实环境里的不可控噪声

process_noise = np.random.multivariate_normal([0, 0], R).reshape(2, 1) true_x = np.dot(A, true_x) + np.dot(B, u) + process_noise

模拟 GPS 硬件传感器的干预：z = C*x_true + 传感器自带的随机抖动误差

obs_noise = np.random.normal(0, np.sqrt(Q[0][0])) z = np.dot(C, true_x) + obs_noise

保存数据用于后续画图

true_positions.append(true_x[0][0]) true_velocities.append(true_x[1][0]) gps_measurements.append(z[0][0])

=====================================

4. 核心环节：把数据喂进手写的卡尔曼滤波器

=====================================

假设我们一开始完全不知道车在哪，随便猜一个位置 0，不确定度设为 1

initial_x = [[0.0], [0.0]]initial_P = [[1.0, 0.0], [0.0, 1.0]]

实例化前面写好的类

kf = KalmanFilter(A, B, C, R, Q, initial_x, initial_P)

estimated_positions = []

存放 KF 滤波后的优良位置

estimated_velocities = []

存放 KF 逆推出来的优良速度

for t in range(time_steps):

1. 物理时钟前进，滤波器执行预测步

kf.predict(u)

2. GPS 收到新数据，滤波器执行更新步（传入当前时刻的标量测量值值）

x_post, _ = kf.update(np.array([[gps_measurements[t]]]))

记录滤波输出

estimated_positions.append(x_post[0][0]) estimated_velocities.append(x_post[1][0])

=====================================

5. 可视化：生成清晰直观的对比图

=====================================

plt.figure(figsize=(14, 5))

左图：位置追踪对比（完美的去噪能力）

plt.subplot(1, 2, 1)plt.plot(true_positions, label='True Position (Real World)', color='g', linewidth=2)plt.scatter(range(time_steps), gps_measurements, label='GPS Measurement (Noisy)', color='r', marker='x', s=40)plt.plot(estimated_positions, label='KF Estimated Position', color='b', linestyle='--', linewidth=2)plt.title('1. Position Tracking Performance', fontsize=12)plt.xlabel('Time Step (s)')plt.ylabel('Distance (m)')plt.legend()plt.grid(True, linestyle=':', alpha=0.6)

右图：速度估计对比（无中生有的神级反推）

plt.subplot(1, 2, 2)plt.plot(true_velocities, label='True Velocity (Real World)', color='g', linewidth=2)plt.plot(estimated_velocities, label='KF Estimated Velocity', color='b', linestyle='--', linewidth=2)plt.title('2. Velocity Estimation (No Direct Sensor Input!)', fontsize=12)plt.xlabel('Time Step (s)')plt.ylabel('Velocity (m/s)')plt.legend()plt.grid(True, linestyle=':', alpha=0.6)

plt.show()

运行代码后，你将看到以下震撼的结果：

• 完美去噪（看左图）：红色的 GPS 测量值抖动得非常厉害，如果自动驾驶汽车直接听从 GPS 的话会疯狂画龙。而蓝色的卡尔曼估计曲线完美过滤掉了噪声，紧紧贴合绿色的真实轨迹。
• 无中生有（看右图）：我们的传感器根本无法测量速度！ 观测矩阵 C把速度完全抹去了。但是依靠卡尔曼滤波在预测步建立的状态相关性，仅凭位置数据，算法就完美“反推”出了小车精确的速度曲线！这就是数据融合的魅力。