การประมาณสถานะและ EKF

ตัวอย่างที่ 1 ความสูงจากแอกเซเลอโรมิเตอร์และบารอมิเตอร์

จำลองโดรนลอยตัว แล้วไต่ระดับ 4 m ช่วงวินาทีที่ 5–9 แอกเซเลอโรมิเตอร์มี bias 0.05 m/s² และสัญญาณรบกวน 0.3 m/s² บารอมิเตอร์มีสัญญาณรบกวน 0.5 m (ข้อมูลจำลอง)

import numpy as np

rng = np.random.default_rng(3)
dt, T = 0.01, 30.0
n = int(T / dt)
t = np.arange(n) * dt
true_a = np.where((t > 5) & (t < 7), 1.0, 0.0) - np.where((t > 7) & (t < 9), 1.0, 0.0)
true_v = np.cumsum(true_a) * dt
true_h = np.cumsum(true_v) * dt
acc = true_a + 0.05 + rng.normal(0, 0.3, n)    # m/s² มี bias
baro = true_h + rng.normal(0, 0.5, n)          # m

A = np.array([[1, dt], [0, 1]])                # สถานะ [ความสูง, ความเร็ว]
B = np.array([0.5 * dt * dt, dt])
H = np.array([[1.0, 0.0]])
Q = np.diag([1e-5, 1e-3])
R = np.array([[0.25]])
x, P, est = np.zeros(2), np.eye(2), []
for k in range(n):
    x = A @ x + B * acc[k]                     # ทำนาย
    P = A @ P @ A.T + Q
    S = H @ P @ H.T + R                        # ปรับแก้
    K = P @ H.T @ np.linalg.inv(S)
    x = x + (K @ (baro[k] - H @ x)).ravel()
    P = (np.eye(2) - K @ H) @ P
    est.append(x[0])

rms = lambda e: float(np.sqrt(np.mean(e ** 2)))
drift = (np.cumsum(np.cumsum(acc) * dt) * dt)[-1] - true_h[-1]
print(f"true height at end {true_h[-1]:.2f} m")
print(f"accelerometer only: error at end {drift:.1f} m")
print(f"barometer only: RMS error {rms(baro - true_h):.2f} m")
print(f"Kalman filter: RMS error {rms(np.array(est) - true_h):.2f} m")

true height at end 3.98 m
accelerometer only: error at end 26.7 m
barometer only: RMS error 0.50 m
Kalman filter: RMS error 0.08 m

แอกเซเลอโรมิเตอร์อย่างเดียวลอยไปหลายสิบเมตรใน 30 วินาทีเพราะ bias เล็ก ๆ ถูกอินทิเกรตสองครั้ง บารอมิเตอร์อย่างเดียวสั่นราวครึ่งเมตร ตัวกรองคาลมานได้ความคลาดเคลื่อนน้อยกว่าทั้งสองแหล่งหลายเท่า เพราะใช้ความเร่งตามการเปลี่ยนแปลงระยะสั้น และใช้บารอมิเตอร์ดึงค่ากลับในระยะยาว

ตัวอย่างที่ 2 ตำแหน่ง GNSS ค่าใดถูกปฏิเสธ

ความแปรปรวนของ innovation ของตำแหน่ง 4 m² (ส่วนเบี่ยงเบนมาตรฐาน 2 m) gate 500 (ข้อมูลจำลอง)

import math

GATE, S = 500, 4.0                             # EK3_POS_I_GATE, m²
innovations = [0.8, -1.5, 3.9, 9.2, 12.0]      # m
for nu in innovations:
    ratio = nu ** 2 / (S * (GATE / 100) ** 2)
    logged = math.sqrt(ratio)                  # ค่าที่เห็นใน XKF4.SP
    print(f"innovation {nu:+5.1f} m: test ratio {ratio:.3f}, logged {logged:.2f} -> {'used' if ratio < 1 else 'REJECTED'}")
print(f"largest accepted innovation: {GATE / 100 * math.sqrt(S):.1f} m")

innovation  +0.8 m: test ratio 0.006, logged 0.08 -> used
innovation  -1.5 m: test ratio 0.022, logged 0.15 -> used
innovation  +3.9 m: test ratio 0.152, logged 0.39 -> used
innovation  +9.2 m: test ratio 0.846, logged 0.92 -> used
innovation +12.0 m: test ratio 1.440, logged 1.20 -> REJECTED
largest accepted innovation: 10.0 m

ค่าที่ห่างจากที่คาดเกิน 5 เท่าของส่วนเบี่ยงเบนมาตรฐานถูกปฏิเสธ ซึ่งปกป้อง EKF จากค่า GNSS ที่กระโดดเพราะสัญญาณสะท้อน แต่ถ้าถูกปฏิเสธต่อเนื่องนาน EKF จะลอยตามแอกเซเลอโรมิเตอร์และอาจรีเซ็ตไปใช้ค่า GNSS ใหม่ การขยาย gate ให้กว้างเพื่อ “ไม่ให้เตือน” จึงไม่ใช่วิธีแก้ ต้องหาสาเหตุที่ค่าไม่สอดคล้อง