Extended Kalman Filter (EKF)

Genişletilmiş Kalman filtresi (EKF), bir dronun yönelimi veya menzil ve açı bildiren bir radarın gördüğü hedefin konumu gibi, hareketi ya da ölçümleri doğrusal olmayan bir sistemin durumunu tahmin eden özyinelemeli bir algoritmadır. Standart Kalman filtresi (KF) yalnızca Gauss gürültülü doğrusal modeller için tam sonuç verir. EKF, her adımda mevcut tahminin çevresinde birinci türevlerden oluşan Jakobiyen matrisleri kullanarak modeli doğrusallaştırır ve ardından bilinen tahmin ve güncelleme denklemlerini uygular. NASA Ames'ten Gerald Smith, Stanley Schmidt ve Leonard McGee, bu yöntemi 1960'ların başında Ay çevresinde dolanacak bir aracın araç içi navigasyonu için geliştirdi; bu çalışma Apollo navigasyon bilgisayarına katkı sağladı. EKF, navigasyon sistemlerinde, GPS alıcılarında, dronlarda ve robotlarda doğrusal olmayan durum tahmini için hâlâ fiilî standarttır.

EKF Nasıl Çalışır#

Kalman filtresi gibi EKF de bir durum tahminini x ve bu tahminin belirsizliğini açıklayan bir kovaryansı P tutar; ancak hareket modeli f ve ölçüm modeli h herhangi bir türevlenebilir fonksiyon olabilir. Her adım iki aşamadan oluşur:

1. Tahmin: Tahmin, doğrusal olmayan hareket modelinden x = f(x) geçer. Filtre, tahmin noktasındaki Jakobiyeni F = ∂f/∂x değerlendirir ve süreç gürültüsü Q ile belirsizliği artırır: P = F P Fᵀ + Q.
2. Güncelleme: Bir ölçüm z gelir. Filtre, tahmin edilen durumdaki ölçüm Jakobiyenini H = ∂h/∂x değerlendirir, artık değerini y = z − h(x) ve kazancı K = P Hᵀ (H P Hᵀ + R)⁻¹ hesaplar; burada R ölçüm gürültüsüdür. Ardından tahmini x = x + K y ve P = (I − K H) P ile düzeltir.

Durum ve tahmin edilen ölçüm, gerçek doğrusal olmayan fonksiyonlardan geçer; doğrusallaştırılmış Jakobiyenleri yalnızca kovaryans ve kazanç kullanır. f ve h doğrusal olduğunda Jakobiyenler model matrislerine eşit olur ve EKF tam olarak Kalman filtresine indirgenir. Modeller belirsizliğin yayılımı boyunca doğrusal olmaya yakınsa yaklaşım iyidir; belirsizlik aralığında keskin biçimde eğrildiklerinde ise doğruluğu düşer.

Menzil ve açı ölçen bir radar klasik örnektir: x ve y doğrultusunda düz bir çizgide hareket eden bir hedef, bu koordinatlarda doğrusaldır; ancak menzil √(x² + y²) ve açı atan2(y, x) doğrusal değildir. Bu EKF, böyle bir hedefi izler ve radarın hiç ölçmediği hızını tahmin eder:

import numpy as np

rng = np.random.default_rng(0)
F = np.eye(4) + np.eye(4, k=2)  # constant velocity, state = [x, y, vx, vy], 1 s per step
Q, R = 1e-4 * np.eye(4), np.diag([1.0**2, 0.02**2])  # process noise; radar noise of 1 m and 0.02 rad
s = np.array([20.0, 50.0, 1.0, -0.5])  # true state
x, P = np.array([20.0, 50.0, 0.0, 0.0]), np.diag([1.0, 1.0, 4.0, 4.0])  # estimate starts with unknown velocity

raw_err, ekf_err = [], []
for _ in range(60):
    s = F @ s
    z = np.array([np.hypot(s[0], s[1]), np.arctan2(s[1], s[0])]) + rng.normal(0, [1.0, 0.02])  # range, bearing
    x, P = F @ x, F @ P @ F.T + Q  # predict
    r = np.hypot(x[0], x[1])
    H = np.array([[x[0] / r, x[1] / r, 0, 0], [-x[1] / r**2, x[0] / r**2, 0, 0]])  # Jacobian of h(x)
    K = P @ H.T @ np.linalg.inv(H @ P @ H.T + R)
    x = x + K @ (z - np.array([r, np.arctan2(x[1], x[0])]))  # update with the nonlinear residual
    P = (np.eye(4) - K @ H) @ P
    raw_err.append(np.hypot(z[0] * np.cos(z[1]) - s[0], z[0] * np.sin(z[1]) - s[1]))
    ekf_err.append(np.hypot(x[0] - s[0], x[1] - s[1]))

print(f"Mean position error: raw radar {np.mean(raw_err):.2f} m, EKF {np.mean(ekf_err):.2f} m")
print(f"Estimated velocity: ({x[2]:.2f}, {x[3]:.2f}) m/s, true (1.00, -0.50)")

Ham menzil ve açı ölçümlerinin her birini x ve y'ye dönüştürmek, 60 adımda ortalama 1,43 m konum hatası verirken EKF tahmini 0,57 m sapar; filtre hızı (1,01, −0,50) m/s olarak bulur.

Sınırlamalar ve Varyantlar#

EKF en iyi tahminci değildir. Başlangıç tahmini çok hatalıysa veya modeller güçlü biçimde doğrusal değilse doğrusallaştırma hataları filtrenin aşırı güvenli olmasına ve ardından sapmasına yol açabilir; bu nedenle mühendisler güvenlik için genellikle süreç gürültüsünü artırır. Jakobiyenler elle ya da otomatik türev alma yoluyla türetilmelidir ve yanlış türev, fark edilmeden oluşan hataların yaygın bir kaynağıdır. Açı veya yönelim gibi değerlerin artıklarının da −π ile π aralığına sarılması gerekir. Bu sorunları ele alan çeşitli varyantlar vardır: İteratif EKF, güncelleme sırasında ölçüm modelini yeniden doğrusallaştırır; hata-durumu EKF'si ise nominal bir durum çevresinde küçük bir düzeltme tahmin eder. Bu yöntem, ataletli navigasyonda kuaterniyonlarla 3B yönelimi izlemek için yaygın olarak kullanılır.

EKF ve Diğer Filtreler#

Filtre seçimi, modellerin doğrusallıktan ne kadar uzak olduğuna ve belirsizliğin biçimine bağlıdır:

• Kalman filtresi: Her iki model de doğrusal olduğunda en iyi ve daha ucuz seçenektir. ByteTrack gibi sınırlayıcı kutu izleyicileri kutu koordinatlarını doğrudan ölçer; bu nedenle doğrusal bir filtre yeterlidir.
• Unscented Kalman filtresi (UKF): Modelleri doğrusallaştırmak yerine küçük ve deterministik bir sigma noktası kümesini doğrusal olmayan fonksiyonlardan geçirir, Jakobiyen gerektirmez ve güçlü eğriliğe sahip modellerde genellikle EKF'den daha doğrudur.
• Parçacık filtresi: İnanç dağılımını çok sayıda ağırlıklı rastgele örnekle temsil eder; bu sayede hangi koridorda olduğundan emin olmayan bir robot gibi çok modlu dağılımları, çok daha yüksek hesaplama maliyetiyle izleyebilir.
• Gizli Markov modeli (HMM): Aynı tahmin ve güncelleme özyinelemesini sonlu sayıdaki ayrık duruma uygular.

Gerçek Dünya Uygulamaları#

• Dronlar ve uçaklar: PX4 otopilotunun EKF2 kestiricisi, IMU, GPS, manyetometre, barometre ve diğer sensör verilerini birleştirerek yönelim, hız, konum, sensör sapmaları, Dünya'nın manyetik alanı ve rüzgâr tahminleri üreten 24 durumlu bir EKF çalıştırır.
• Robotik: ROS'un robot_localization paketi tekerlek odometrisini, IMU'ları ve GPS'i birleştiren EKF ve UKF düğümleri sağlar; EKF-SLAM, eşzamanlı konum belirleme ve haritalama için klasik yaklaşımdır. Çok durumlu kısıt Kalman filtresi (MSCKF) gibi EKF tabanlı görsel-ataletsel odometri, kamera pozunu görüntü özelliklerinden ve IMU okumalarından izler.
• Otomotivde sensör füzyonu: Sürücü destek ve otonom araç sistemleri, çevredeki araçların konumunu ve hızını izlemek için radar menzil ve açı ölçümlerini kamera ve lidar tespitleriyle birleştirir.
• Kamera tabanlı konum belirleme: Tek bir kamera, yalnızca bir nesnenin yönünü ölçer. Tespit edilen nesneleri haritaya yerleştiren işlem hatları, EKF ölçümleri olarak Ultralytics YOLO26 tespitlerini kullanır: Her kutunun merkezi bir piksel konumudur ve kamera modeli aracılığıyla nesnenin 3B konumunun doğrusal olmayan izdüşümüdür. Filtre bu ölçümü radar menziliyle ya da kamera platformunun GPS ve IMU verileriyle birleştirir.