卡尔曼滤波。 （代码非常详细、非常齐全） 1、卡尔曼滤波的含义是现时刻的最佳估计为在前一时刻的...

卡尔曼滤波。 （代码非常详细、非常齐全） 1、卡尔曼滤波的含义是现时刻的最佳估计为在前一时刻的最佳估计的基础上根据现时刻的观测值作线性修正。 2、卡尔曼滤波在数学上是一种线性最小方差统计估算方法，它是通过处理一系列带有误差的实际测量数据而得到物理参数的最佳估算。 1、包含噪声的，对物体位置的观察序列（可能有偏差）预测出物体的位置的坐标及速度。 3、这个估计可以是对当前目标位置的估计(滤波)，也可以是对于将来位置的估计(预测)，也可以是对过去位置的估计(插值或平滑)。

卡尔曼滤波这玩意儿听起来高大上，其实本质就是"用过去猜现在，再用测量值修bug"。举个栗子，你在用GPS追踪无人机，GPS数据跳来跳去像抽风，这时候就需要它来算个靠谱的位置。咱们直接上代码实战——假设有个小车在直线上匀速运动，但观测数据有噪声。

先导入必要的库：

import numpy as np import matplotlib.pyplot as plt real_v = 0.5 # 真实速度0.5m/s total_time = 30 dt = 1.0 # 采样间隔

接下来造点假数据：

# 生成带噪声的观测数据 np.random.seed(42) true_pos = [real_v * t for t in range(total_time)] obs_noise = np.random.normal(0, 0.6, total_time) # 标准差0.6m的噪声 obs_pos = true_pos + obs_noise

重点来了，卡尔曼滤波器实现：

class KalmanFilter: def __init__(self, initial_pos, initial_vel, process_noise, measurement_noise): # 状态向量 [位置, 速度] self.state = np.array([[initial_pos], [initial_vel]]) # 状态转移矩阵（假设匀速模型） self.F = np.array([[1, dt], [0, 1]]) # 测量矩阵（只能观测位置） self.H = np.array([[1, 0]]) # 过程噪声协方差 self.Q = np.eye(2) * process_noise # 测量噪声协方差 self.R = np.array([[measurement_noise]]) # 协方差矩阵 self.P = np.eye(2) * 500 # 初始不确定度大 def predict(self): self.state = self.F @ self.state self.P = self.F @ self.P @ self.F.T + self.Q return self.state[0][0] def update(self, measurement): y = measurement - self.H @ self.state S = self.H @ self.P @ self.H.T + self.R K = self.P @ self.H.T @ np.linalg.inv(S) self.state = self.state + K @ y self.P = (np.eye(2) - K @ self.H) @ self.P

代码里有个骚操作是过程噪声Q和测量噪声R的设置。这两个参数相当于调参师的快乐开关——Q调大表示更相信测量值，R调大则更相信预测值。比如在跟踪无人机时，如果GPS信号不稳定，就该把R设大些。

跑个完整流程试试：

kf = KalmanFilter(initial_pos=0, initial_vel=0.5, process_noise=0.01, measurement_noise=0.6**2) estimates = [] for z in obs_pos: kf.predict() estimates.append(kf.update(z)) # 画个对比图 plt.figure(figsize=(12,6)) plt.plot(true_pos, label='真实轨迹', linestyle='--') plt.scatter(range(total_time), obs_pos, s=15, label='观测值', alpha=0.6) plt.plot(estimates, color='r', linewidth=2, label='卡尔曼滤波') plt.legend() plt.title('卡尔曼滤波效果对比（红线和真实轨迹几乎重合）') plt.show()

运行后会发现红线几乎贴着真实轨迹走，这说明滤波成功地把那些乱跳的观测点给熨平了。重点观察协方差矩阵P的变化——初始值设得很大，随着迭代会逐渐收敛，说明系统对自己估计越来越有信心。

想玩预测功能的话，可以连续调用predict()不更新：

# 预测未来5秒 future_pred = [] current_state = kf.state.copy() for _ in range(5): current_state = kf.F @ current_state future_pred.append(current_state[0][0]) print(f'未来5秒预测位置: {future_pred}')

这时候就能看出卡尔曼滤波的另一个妙用：在目标短暂丢失时（比如无人机飞进楼宇遮挡区），能继续给出合理的位置推测。不过要注意，长期预测会越来越不靠谱，毕竟现实世界的运动模型不可能永远匀速直线。