Filtr komplementarny orientacji

Filtr komplementarny łączy pomiary, których błędy dominują w różnych pasmach częstotliwości. Żyroskop dobrze opisuje szybkie zmiany orientacji, lecz po całkowaniu dryfuje. Akcelerometr może dostarczyć wolnego odniesienia do grawitacji, ale podczas manewru mierzy również przyspieszenie translacyjne. Magnetometr daje odniesienie kierunkowe, lecz reaguje na pole instalacji. Poprawny filtr nie „uśrednia trzech sensorów”; świadomie rozdziela ich role i odrzuca obserwacje, gdy przestają spełniać model.

Spis treści#

Idea komplementarności
Filtr jednej osi
Związek α z czasem i pasmem
Dlaczego roll/pitch osobno nie wystarczą
Obserwator na SO(3)
Błąd z iloczynu wektorowego
Estymacja biasu żyroskopu
Wersja kwaternionowa
Filtr Mahony’ego
Filtr Madgwicka
Akcelerometr jako obserwacja
Magnetometr jako obserwacja
Dobór wzmocnień
Czas próbkowania i opóźnienie
Normalizacja i warunki brzegowe
Inicjalizacja orientacji
Odrzucanie zaburzeń
Implementacja krok po kroku
Testy
Diagnostyka w locie
Kiedy użyć EKF
Powiązane tematy
Przypisy

Idea komplementarności#

Niech θg będzie kątem uzyskanym z całkowania żyroskopu, a θa kątem wynikającym z kierunku grawitacji. Klasyczny zapis w dziedzinie częstotliwości ma postać:

θ̂(s) = H_HP(s) θg(s) + H_LP(s) θa(s)

Jeżeli H_HP + H_LP = 1, filtry są komplementarne. Dla pierwszego rzędu:

H_LP(s) = 1 / (τs + 1)
H_HP(s) = τs / (τs + 1)

Niskie częstotliwości pochodzą głównie z odniesienia absolutnego, a szybki ruch z żyroskopu. τ określa granicę kompromisu. Nie oznacza to, że akcelerometr jest „prawdziwy” poniżej jednej konkretnej częstotliwości. W locie przyspieszenie może utrzymywać się długo, dlatego potrzebne są detektory wiarygodności.

Komplementarność dotyczy charakterystyki błędów. Żyroskop ma bias i dryf niskoczęstotliwościowy; akcelerometr ma zakłócenia ruchem i drganiami. Jeśli oba sensory są nasycone albo źle skalibrowane, filtr nie odzyska informacji.

Filtr jednej osi#

Popularna dyskretna postać wynosi:

θ_pred = θ̂[k] + (ω[k] - b[k]) Δt
θ̂[k+1] = α θ_pred + (1 - α) θa[k]

Pierwsza linia propaguje orientację prędkością kątową. Druga powoli przyciąga wynik do obserwacji z akcelerometru. Ten zapis jest użyteczny edukacyjnie i dla układu o małych kątach. Ma jednak ukryte założenia: stały krok czasu, poprawne znaki, zgodne osie, brak sprzężeń 3D oraz prawidłowo wyliczony θa.

Nie należy liczyć roll i pitch z akcelerometru, a następnie filtrować każdy kąt niezależnie przy dowolnym ruchu 3D. Kąty Eulera zależą od kolejności obrotów i mają osobliwości. Blisko pitch ±90° małe zakłócenie wektora może wywołać dużą zmianę yaw/roll.

Mimo ograniczeń filtr jednej osi jest dobrym testem toru danych. Pozwala sprawdzić timestamp, skalę żyroskopu, znak grawitacji i wpływ τ na odpowiedź. Nie powinien być jednak bezrefleksyjnie rozszerzany na pełny autopilot.

Związek α z czasem i pasmem#

Dla dyskretyzacji prostego filtra dolnoprzepustowego można przyjąć:

α = τ / (τ + Δt)

Jeżeli częstotliwość graniczna ma wynosić fc, to:

τ = 1 / (2πfc)

Stała α wpisana na sztywno zmienia fizyczne pasmo, gdy zmienia się ODR. Przy α=0,98 i 100 Hz zachowanie jest inne niż przy 1 kHz. Dlatego konfiguracja powinna używać czasu lub częstotliwości, a współczynnik obliczać z rzeczywistego Δt albo z kontrolowanego nominalnego ODR.

Duże wahania Δt zmieniają wzmocnienie i integrację. Jeżeli próbki z FIFO są odczytane jednym transferem, każda powinna dostać własny czas próbkowania, nie czas końca transferu.

Dlaczego roll/pitch osobno nie wystarczą#

Orientacje należą do grupy obrotów SO(3), a składanie obrotów nie jest przemienne. Wektor prędkości kątowej w body nie jest po prostu [roll_dot, pitch_dot, yaw_dot]. Relacja zależy od bieżącej orientacji i wybranej sekwencji Eulera.

Estymator pracujący bezpośrednio na macierzy obrotu lub kwaternionie:

nie ma osobliwości gimbal lock reprezentacji stanu;
składa obroty w prawidłowej kolejności;
może używać błędu między przewidywanym i zmierzonym wektorem;
łatwo normalizuje reprezentację;
nadal wymaga jawnej konwencji kierunku transformacji.

Mahony, Hamel i Pflimlin sformułowali nieliniowe obserwatory bezpośrednio na SO(3), z analizą stabilności. Praktyczne filtry AHRS często implementują podobną korekcję w kwaternionach.

Obserwator na SO(3)#

Niech R̂ opisuje estymowaną orientację, a ωm pomiar żyroskopu. Bez korekcji orientacja ewoluuje:

R̂_dot = R̂ [ωm - b̂]×

[v]× jest macierzą skośno-symetryczną realizującą iloczyn wektorowy. Obserwacje znanych kierunków dodają sygnał korekcyjny e:

R̂_dot = R̂ [ωm - b̂ + kP e]×
b̂_dot = -kI e

Dokładne znaki zależą od tego, czy R obraca navigation→body czy body→navigation oraz jak zdefiniowano e. Nie wolno kopiować równań bez przeniesienia konwencji. Test prostego przechylenia musi potwierdzić, że korekcja zmniejsza błąd, a nie tworzy dodatnie sprzężenie.

Obserwator jest nieliniowym odpowiednikiem komplementarnego rozdzielenia pasm. kP szybko koryguje orientację, a kI powoli estymuje bias żyroskopu.

Błąd z iloczynu wektorowego#

Jeżeli znamy kierunek odniesienia v_ref w ramce świata i mierzymy odpowiadający mu znormalizowany wektor v_meas w body, przewidywany kierunek wynosi:

v_pred = R̂ v_ref

Wektor błędu orientacji można zbudować jako:

e = v_meas × v_pred

lub z przeciwną kolejnością, zależnie od konwencji. Dla małego błędu iloczyn wskazuje oś i przybliżoną amplitudę korekcji. Grawitacja dostarcza dwóch obserwowalnych stopni swobody: obrót wokół samego wektora grawitacji pozostaje nieokreślony. Pole magnetyczne lub inne odniesienie niekolinearne dodaje informację o yaw.

Dla wielu wektorów błędy można sumować z wagami:

e = Σ wi (v_meas_i × v_pred_i)

Waga nie powinna wynikać wyłącznie z szumu sensora. Musi uwzględniać chwilową zgodność z modelem: akcelerometr podczas manewru i magnetometr przy dużym prądzie wymagają zmniejszenia wpływu.

Estymacja biasu żyroskopu#

Człon całkujący korekcję może śledzić wolny bias:

b̂[k+1] = b̂[k] - kI e Δt

Znak ponownie zależy od definicji. Bias powinien mieć ograniczenia fizyczne, anti-windup i kontrolę obserwowalności. Jeżeli akcelerometr jest zakłócony stałym przyspieszeniem, całkowanie błędu może „nauczyć” fałszywy bias. Podobnie zakłócony magnetometr może wprowadzić bias yaw.

Integrację można zatrzymać, gdy:

norma akcelerometru odbiega od oczekiwanej;
wariancja lub clipping są duże;
pole magnetyczne nie przechodzi testów;
prędkość kątowa przekracza zakres kalibracji;
pojazd doświadcza kontaktu z podłożem;
brak obserwacji danego stopnia swobody.

Bias z filtra nie zastępuje kalibracji temperatury. Jest stanem dynamicznym o ograniczonym paśmie. Warto logować go osobno dla każdej osi.

Wersja kwaternionowa#

Kwaternion jednostkowy q reprezentuje orientację. Prędkość kątową zapisuje się jako czysty kwaternion ωq = [0, ωx, ωy, ωz]. Dla jednej z popularnych konwencji:

q_dot = 1/2 q ⊗ ωq

Jeśli q opisuje odwrotny kierunek transformacji lub biblioteka mnoży wektory z innej strony, kolejność iloczynu zmienia się. Po kroku Eulera:

q ← normalize(q + q_dot Δt)

Prosta metoda działa przy małym Δt, lecz dla dużych przyrostów dokładniejszy jest kwaternion wykładniczy z wektora rotacji. Normalizacja ogranicza dryf normy, ale nie naprawia błędu orientacji.

Korekcję kP e dodaje się do prędkości kątowej przed integracją. Jest to wygodne obliczeniowo i zachowuje stan na rozmaitości po normalizacji.

Filtr Mahony’ego#

Nazwa „filtr Mahony’ego” jest często używana dla implementacji obserwatora z proporcjonalno-całkującą korekcją błędu wektorowego. Istotne elementy to:

propagacja kwaternionu z żyroskopu;
rekonstrukcja przewidywanego kierunku grawitacji i pola;
iloczyn wektorowy jako błąd orientacji;
człon proporcjonalny korygujący rate;
człon całkujący estymujący bias.

Wzmocnienia nie są bezpośrednio „częstotliwością odcięcia” prostego filtra jednej osi, choć mają analogiczny wpływ. Dynamika zależy od geometrii, wag wektorów i normalizacji.

Zaletą jest mała złożoność oraz jawna interpretacja błędu. Wadą — brak pełnej probabilistycznej kowariancji i konieczność ręcznej logiki wiarygodności. Dobrze zaimplementowany obserwator może być bardzo skuteczny dla attitude, zwłaszcza gdy pozycja jest estymowana osobno.

Filtr Madgwicka#

Madgwick zaproponował korekcję orientacji przez gradient funkcji celu opisującej różnicę między zmierzonym i przewidywanym kierunkiem grawitacji oraz pola magnetycznego. Krok gradientowy jest łączony z pochodną kwaternionu z żyroskopu.

Parametr β reguluje szybkość korekcji. Duże β szybciej usuwa błąd początkowy, ale silniej przenosi przyspieszenie i zakłócenia magnetyczne do orientacji. Małe β polega bardziej na żyroskopie i wolniej usuwa dryf.

Raport Madgwicka zawiera wersję IMU bez magnetometru oraz MARG z magnetometrem. Kod historyczny jest szeroko kopiowany, lecz warianty bibliotek różnią się jednostkami, domyślnym ODR i konwencją kwaternionu. Należy porównać implementację z równaniami oraz testami, nie tylko nazwą funkcji.

Filtr Madgwicka i obserwator Mahony’ego nie są zamiennymi zestawami magicznych stałych. Mają inną konstrukcję korekcji i parametry trzeba dobrać dla konkretnego szumu, ruchu i częstotliwości.

Akcelerometr jako obserwacja#

Przed użyciem wektor przechodzi:

korekcję biasu, skali i osi;
transformację sensor→body;
test clippingu oraz aktualności;
test normy i wariancji;
normalizację.

Norma bliska 1 g nie dowodzi, że wektor wskazuje pion. Przyspieszenie poziome może obracać wektor bez dużej zmiany modułu. Pomocne są dodatkowo informacje o throttle, GNSS velocity, modelu dynamiki i czasie trwania zaburzenia.

W zawisie akcelerometr jest zwykle dobrym odniesieniem roll/pitch po odfiltrowaniu drgań. W agresywnym locie wagę trzeba ograniczać. Po zakończeniu manewru filtr powinien płynnie odzyskać odniesienie, bez skoku.

Normalizacja wektora usuwa informację o module, dlatego testy należy wykonać wcześniej. Nie wolno normalizować wektora bliskiego zeru.

Magnetometr jako obserwacja#

Magnetometr wymaga korekcji hard/soft iron, alignmentu i aktualnego modelu pola. Można użyć pełnego wektora WMM albo wyznaczyć poziome odniesienie. Pełny model wykorzystuje inklinację, ale jest wrażliwy na lokalne zakłócenia.

Kontrola obejmuje normę, kąt do przewidywania, saturację, zależność od prądu i zmianę w czasie. Gdy pomiar jest odrzucony, yaw propaguje się z żyroskopu i zaczyna dryfować. Filtr powinien raportować utratę obserwowalności, nie udawać niezmienionej jakości.

Przy odzyskaniu kompasu korekcję yaw należy wprowadzać kontrolowanym pasmem. Natychmiastowe ustawienie może obrócić ramkę lokalną i wygenerować skok regulatora pozycji.

Dobór wzmocnień#

Proces zaczyna się od zidentyfikowania szumu i biasu żyroskopu oraz typowych przyspieszeń. Następnie:

wyłącz człon całkujący;
ustaw małe wzmocnienie korekcji;
sprawdź odpowiedź na znany przechył i powrót;
zwiększaj do uzyskania akceptowalnego czasu zbieżności bez nadmiernego przenoszenia drgań;
dodaj wolny człon biasu z ograniczeniem;
osobno dobierz wagę magnetometru;
sprawdź manewry i zakłócenia, nie tylko stół.

Odpowiedź początkowa i praca ustalona mogą wymagać różnych wzmocnień. Po uruchomieniu można użyć szybszej zbieżności, a po ustaleniu zmniejszyć korekcję. Przełączenie powinno być płynne.

Wzmocnienia zależą od jednostek. Jeżeli żyroskop przekazano w °/s do równań oczekujących rad/s, parametr pozornie wymaga 57,3-krotnej korekty, a integracja jest błędna.

Czas próbkowania i opóźnienie#

Filtr orientacji wymaga timestampu pomiaru. Żyroskop i akcelerometr z jednego IMU mogą mieć wspólny ODR lub różne wewnętrzne ścieżki. Magnetometr zwykle ma mniejszą częstotliwość i większe opóźnienie.

Wektor magnetyczny należy porównać z orientacją z chwili jego pomiaru. Przy szybkim yaw opóźnienie wygląda jak stały błąd kierunku. Prosty filtr bez historii może przyjąć małe opóźnienie, ale musi je zmierzyć i uwzględnić w budżecie.

Przerwa w danych żyroskopu nie może zostać ukryta przez duży Δt. Integracja jednego rate przez 100 ms po utracie próbek jest spekulacją. Sterownik powinien oznaczyć lukę, a filtr przejść w stan degraded lub ograniczyć krok.

Normalizacja i warunki brzegowe#

Kwaternion normalizuje się okresowo lub po każdym kroku:

q ← q / ||q||

Przed dzieleniem sprawdza się skończoność i minimalną normę. NaN w jednej próbce nie może zatruć stanu na resztę lotu. Wektor obserwacji także wymaga minimalnej normy.

Kwaterniony q i -q oznaczają tę samą orientację. Dla ciągłości logu można zmienić znak, gdy iloczyn skalarny z poprzednią próbką jest ujemny. Nie należy jednak traktować zmiany wszystkich znaków jako skoku fizycznego.

Kąty z atan2 i asin wymagają clampu argumentu wynikającego z błędów numerycznych. Macierz obrotu powinna mieć wyznacznik +1 i zachowywać normy.

Inicjalizacja orientacji#

W stanie nieruchomym akcelerometr wyznacza roll/pitch. Magnetometr po kompensacji przechylenia wyznacza yaw. Jeżeli kompas jest niewiarygodny, yaw może zacząć od arbitralnego zera i zostać później związany z GNSS lub wizją.

Inicjalizację należy odróżnić od zbieżności filtra. Start od kwaternionu jednostkowego przy pojeździe przechylonym powoduje duży błąd, z którym lokalny obserwator może zbiegać długo. Bezpośrednia rekonstrukcja z dwóch niekolinearnych wektorów daje lepszy stan początkowy.

Trzeba wykryć przypadek, gdy pojazd się porusza. Inicjalizacja grawitacji podczas przyspieszenia zapisze fałszywy przechył. W takim stanie lepiej czekać albo zwiększyć niepewność.

Odrzucanie zaburzeń#

Wagi mogą być funkcją jakości:

w_acc = f(| ||a|| - g |, variance, clipping, motion)
w_mag = f(| ||B|| - B_model |, angle_residual, current, saturation)

Funkcja powinna być ciągła lub mieć histerezę. Ostre włączanie obserwacji generuje skoki pochodnej. W stanie całkowitego odrzucenia bias nie powinien dalej całkować błędu.

Odporność sprawdza się fault injection: stałe przyspieszenie, impuls, zamrożona próbka, pole od magnesu, opóźnienie, zły timestamp, utrata pakietu i saturacja. Test „obracam płytkę w dłoni” nie pokrywa tych przypadków.

Implementacja krok po kroku#

Poniższy pseudokod pokazuje strukturę, nie gotową bibliotekę:

bool attitude_update(State *s, const ImuSample *imu, float dt) {
    if (!valid_dt(dt) || !finite3(imu->gyro_rad_s)) return false;

    Vec3 omega = imu->gyro_rad_s - s->gyro_bias;
    Vec3 error = {0};
    float weight_sum = 0.0f;

    if (accel_is_valid(imu)) {
        Vec3 a = normalize(imu->accel_m_s2);
        Vec3 g_pred_body = rotate_world_to_body(s->q, WORLD_DOWN);
        float w = accel_weight(imu);
        error += w * cross(a, -g_pred_body); /* znak zależy od konwencji */
        weight_sum += w;
    }

    if (mag_is_valid(s->mag)) {
        Vec3 m = normalize(s->mag.field_body);
        Vec3 m_pred = rotate_world_to_body(s->q, s->field_world);
        float w = mag_weight(s->mag);
        error += w * cross(m, m_pred);
        weight_sum += w;
    }

    if (weight_sum > 0.0f) {
        error /= weight_sum;
        s->gyro_bias = clamp3(s->gyro_bias - s->ki * error * dt,
                              s->bias_limit);
        omega += s->kp * error;
    }

    s->q = integrate_quaternion(s->q, omega, dt);
    return normalize_checked(&s->q);
}

Komentarz o znaku jest celowy: dopóki projekt nie definiuje q, WORLD_DOWN i kierunku rotate, nie istnieje uniwersalnie poprawna linia. Testy osi są częścią implementacji.

Testy#

Testy jednostkowe powinny obejmować:

brak ruchu i brak biasu;
znany stały obrót wokół każdej osi;
składanie dwóch obrotów w różnej kolejności;
q i -q jako tę samą orientację;
odzyskiwanie małego błędu roll/pitch;
brak korekcji yaw bez magnetometru;
estymację znanego biasu;
odrzucenie akcelerometru przy 2 g;
odrzucenie pola o złej normie;
lukę timestampu i NaN;
niezmienność normy kwaternionu.

Testy symulacyjne generują prawdziwą trajektorię, idealne sensory, potem kontrolowanie dodają bias, szum, opóźnienie i zakłócenia. Błąd orientacji należy liczyć jako kąt względnego kwaternionu, nie różnicę trzech kątów Eulera.

Replay prawdziwego logu pozwala porównywać wersje deterministycznie. Każda zmiana filtra powinna dać zestaw metryk, nie tylko ładniejszy wykres.

Diagnostyka w locie#

Loguj co najmniej:

kwaternion i normę;
surowy oraz skorygowany rate;
estymowany bias;
wektory zmierzone i przewidywane;
błędy krzyżowe oraz wagi;
powody odrzucenia obserwacji;
Δt, luki i liczbę resetów;
kąt do niezależnego odniesienia.

Oscylujący błąd roll/pitch może oznaczać zbyt duże wzmocnienie lub opóźnienie akcelerometru. Powolny dryf oznacza małą korekcję, zły bias albo brak obserwowalności. Skok yaw zależny od throttle wskazuje magnetometr, nie parametr filtra.

Filtr nie powinien maskować awarii. Status jakości ma być publikowany do reszty systemu. Regulator może ograniczyć tryb lotu, gdy yaw nie jest obserwowany.

Kiedy użyć EKF#

Filtr komplementarny jest dobry, gdy potrzebna jest przede wszystkim orientacja, model ma być mały i przejrzysty, a MCU ma ograniczone zasoby. Sprawdza się również jako niezależny estimator zapasowy i narzędzie uruchomieniowe.

EKF jest uzasadniony, gdy wspólnie estymuje się orientację, prędkość, pozycję, biasy, wiatr, teren i opóźnione sensory oraz potrzebna jest jawna kowariancja. Nie oznacza automatycznie lepszej jakości: zły model Q/R, ramki albo timestamp daje złożony, lecz błędny wynik.

W praktycznych autopilotach oba podejścia mogą współistnieć. PX4 EKF2 pracuje na opóźnionym horyzoncie fuzji, a następnie używa komplementarnej propagacji wyjścia do czasu bieżącego.

Powiązane tematy#

Przypisy#

R. Mahony, T. Hamel, J.-M. Pflimlin, Nonlinear Complementary Filters on the Special Orthogonal Group, IEEE Transactions on Automatic Control 53(5), 2008.
S. O. H. Madgwick, An efficient orientation filter for inertial and inertial/magnetic sensor arrays — wyprowadzenie i parametry filtra gradientowego.
x-io Technologies, Open source IMU and AHRS algorithms — kod referencyjny i materiały źródłowe.
PX4, Using PX4’s Navigation Filter EKF2 — przykład komplementarnej propagacji z opóźnionego horyzontu fuzji.

Utworzono: 15 sierpnia 2026. Ostatnia aktualizacja: 15 sierpnia 2026. Źródła zweryfikowano: 15 sierpnia 2026.

Źródła z centralnego rejestru

Mahony, Hamel, Pflimlin: Nonlinear Complementary Filters on the Special Orthogonal Group [publikacja naukowa]
Sebastian Madgwick: An efficient orientation filter for inertial and inertial/magnetic sensor arrays [raport badawczy]
x-io Technologies: Open source IMU and AHRS algorithms [kod i dokumentacja projektu]
Sumit Sharma, „Drone Development from Concept to Flight” [książka]