Прив'язування фільтрів Кальмана

Моя команда будує робота, щоб самостійно орієнтуватися у зовнішніх умовах. Нещодавно ми отримали новий інтегрований датчик IMU / GPS, який, очевидно, робить деяку розширену фільтрацію Kalman на мікросхемі. Це дає швидкість нахилу, кочення та пронизування, швидкості на північ, схід та вниз, а також широту та довготу.

Однак у нас також є деякі кодери, прикріплені до наших коліс, які забезпечують лінійну та кутову швидкість. Перш ніж ми отримали цей новий датчик IMU / GPS, ми створили власний EKF, щоб оцінити стан за допомогою кодерів та деяких інших датчиків низької вартості. Ми хочемо використовувати фільтр цього нового датчика на мікросхемі, але також включити наші кодери в суміш.

Чи є якась проблема з ланцюжком фільтрів? Я маю на увазі те, що ми використовуємо висновок датчика IMU / GPS на мікросхемі EKF як оновлення до власного EKF так само, як ми використовуємо дані, прочитані з кодерів, як оновлення до нашого EKF. Мені це здається розумним, але мені було цікаво, що зазвичай слід зробити в цьому випадку.

Ви можете використовувати INS / GPS як оновлення до результатів свого першого EKF. Це насправді не ланцюжок, а просто обумовлення оцінки на основі додаткової інформації з INS / GPS.

Припустимо, у нас є такі функції:

$x_{t+1|t}$ , = EKF_PREDICT ( , , ), для входів у вигляді стану , коваріації та керуючих входів (оцінюється за одометрією) .$P_{t+1|t}$$x_t$$P_t$$u_t$$x$$P$$u_t$

і

$x_{t+1|t+1}$ , = EKF_UPDATE ( , , ). $P_{t+1|t+1}$$x_{t+1|t}$$P_{t+1|t}$$\hat{x}_{t+1}$

Оцінками датчиків є . У нас є такі речі, як:$\hat{x}_{t+1}$

$\hat{x}^{gps}_{t+1} = f(GPS)$

$\hat{x}^{map}_{t+1} = f(map)$

$\hat{x}^{ins}_{t+1} = f(INS)$

і т. д. для всіх інших способів оцінки стану робота. Тож функція EKF_UPDATE для всіх цих сенсорів є досить хорошою.

Ваш цикл буде приблизно таким:

за весь час$t$

• Нехай - поточна оцінка одометрії / кінематики пози, а - шум цієї оцінки.$u_t$$R_u$

• $x_{t+1|t}$ , = EKF_PREDICT ( , , , )$P_{t+1|t}$$x_t$$P_t$$u_t$$R_u$

• для всіх датчиків ,$S$

  • Нехай - оцінка пози від цього датчика, а - шум від цієї оцінки$\hat{x}^{S}_{t+1}$$R_{S}$

  • $x_{t+1|t+1}$ , = EKF_UPDATE ( , , ). $P_{t+1|t+1}$$x_{t+1|t}$$P_{t+1|t}$$\hat{x}_{t+1}, R_S$

  • кінець-для

• кінець-для

Деякі застереження:

• Оскільки ми використовуємо EKF, немає гарантії, що оцінка не залежить від впорядкування оновлень. Тобто, якщо ви робите INS, то GPS, отримана оцінка може бути іншою, ніж якщо ви оновлюєте GPS, то INS. Це в основному не є великою справою, але для фільтра потрібно буде значно більше налаштування.

• Зауважте, що ваш INS має ухил і дрейф, що може вплинути на вашу довгострокову надійність. GPS може вам тут дуже допомогти . Більшість літератури одночасно оцінює зміщення та дрейф в INS.