状态向量：位置和速度
[x, y, vx, vy]预测阶段：用加速度估算速度和位置（IMU数据）
更新阶段：用 GPS 位置修正漂移（每隔一定时间才来一次）
import numpy as np# 时间步长（秒）
dt = 0.1# 状态向量: [x, y, vx, vy]
x = np.array([[0], [0], [0], [0]])# 状态协方差矩阵
P = np.eye(4) * 1.0# 状态转移矩阵 F
F = np.array([[1, 0, dt, 0],[0, 1, 0, dt],[0, 0, 1, 0],[0, 0, 0, 1]
])# 控制输入矩阵 B（加速度影响速度和位置）
B = np.array([[0.5 * dt**2, 0],[0, 0.5 * dt**2],[dt, 0],[0, dt]
])# 观测矩阵 H（GPS 只能测位置）
H = np.array([[1, 0, 0, 0],[0, 1, 0, 0]
])# 过程噪声协方差 Q（IMU 不准）
Q = np.eye(4) * 0.2# 观测噪声协方差 R（GPS 有噪声）
R = np.eye(2) * 2.0# 单位矩阵 I
I = np.eye(4)# 模拟数据：IMU 每次有，GPS 每10次有
imu_acc = [0.1, 0.0] # 恒定 x 方向加速度
for step in range(50):# === 1. 预测阶段 ===u = np.array([[imu_acc[0]], [imu_acc[1]]]) # IMU 加速度输入x = F @ x + B @ uP = F @ P @ F.T + Q# === 2. 更新阶段（GPS 每10步更新一次）===if step % 10 == 0:gps_pos = np.array([[x[0, 0] + np.random.normal(0, 1)],[x[1, 0] + np.random.normal(0, 1)]]) # 模拟GPS测量y = gps_pos - H @ xS = H @ P @ H.T + RK = P @ H.T @ np.linalg.inv(S)x = x + K @ yP = (I - K @ H) @ Pprint(f"Step {step:02d} -> Position: ({x[0,0]:.2f}, {x[1,0]:.2f}) Velocity: ({x[2,0]:.2f}, {x[3,0]:.2f})")
