python import numpy as np import matplotlib.pyplot as plt

仿真参数

dt = 0.1 # 时间步长 T = 10 # 总时长（秒） N = int(T/dt) # 步数

真实系统（模拟真值）

true_pos = np.zeros(N) true_vel = 2.0 # 恒定速度 2 m/s true_pos[0] = 0.0 for k in range(1, N): true_pos[k] = true_pos[k-1] + true_vel * dt + np.random.normal(0, 0.1) # 加入过程噪声

模拟带噪声的测量

measure_std = 0.5 measurements = true_pos + np.random.normal(0, measure_std, N)

初始化卡尔曼滤波

x_est = np.array([0.0, 1.0]) # 初始状态估计 [位置, 速度] P = np.eye(2) * 1000 # 初始协方差，表示很大不确定性

系统矩阵

F = np.array([[1, dt], [0, 1]]) H = np.array([[1, 0]]) R = np.array([[measure_std**2]])

过程噪声协方差（使用离散白噪声加速度模型）

q = 1.0 # 噪声强度谱密度 Q = q * np.array([[dt4/4, dt3/2], [dt3/2, dt2]])

存储结果

estimated_pos = np.zeros(N) estimated_vel = np.zeros(N)

for k in range(N): # 预测 x_pred = F @ x_est P_pred = F @ P @ F.T + Q

# 更新（如果有测量）
z = measurements[k]
y = z - H @ x_pred           # 残差
S = H @ P_pred @ H.T + R     # 残差协方差
K = P_pred @ H.T @ np.linalg.inv(S)  # 卡尔曼增益

x_est = x_pred + K @ y
P = (np.eye(2) - K @ H) @ P_pred

estimated_pos[k] = x_est[0]
estimated_vel[k] = x_est[1]

绘图

time = np.linspace(0, T, N) plt.figure(figsize=(10,6)) plt.plot(time, true_pos, 'g-', label='真实位置') plt.plot(time, measurements, 'r.', markersize=3, alpha=0.6, label='测量值') plt.plot(time, estimated_pos, 'b-', label='卡尔曼滤波估计') plt.xlabel('时间 (s)') plt.ylabel('位置 (m)') plt.legend() plt.title('一维位置估计') plt.grid(True) plt.show()