狀態向量：位置和速度
[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})")
