Filtrowanie Kalmana

Dwa niedoskonałe źródła, jedno lepsze rozwiązanie

Załóżmy, że chcesz poznać dokładną pozycję i prędkość drona. Masz dwie metody odgadywania, ani jedna z nich nie jest niezawodna sama w sobie. Twój model może powiedzieć Ci, na podstawie ostatniego znanego stanu i wprowadzonych przez Ciebie sterowań, gdzie dron powinien się teraz znajdować – dokładne na krótkich dystansach, ale powoli dryfuje w miarę kumulowania się nieujętych prądów wiatru i niedoskonałości silników. Twój czujnik – GPS, np. – mierzy pozycję bezpośrednio, ale jest szumowy (zwykle ±3 m) i aktualizuje się wolno (1-10 Hz). Żadne z tych źródeł samo w sobie nie jest wystarczające. Filtr Kalmana, opracowany przez Rudolfa Kálmana w 1960 roku i słynnie wykorzystywany do nawigacji Apolla na Księżyc, to matematycznie optymalny sposób na połączenie obu: wycenia go w odwrotnej proporcji do aktualnego stopnia niepewności, ufając czujnikowi bardziej, gdy model się przesunął, i modelowi bardziej, gdy czujnik jest szumowy.

Podstawy modelu stanowego

Liniowy układ dynamiczny opisuje się dwoma równaniami – jednym definiującym ewolucję stanu i drugim określającym, jak ten stan jest obserwowany:

x_k = A · x_(k-1) + B · u_k + w_k    // process model
y_k = C · x_k + v_k                   // measurement model

x  state vector (position, velocity, ...)     A  state-transition matrix
B  control-input matrix; u  control input     C  observation matrix
w ~ N(0, Q)  process noise                    v ~ N(0, R)  measurement noise

1D constant-velocity example (state = [position, velocity]):
  A = [[1, dt], [0, 1]]      C = [1, 0]   // we only measure position

Cykl predykcji-aktualizacji

W każdym kroku czasu wykonuje się dwie fazy sekwencyjnie. Predykcja projektuje stan z góry, wykorzystując model sam w sobie, i pozwala jej niepewności rosnąć: x̂⁻ₖ = A·x̂ₖ₋₁ + B·uₖ, a macierz błędu rozkładu P⁻ₖ = A·Pₖ₋₁·Aᵀ + Q rośnie dzięki szumowi procesowemu Q. Aktualizacja następnie uwzględnia nową pomiarową i oblicza wygranej współczynnik Kalmana K = P⁻·Cᵀ·(C·P⁻·Cᵀ + R)⁻¹, koryguje oszacowanie stanu używając reszty pomiędzy pomiarem a predykcją, x̂ₖ = x̂⁻ₖ + K·(yₖ − C·x̂⁻ₖ), i skraca macierz rozkładu odpowiednio, Pₖ = (I − K·C)·P⁻ₖ. Termin (yₖ − C·x̂⁻ₖ) nazywany jest innowacją – czyli tym, jak bardzo filtr zaskoczony jest nowym odczytem – a K decyduje, ile tej niespodzianki w ogóle uwierzyć.

Odczyt wygranej

W przypadku skalarnym wygrana upada do pojedynczego, bardzo czytelnego stosunku:

K = P⁻ / (P⁻ + R)

K → 1 wraz z wzrostem P⁻/R do nieskończoności (model bardzo niepewny, pomiar dokładny) → ufaj czujnikowi

K → 0 wraz ze spadkiem P⁻/R do zera (model dokładny, czujnik szumowy) → ufaj modelowi

Przykład: niepewność modelu P⁻ = 9 m², wariancja GPS R = 4 m²

K = 9 / (9 + 4) ≈ 0.69 → skłony się głównie do odczytu z GPS

Przydatne uproszczenie praktyczne: gdy A, C, Q i R pozostają stałe w czasie, kowariancja P konwerguje do ustalonej wartości bezwzględnej po jedynie kilku iteracjach, a można ją obliczyć offline – istotna oszczędność dla systemów wbudowanych, które nie mogą sobie pozwolić na wykonywanie odwrotności macierzy w każdym cyklu.

K = P⁻ / (P⁻ + R)

K → 1   as P⁻/R → ∞   (model very uncertain, sensor accurate)  → trust sensor
K → 0   as P⁻/R → 0   (model accurate, sensor noisy)           → trust model

Example: model uncertainty P⁻ = 9 m², GPS variance R = 4 m²
K = 9 / (9 + 4) ≈ 0.69  →  lean mostly toward the GPS reading

Kiedy świat nie jest liniowy: EKF i UKF

Standardowy filtr zakłada, że zarówno A jak i C są macierzami stałymi, co powoduje naruszenie, gdy dynamika staje się naprawdę nieliniowa – robot, którego kierunek zmiany zmienia jego prędkość sinusoidalnie, lub czujnik opartego na kącie. Rozszerzony Filtr Kalmana (EKF) zachowuje tę samą strukturę predykcji-aktualizacji, ale w każdym kroku wykorzystuje ich macierze Jacobiego F = ∂f/∂x i H = ∂h/∂x, aby zrelinearizować funkcje nieliniowe f i h, obliczone przy aktualnej szacowanej wartości – jest to domyślny wybór w większości systemów robotyki, w tym pakietu robot_localization ROS oraz typowych magistral SLAM. Gdy nieliniowość jest wystarczająco poważna, aby pierwszorzędne przybliżenie Jacobiego zaczęło się rozbiegać, Filtr Kalmana Nieprzeskalowany (UKF) propaguje mały deterministyczny zestaw punktów próbnych – punkty sigma – bezpośrednio przez prawdziwą funkcję nieliniową, przechwytując dystrybucję z dokładnością trzeciego rzędu bez konieczności obliczania Jacobiego.

Gdzie faktycznie działa

Oprócz sterowania statkami kosmicznymi, ten sam pętla predykcji-aktualizacji pojawia się niemal wszędzie tam, gdzie potrzebne jest łączenie danych z czujników generujących szum z modelem: fuzja GPS i IMU w smartfonach i dronach, śledzenie celów radarowych, wieloobiektowe śledzenie w kanałach przetwarzania obrazu takich jak SORT i DeepSORT (gdzie filtr Kalmana przewiduje ruch każdej ramki ogranicznika), szacowanie ukrytej zmienności z szumów finansowych szeregów czasowych oraz nawet usuwanie szumów z sygnału mowy z mikrofonu. Jego trwały urok polega na tym, że nie wymaga danych treningowych, działa w zamknięciu poprzez kilka operacji macierzowych na krok i jest dowodem optymalnego estymatora liniowego pod warunkiem szumu gaussowskiego – właściwości, które sprawiają, że trudno go przebić w zastosowaniach wbudowanych i w czasie rzeczywistym, nawet w erze alternatyw opartych na uczeniu maszynowym.

Frequently asked questions

Wypróbuj na żywo

Wszystko powyżej działa bezpośrednio w Twojej przeglądarce — otwórz the simulation i zmieniaj parametry podczas działania. Nic nie jest instalowane ani przesyłane na serwer, cały model działa w jednej karcie.