Extended Kalman Filter (EKF)

مرشح كالمان الممتد (EKF) خوارزمية تكرارية تقدّر حالة نظام تكون حركته أو قياساته غير خطية، مثل اتجاه طائرة مسيّرة أو موضع هدف يرصدُه رادار يبلّغ عن المدى والسمت. لا يكون مرشح كالمان (KF) القياسي دقيقًا إلا للنماذج الخطية ذات الضوضاء الغاوسية. ويتعامل EKF مع النماذج غير الخطية عبر خطيّتها حول التقدير الحالي في كل خطوة باستخدام مصفوفات يعقوبي للمشتقات الأولى، ثم تطبيق معادلات التنبؤ والتحديث المألوفة. طوّره جيرالد سميث وستانلي شميدت وليونارد ماكغي في مركز أيمز التابع لناسا في أوائل ستينيات القرن العشرين من أجل الملاحة على متن مركبة تحلق حول القمر، وأسهم هذا العمل في حاسوب الملاحة الخاص ببرنامج أبولو. ولا يزال المعيار الفعلي لتقدير الحالة غير الخطية في أنظمة الملاحة ومستقبلات GPS والطائرات المسيّرة والروبوتات.

آلية عمل EKF#

مثل مرشح كالمان، يحتفظ EKF بتقدير للحالة x وتغاير P يصف عدم اليقين فيها، لكن يمكن أن يكون نموذج الحركة f ونموذج القياس h أي دالتين قابلتين للاشتقاق. وتتألف كل خطوة من مرحلتين:

1. التنبؤ: يمر التقدير عبر نموذج الحركة غير الخطي x = f(x). ويقيّم المرشح مصفوفة يعقوبي F = ∂f/∂x عند التقدير، ويزيد عدم اليقين باستخدام ضوضاء العملية Q: P = F P Fᵀ + Q.
2. التحديث: يصل قياس z. ويقيّم المرشح مصفوفة يعقوبي للقياس H = ∂h/∂x عند الحالة المتوقعة، ويحسب المتبقي y = z − h(x) والكسب K = P Hᵀ (H P Hᵀ + R)⁻¹، حيث تمثل R ضوضاء القياس، ثم يصحح التقدير باستخدام x = x + K y وP = (I − K H) P.

تمر الحالة والقياس المتوقعان عبر الدوال غير الخطية الحقيقية؛ ولا تستخدم مصفوفتا يعقوبي الخطيتان إلا في حساب التغاير والكسب. عندما يكون f وh خطيين، تتساوى مصفوفتا يعقوبي مع مصفوفتَي النموذج، ويختزل EKF تمامًا إلى مرشح كالمان. يكون التقريب جيدًا عندما تكون النماذج قريبة من الخطية على امتداد نطاق عدم اليقين، وتتراجع دقته عندما تنحني النماذج بشدة ضمن ذلك النطاق.

الرادار الذي يقيس المدى والسمت هو الحالة الكلاسيكية: فالهدف المتحرك في خط مستقيم على المحورين x وy خطي في تلك الإحداثيات، لكن المدى √(x² + y²) والسمت atan2(y, x) ليسا كذلك. يتتبع EKF هذا الهدف ويقدّر سرعته، التي لا يقيسها الرادار مطلقًا:

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)")

يؤدي تحويل كل قيمة خام للمدى والسمت إلى إحداثيي x وy إلى متوسط خطأ في الموضع قدره 1.43 m خلال 60 خطوة، بينما يبلغ خطأ تقدير EKF مقدار 0.57 m، ويستعيد المرشح السرعة بقيمة (1.01, −0.50) m/s.

القيود والأنواع المختلفة#

ليس EKF مقدّرًا أمثلًا. فإذا كان التقدير الأولي بعيدًا جدًا عن القيمة الصحيحة أو كانت النماذج غير خطية بشدة، فقد تجعل أخطاء الخطّيّة المرشح مفرط الثقة ثم تتسبب في تباعده، لذا يضخم المهندسون عادةً ضوضاء العملية احتياطًا. ويجب اشتقاق مصفوفات يعقوبي يدويًا أو باستخدام الاشتقاق التلقائي، ويُعد الاشتقاق الخاطئ مصدرًا شائعًا للإخفاقات الصامتة. كما يلزم تطويق متبقيات الزوايا، مثل السمت أو الاتجاه، ضمن النطاق من −π إلى π. وتعالج عدة أنواع مختلفة هذه المشكلات: يعيد EKF التكراري خطيّة نموذج القياس أثناء التحديث، بينما يقدّر EKF لحالة الخطأ تصحيحًا صغيرًا حول حالة اسمية، وهي طريقة شائعة لتتبع الاتجاه ثلاثي الأبعاد باستخدام الكواتيرنيونات في الملاحة بالقصور الذاتي.

مقارنة EKF بالمرشحات الأخرى#

يعتمد اختيار المرشح على مدى ابتعاد النماذج عن الخطية وشكل عدم اليقين:

• مرشح Kalman: أمثل وأقل كلفة عندما يكون كلا النموذجين خطيًا. تقيس متتبعات مربعات الإحاطة مثل ByteTrack إحداثيات المربعات مباشرةً، لذا يكفيها مرشح خطي.
• مرشح كالمان غير المنقح (UKF): يمرر مجموعة صغيرة وحتمية من نقاط سيغما عبر الدوال غير الخطية بدلًا من خطيّتها، ولا يحتاج إلى مصفوفات يعقوبي، ويكون عادةً أدق من EKF مع النماذج شديدة الانحناء.
• مرشح الجسيمات: يمثل الاعتقاد بعدد كبير من العينات العشوائية الموزونة، ما يتيح له تتبع التوزيعات متعددة الأنماط، مثل حالة روبوت لا يعرف أيًّا من ممرين يسلك، لكن بتكلفة حوسبية أعلى بكثير.
• نموذج ماركوف المخفي (HMM): يطبق التكرار نفسه للتنبؤ والتحديث على مجموعة محدودة من الحالات المتقطعة.

تطبيقات عملية#

• الطائرات المسيّرة والطائرات: يشغّل مقدّر EKF2 في الطيار الآلي PX4 مرشح EKF ذا 24 حالة، يدمج بيانات IMU وGPS ومقياس المغناطيسية ومقياس الضغط ومستشعرات أخرى لتقدير الاتجاه والسرعة والموضع وانحيازات المستشعرات والمجال المغناطيسي للأرض والرياح.
• الروبوتات: توفر حزمة robot_localization في ROS عُقد EKF وUKF تدمج قياسات عجلات الروبوت ووحدات IMU وGPS، ويُعد EKF-SLAM النهج الكلاسيكي لـالتوطين ورسم الخرائط المتزامنين. ويتتبع التوطين البصري بالقصور الذاتي القائم على EKF، مثل مرشح كالمان متعدد الحالات المقيدة (MSCKF)، وضعية الكاميرا من ميزات الصورة وقراءات IMU.
• دمج المستشعرات في السيارات: تدمج أنظمة مساعدة السائق والمركبات ذاتية القيادة مدى الرادار وسمته مع اكتشافات الكاميرات وLiDAR لتتبع موضع السيارات المحيطة وسرعتها.
• التوطين بالاعتماد على الكاميرا: لا تقيس الكاميرا الواحدة سوى اتجاه الجسم. وتستخدم المسارات التي تحدد مواقع الأجسام المكتشفة على خريطة اكتشافات Ultralytics YOLO26 كقياسات EKF: فمركز كل صندوق موضعٌ بالبكسل، وهو إسقاط غير خطي لموضع الجسم ثلاثي الأبعاد عبر نموذج الكاميرا، يدمجه المرشح مع مدى الرادار أو بيانات GPS وIMU الخاصة بمنصة الكاميرا.