Debugowanie dryfu IMU w UAV i robotyce

Jeden z najczęstszych problemów, z jakimi spotykam się podczas Rescue Sprints u klientów budujących drony lub roboty mobilne, to niestabilna estymacja stanu. Dron zaczyna „pływać” w zawisie, robot gubi orientację po kilkunastu metrach, a w logach widać rosnący dryf.

Najczęstszą reakcją zespołu jest: „Zmieńmy zyski w filtrze Kalmana”. Niestety, w 90% przypadków problem leży gdzie indziej: filtr tylko uczciwie pokazuje, że coś wcześniej w łańcuchu (fizyka, timing, dane wejściowe) jest zepsute. Oto moja sprawdzona ścieżka debugowania takich problemów, razem z matematyką, którą warto znać, żeby rozumieć dlaczego te kroki działają, a nie tylko że działają.

1. Wyklucz problemy sprzętowe (Hardware First)

Zanim dotkniesz kodu filtra EKF (Extended Kalman Filter), upewnij się, że dane wejściowe nie są zepsute na poziomie fizycznym. To najczęściej ignorowany etap, bo "przecież sensor jest markowy", a jednak montaż, temperatura i okablowanie potrafią zniszczyć nawet najlepszy czujnik.

Wibracje: Czy IMU jest odpowiednio izolowane mechanicznie? Uruchom silniki bez śmigieł i zrób log z surowych danych akcelerometru. Jeśli widzisz aliasing lub potężny szum w wysokich częstotliwościach, musisz poprawić tłumienie wibracji (damping) lub dodać sprzętowy/software'owy filtr dolnoprzepustowy (LPF).
Zakłócenia EMI: Czy magnetometr (kompas) nie jest za blisko przewodów wysokoprądowych zasilających ESC? Skoki prądu generują pole magnetyczne, które niszczy odczyty heading'u.
Zasilanie: Sprawdź oscyloskopem zasilanie na pinach IMU. Szumy na szynie 3.3V przenoszą się bezpośrednio na błędy pomiarowe MEMS.

Dla dociekliwych: matematyka wibracji

Aliasing to nie "jakiś szum". To bezpośrednia konsekwencja kryterium Nyquista-Shannona. Jeśli IMU próbkuje z częstotliwością f_s, każdy sygnał powyżej częstotliwości Nyquista zostanie błędnie "złożony" w dół, do pasma, które właśnie analizujesz:

f_nyquist = f_s / 2

Przykład z życia: śmigła drona wibrują często w paśmie 800-1500 Hz (zależnie od RPM i liczby łopat). Jeśli próbkujesz IMU z f_s = 1000 Hz bez analogowego filtru wejściowego, wibracja 1200 Hz "zawinie się" i pojawi się jako fałszywy sygnał ok. 200 Hz, nie do odróżnienia od realnego ruchu.

Drugi parametr do sprawdzenia w datasheet: gęstość szumu (Noise Density, ND), podawana w µg/√Hz dla akcelerometru. Całkowity szum RMS w danym paśmie liczysz jako:

noise_rms = ND × √(BW)

gdzie BW to szerokość pasma filtru dolnoprzepustowego w Hz. Dla typowego MEMS IMU klasy konsumenckiej (ND ≈ 150 µg/√Hz) i pasma 100 Hz wychodzi ok. 1.5 mg szumu RMS, czyli losowy błąd rzędu 0.015 m/s² na każdy pojedynczy pomiar, zanim jeszcze zaczniesz cokolwiek całkować.

Wniosek praktyczny: filtr dolnoprzepustowy to nie kosmetyka ani "domyślne ustawienie z przykładu". To bezpośredni regulator ilości szumu, który wpuszczasz do swojego estymatora.

2. Sprawdź synchronizację czasu (Timestamping)

Sensor fusion działa tylko wtedy, gdy dane z różnych czujników (IMU, GPS, kamera) są idealnie zsynchronizowane w czasie. To jeden z tych błędów, które nie widać w statycznych testach na stole. Wychodzi tylko w ruchu, i to zwykle podczas agresywnych manewrów.

Czy używasz sprzętowych przerwań (Data Ready) z IMU, czy odpytujesz je w pętli?
Czy timestampy z GPS i IMU korzystają z tego samego zegara referencyjnego (np. systemowego timera RTOS)?
Opóźnienie rzędu 50-100 ms między danymi z kamery (odometria wizyjna) a IMU zniszczy każdą estymację.

Policzmy to na konkretnym przykładzie. Dron wykonuje manewr z prędkością kątową ω = 200°/s (typowe dla drona wyścigowego w agresywnym flipie). Błąd orientacji wynikający z niezsynchronizowanych timestampów rośnie w przybliżeniu liniowo z opóźnieniem:

Δθ_error ≈ ω × Δt_latency

Przy opóźnieniu Δt = 50 ms błąd orientacji to już Δθ ≈ 10°. Wystarczy, żeby fuzja z kamerą (VIO) zaczęła rozjeżdżać się z rzeczywistością i generować dryf pozycyjny, który później niesprawiedliwie zrzucisz na filtr Kalmana, a problem siedział w brakującym znaczniku czasu.

3. Walidacja surowych danych (Raw Data Sanity Check)

Zaloguj surowe dane i sprawdź je w narzędziu takim jak Foxglove, PlotJuggler lub zwykłym skrypcie w Pythonie.

Połóż urządzenie płasko na stole. Czy akcelerometr pokazuje dokładnie 1g (9.81 m/s²) na osi Z i 0 na X/Y?
Czy żyroskop pokazuje 0 deg/s na wszystkich osiach? Jak duży jest szum bazowy (bias)?
Zrób obrót o dokładnie 90 stopni wokół osi Z. Czy całka z żyroskopu daje 90 stopni?

Dla dociekliwych: wariancja Allana

Test "połóż na stole" powie Ci, czy sensor jest oczywiście zepsuty. Nie powie Ci, jak dobry jest w długim horyzoncie czasowym. Do tego służy analiza wariancji Allana (Allan Variance), technika zapożyczona z metrologii zegarów atomowych, którą IEEE (std. 952/647) uczynił standardem charakteryzacji IMU.

Ideę można uprościć (zapis przybliżony) do: bierzesz długi log statycznego żyroskopu, dzielisz go na okna czasowe o długości τ, liczysz wariancję różnic między średnimi kolejnych okien i powtarzasz dla wielu wartości τ:

σ²(τ) = 1 / (2(N−1)τ²) · Σᴺ⁻¹ (θ̄_{i+1} − θ̄_i)²

Wynik rysujesz na wykresie log-log: σ(τ) w funkcji τ. Nachylenie krzywej w różnych regionach mówi Ci wprost, jaki typ szumu dominuje:

nachylenie −1/2 oznacza Angle Random Walk (ARW), czyli szum biały żyroskopu, dominujący dla krótkich τ
nachylenie 0 (lokalne minimum krzywej) oznacza Bias Instability, czyli najlepszy osiągalny punkt stabilności sensora
nachylenie +1/2 oznacza Rate Random Walk (RRW), czyli powolny dryf biasu, dominujący dla długich τ

Orientacyjnie: konsumenckie MEMS IMU mają ARW rzędu 0.1-0.3°/√h i bias instability 2-10°/h. Nawigacyjne IMU klasy tactical to już 0.001-0.01°/√h, różnica dwóch-trzech rzędów wielkości. To ta liczba, nie kod filtra, decyduje o tym, jak długo system może latać "na ślepo" (dead reckoning) bez korekcji GPS czy VIO.

Jeśli minimum Twojej krzywej Allana leży wyżej niż specyfikacja producenta, masz problem sprzętowy (montaż, temperatura, zasilanie), nie programistyczny.

4. Dopiero teraz patrz na Filtr Kalmana

Jeśli sprzęt, czas i surowe dane są w porządku, czas na EKF. Warto tu zrozumieć nie tylko "co zmienić", ale co faktycznie liczy filtr w każdym kroku.

Covariance Tuning: Czy macierze kowariancji szumu pomiarowego (R) i szumu procesu (Q) odpowiadają rzeczywistości? Nie kopiuj ich z tutoriali w internecie. Wylicz wariancję z logów dla Twojego konkretnego sensora, najlepiej z analizy Allana opisanej wyżej.
Innowacje (Innovations): W EKF kluczowym wskaźnikiem są „innowacje” (różnica między przewidywanym a zmierzonym stanem). Jeśli innowacje dla GPS stale rosną i są odrzucane przez filtr, oznacza to, że model matematyczny rozjeżdża się z rzeczywistością.

Dla dociekliwych: równania EKF i test spójności NIS

Pełny cykl Extended Kalman Filter to dwa kroki, wykonywane w każdej iteracji.

Predykcja (propagacja stanu i niepewności w czasie, na podstawie modelu ruchu f i wejść IMU u):

x̂_k|k-1 = f(x̂_k-1|k-1, u_k) P_k|k-1 = F_k · P_k-1|k-1 · F_kᵗ + Q_k

Aktualizacja (korekcja po nadejściu pomiaru z_k, np. z GNSS):

Najbardziej niedocenianym elementem tego równania jest innowacja y_k i jej kowariancja S_k. Z nich liczy się test spójności filtra: Normalized Innovation Squared (NIS), znany też jako odległość Mahalanobisa do kwadratu:

NIS_k = y_kᵗ · S_k⁻¹ · y_k

Przy prawidłowo dobranym modelu i macierzach R/Q, NIS podlega rozkładowi χ² (chi-kwadrat) z liczbą stopni swobody równą wymiarowi pomiaru. Dla pomiaru 3D (np. fix GNSS) i poziomu ufności 95% próg wynosi χ² = 7.815. Jeśli Twoje NIS regularnie przekracza ten próg, filtr nie jest "źle wytuningowany". On Ci mówi, że model fizyczny (f, h) nie odpowiada rzeczywistości. To najczęściej ignorowany sygnał w całym sensor fusion, a jeden z najbardziej wiarygodnych.

5. Dlaczego dryf jest matematycznie nieunikniony

Tu dochodzimy do rzeczy, którą warto zrozumieć na poziomie intuicji, nawet jeśli nigdy nie policzysz tego ręcznie: nawet perfekcyjnie zaimplementowany EKF na perfekcyjnie zsynchronizowanych danych będzie dryfować, jeśli nawigacja opiera się na czystym dead reckoning (integracji IMU bez zewnętrznej korekcji z GPS, VIO czy innego absolutnego źródła).

Wynika to z teorii błądzenia losowego (random walk). Błąd pozycji z podwójnie zintegrowanego szumu białego akcelerometru rośnie nie liniowo z czasem, a w przybliżeniu z wykładnikiem 3/2:

σ_pozycja(t) ∝ σ_accel · t^(3/2) / √3

To zapis rzędu wielkości, nie precyzyjny wzór inżynierski, ale pokazuje istotę problemu: 10 sekund bez korekcji GPS/VIO to nie "dwa razy gorzej" niż 5 sekund, to prawie trzy razy gorzej. Świadomość tego wykładnika zmienia sposób, w jaki projektuje się failsafe i okna ważności estymacji w systemach autonomicznych.

Podsumowanie

Debugowanie sensor fusion to proces eliminacji, poparty konkretną matematyką na każdym etapie: kryterium Nyquista i gęstość szumu na poziomie hardware, budżet opóźnień przy timingu, wariancja Allana przy walidacji surowych danych, równania EKF i test NIS przy samym filtrze. Zaczynaj od warstwy fizycznej, przejdź przez timing i surowe dane, a pełną matematykę filtra zostaw na koniec. W tej kolejności, w 90% przypadków, problem znajdziesz zanim do niej dojdziesz.

Jeśli Twój zespół utknął z problemem dryfującej estymacji, nawigacji lub integracji sensorów, daj znać. W ramach Rescue Sprint w 1-2 tygodnie diagnozuję takie problemy, analizuję logi (łącznie z analizą Allana i statystykami NIS) i dostarczam działające rozwiązanie na kodzie klienta.

Jak debugować dryf estymacji (IMU) w systemach UAV i robotyce?