Matematyka i fizyka od podstaw
PLEN

Automatyka › IV. Przestrzeń stanów, estymacja i sterowanie optymalne › 4.5 w planach · PDF do druku

Zanim zaczniesz Matematyka 4.6 Układy nieliniowe, płaszczyzna fazowa i stabilność4.4 Filtr Kalmana

Nowe pojęcia w tej lekcji (3)

filtr komplementarnyfuzja danych z czujnikówrozszerzony filtr Kalmana (EKF)

Automatyka 4.5

Rozszerzony filtr Kalmana i fuzja czujników w jednej osi

Skąd dron wie, jak bardzo jest przechylony? Żaden czujnik nie mierzy kąta wprost. Żyroskop mierzy prędkość obrotu, ale jego całka dryfuje. Akcelerometr widzi kierunek grawitacji, ale ten pomiar jest zaszumiony i nieliniowy. Ta lekcja łączy oba czujniki na dwa sposoby: prostym filtrem komplementarnym, który wystarcza wielu dronom, i rozszerzonym filtrem Kalmana, który radzi sobie z nieliniowością i sam szacuje błąd żyroskopu. Zostajemy przy jednej osi. Pełne 3D na kwaternionach jest w kursie dynamiki lotu.

Czas
ok. 2,5 h teorii + ok. 2,5 h zadań
Zadania
11 zadań w treści + 5 do przemyślenia, z odpowiedziami
Źródła
G. Welch, G. Bishop, An Introduction to the Kalman Filter, rozdz. 2; R. Mahony i in., „Nonlinear complementary filters on the special orthogonal group” (2008)

0 Prolog: jak telefon wie, gdzie jest góra

Obróć telefon, a ekran obróci się razem z tobą. Potrząśnij nim, a obraz ani drgnie. Za tym stoją dwa mikroskopijne czujniki MEMS, które kosztują razem kilka złotych: akcelerometr i żyroskop. Każdy z osobna daje kiepską informację o orientacji. Razem, połączone kilkunastoma linijkami kodu, wystarczają do stabilizacji drona, nawigacji w grach i kierowania kamerą.

Pierwsze amatorskie kontrolery lotu używały prostych filtrów komplementarnych: MultiWii w wersji liniowej, wczesne ArduPilot w postaci filtru DCM (Premerlani i Bizard), nieliniowej odmiany z korekcją dryfu typu PI. Profesjonalne systemy, od samolotów po statek Apollo, używają rozszerzonego filtru Kalmana. W tej lekcji zobaczymy oba i sprawdzimy, ile daje ta różnica.

Pytanie na start. Żyroskop drona ma błąd stały 3°/s. Akcelerometr ma szum, który daje błąd kąta ok. 4°. Jakiego błędu kąta można się spodziewać po połączeniu: 4°, 1° czy 0,2°? Odpowiedź w części 4.

1 Dwa czujniki, dwie wady

Jedna oś: przechylenie φ\varphi drona.

  • Żyroskop mierzy prędkość kątową: ωm=φ˙+b+szum\omega_m = \dot\varphi + b + \text{szum}, gdzie bb to bias (błąd stały, typowo ułamki do kilku stopni na sekundę, zmienny z temperaturą). Całkowanie daje kąt, ale bias całkuje się w dryf: 3°/s to po minucie 180°.
  • Akcelerometr w zawisie mierzy składową grawitacji: ay=gsin⁡φ+szum+przyspieszenia ruchua_y = g\sin\varphi + \text{szum} + \text{przyspieszenia ruchu}. Daje kąt bez dryfu: φ≈arcsin⁡(ay/g)\varphi \approx \arcsin(a_y/g), ale z dużym szumem (drgania śmigieł) i z błędem, gdy dron przyspiesza w bok.

Wady są komplementarne: żyroskop jest dobry dla szybkich zmian i zły dla wolnych, akcelerometr odwrotnie. To ta sama sytuacja co barometr i akcelerometr w lekcji 4.4 Filtr Kalmana, tyle że pomiar jest nieliniowy (sin⁡φ\sin\varphi).

Zadania · część 1

3 zadania · ok. 15 min

  1. Bias żyroskopu 0,05 rad/s. O ile stopni dryfuje scałkowany kąt po 10 s i po minucie?

    Odpowiedź

    0,5 rad = 28,6° po 10 s; 3 rad = 172° po minucie.

  2. Akcelerometr ma szum 0,5 m/s². Jaki błąd kąta daje przy φ≈0\varphi \approx 0? A przy φ=60∘\varphi = 60^\circ?

    Odpowiedź

    δφ≈δa/(gcos⁡φ)\delta\varphi \approx \delta a/(g\cos\varphi): przy 0° ok. 0,0510{,}051 rad = 2,9°; przy 60° dwa razy więcej, 5,8°. Im większy kąt, tym mniej czuły pomiar.

  3. Dron przyspiesza w bok o 2 m/s² bez przechylania (np. po uderzeniu wiatru w bok). Jaki fałszywy kąt pokaże akcelerometr?

    Odpowiedź

    arcsin⁡(2/9,81)≈11,8∘\arcsin(2/9{,}81) \approx 11{,}8^\circ. Akcelerometr nie odróżnia grawitacji od przyspieszenia (zasada równoważności). Dlatego przy manewrach trzeba mu mniej ufać.

2 Filtr komplementarny

Najprostsza fuzja: co krok scałkuj żyroskop i lekko pociągnij wynik w stronę kąta z akcelerometru:

Filtr komplementarny

φ^k=α(φ^k−1+ωmT)+(1−α) φacc,k,α=ττ+T.\hat\varphi_k = \alpha\left(\hat\varphi_{k-1} + \omega_mT\right) + (1 - \alpha)\,\varphi_{\text{acc},k},\qquad \alpha = \frac{\tau}{\tau + T} .

W dziedzinie ss: φ^=τsτs+1⋅ωms+1τs+1 φacc\hat\varphi = \frac{\tau s}{\tau s + 1}\cdot\frac{\omega_m}{s} + \frac{1}{\tau s + 1}\,\varphi_\text{acc}. Żyroskop przez filtr górnoprzepustowy, akcelerometr przez dolnoprzepustowy; suma filtrów to 1.

Nazwa pochodzi właśnie stąd: oba filtry się uzupełniają (sumują do jedności), jak S+T=1S + T = 1 w lekcji 2.5 Zakłócenia, szum i wrażliwość: S, T i kształtowanie pętli. Prawdziwy kąt przechodzi bez zniekształceń, a każdy czujnik dostaje tylko to pasmo, w którym jest dobry. Jedno pokrętło τ\tau wyznacza częstotliwość przejścia 1/τ1/\tau: poniżej ufamy akcelerometrowi, powyżej żyroskopowi.

Słabość: bias żyroskopu nie znika, tylko zostaje „zatrzymany” na stałym błędzie bτb\tau (zadanie 2.3). Przy τ=0,5\tau = 0{,}5 s i biasie 3°/s to ok. 1,4°.

Symulacja 1 Dron przechyla się do ±65° przez minutę. Żyroskop z biasem i szumem, akcelerometr z szumem, pomiary co 0,01 s. Sam żyroskop odpływa, sam akcelerometr szumi. Zmieniaj τ\tau i patrz na błąd filtru: za krótkie przepuszcza szum, za długie zostawia błąd od biasu.

Zadania · część 2

3 zadania · ok. 25 min

  1. T=0,01T = 0{,}01 s, α=0,98\alpha = 0{,}98. Jaka jest stała czasowa τ\tau?

    Odpowiedź

    τ=αT/(1−α)=0,49\tau = \alpha T/(1 - \alpha) = 0{,}49 s.

  2. Pokaż, że transmitancje τsτs+1\frac{\tau s}{\tau s + 1} (dla ωm/s\omega_m/s) i 1τs+1\frac{1}{\tau s + 1} sumują się do 1 i co z tego wynika dla prawdziwego kąta.

    Odpowiedź

    τs+1τs+1=1\frac{\tau s + 1}{\tau s + 1} = 1. Gdy oba czujniki są idealne (ωm/s=φacc=φ\omega_m/s = \varphi_\text{acc} = \varphi), wynik to dokładnie φ\varphi bez opóźnienia i zniekształceń, przy każdej częstotliwości.

  3. Pokaż, że stały bias bb żyroskopu daje stały błąd bτb\tau.

    Odpowiedź

    Bias wchodzi jako b/sb/s (całka) przez τsτs+1\frac{\tau s}{\tau s + 1}: bττs+1⋅\frac{b\tau}{\tau s + 1}\cdot (skok). Wartość ustalona: bτb\tau. Dla 0,05 rad/s i 0,49 s: 0,0245 rad = 1,4°.

3 Rozszerzony filtr Kalmana

Filtr Kalmana z lekcji 4.4 wymaga modelu liniowego. Tu pomiar to gsin⁡φg\sin\varphi. Pomysł Schmidta: linearyzować wokół bieżącej estymaty (lekcja 4.6 Układy nieliniowe, płaszczyzna fazowa i stabilność). Predykcja idzie po prawdziwym, nieliniowym modelu, a do równań na kowariancję i wzmocnienie bierzemy jakobiany:

Rozszerzony filtr Kalmana (EKF)

Model: xk=f(xk−1,uk−1)+w\mathbf x_k = f(\mathbf x_{k-1}, \mathbf u_{k-1}) + \mathbf w,   zk=h(xk)+v\;\mathbf z_k = h(\mathbf x_k) + \mathbf v.

Predykcja: x^−=f(x^,u)\hat{\mathbf x}^- = f(\hat{\mathbf x}, \mathbf u),   P−=FPFT+Q\;P^- = FPF^T + Q,   F=∂f∂x∣x^\;F = \frac{\partial f}{\partial\mathbf x}\Big|_{\hat{\mathbf x}}.

Korekta: H=∂h∂x∣x^−H = \frac{\partial h}{\partial\mathbf x}\Big|_{\hat{\mathbf x}^-},   K=P−HT(HP−HT+R)−1\;K = P^-H^T(HP^-H^T + R)^{-1},   x^=x^−+K(z−h(x^−))\;\hat{\mathbf x} = \hat{\mathbf x}^- + K\left(\mathbf z - h(\hat{\mathbf x}^-)\right),   P=(I−KH)P−\;P = (I - KH)P^-.

Dla przechylenia ze stanem x=[φ,b]\mathbf x = [\varphi, b] (kąt i bias żyroskopu), żyroskopem jako wejściem i akcelerometrem jako pomiarem:

f=[φ+(ωm−b)Tb],F=[1−T01];h=gsin⁡φ,H=[gcos⁡φ^0].f = \begin{bmatrix}\varphi + (\omega_m - b)T\\ b\end{bmatrix},\quad F = \begin{bmatrix}1 & -T\\ 0 & 1\end{bmatrix};\qquad h = g\sin\varphi,\quad H = \begin{bmatrix}g\cos\hat\varphi & 0\end{bmatrix} .

Predykcja jest tu liniowa, nieliniowy jest tylko pomiar. Jakobian H=gcos⁡φ^H = g\cos\hat\varphi ma prostą interpretację: przy dużym kącie akcelerometr mniej mówi o kącie, więc filtr sam zmniejsza wzmocnienie. Gdybyśmy zamiast tego przyjęli model liniowy h=gφh = g\varphi, przy φ=60∘\varphi = 60^\circ akcelerometr pokazałby g⋅0,87g\cdot 0{,}87, a filtr uznałby, że kąt wynosi 0,87 rad, czyli 50°.

Symulacja 2 Ten sam lot co w symulacji 1. Filtr ze stanem [kąt, bias żyroskopu]. Porównaj EKF z linearyzacją w bieżącej estymacie i zwykły filtr Kalmana z modelem sin⁡φ≈φ\sin\varphi \approx \varphi. Dolny wykres: estymata biasu na tle prawdziwego.

Zadania · część 3

3 zadania · ok. 25 min

  1. Policz HH dla φ^=0∘\hat\varphi = 0^\circ, 30∘30^\circ, 80∘80^\circ.

    Odpowiedź

    gcos⁡φ^g\cos\hat\varphi: 9,81; 8,50; 1,70 m/s² na radian. Przy 80° pomiar jest prawie nieczuły na kąt.

  2. Dlaczego w EKF innowację liczymy jako z−h(x^−)z - h(\hat{\mathbf x}^-), a nie z−Hx^−z - H\hat{\mathbf x}^-?

    Odpowiedź

    Hx^H\hat{\mathbf x} to przybliżenie liniowe hh wokół punktu, w którym je liczono, a nie przewidywany pomiar. Prawdziwe przewidywanie to h(x^−)=gsin⁡φ^h(\hat{\mathbf x}^-) = g\sin\hat\varphi. Jakobian służy tylko do propagacji niepewności i wzmocnienia.

  3. Czy bias żyroskopu jest obserwowalny, gdy dron cały czas wisi poziomo (φ=0\varphi = 0)?

    Odpowiedź

    Tak: bias powoduje dryf przewidywanego kąta, a akcelerometr pokazuje, że kąt się nie zmienia. Para (F,H)(F, H) z H=[g 0]H = [g\ 0]: Wo=[g0g−gT]W_o = \begin{bmatrix}g & 0\\ g & -gT\end{bmatrix}, rząd 2.

4 Komplementarny czy EKF?

Odpowiedź na pytanie z prologu. Symulacja (60 s, przechylenia do ±65°, bias 2,9°/s, szum akcelerometru 0,5 m/s², błąd liczony w drugiej połowie):

metodaRMS błędunajwiększy błąd
sam żyroskopok. 130° (dryf)ok. 170°
sam akcelerometr4,2°27°
komplementarny, τ=0,49\tau = 0{,}49 s1,5°3,0°
KF z sin⁡φ≈φ\sin\varphi \approx \varphi3,8°7,4°
EKF ze stanem biasu0,21°0,6°

EKF jest siedem razy dokładniejszy od filtru komplementarnego, głównie dlatego, że szacuje bias (estymata 0,0505 rad/s przy prawdziwym 0,05). Filtr komplementarny można poprawić tym samym pomysłem: dodać człon całkujący, który powoli „uczy się” biasu. To filtr Mahony’ego (2008), w praktyce regulator PI w pętli korekty. Jest prawie tak dobry jak EKF, przy ułamku obliczeń.

Zadania · część 4

2 zadania · ok. 15 min

  1. Dlaczego zwykły KF z modelem liniowym ma w tabeli większy błąd niż filtr komplementarny, choć szacuje bias?

    Odpowiedź

    Filtr komplementarny korzysta z arcsin⁡(ay/g)\arcsin(a_y/g), czyli z poprawnej nieliniowości. KF z h=gφh = g\varphi ma systematycznie zły model pomiaru przy dużych kątach (60° widzi jako 50°), a ten błąd jeszcze psuje estymatę biasu (0,039 zamiast 0,05).

  2. Filtr komplementarny z członem całkującym: φ^←φ^+(ωm−b^)T+kp(φacc−φ^)T\hat\varphi \leftarrow \hat\varphi + (\omega_m - \hat b)T + k_p(\varphi_\text{acc} - \hat\varphi)T, b^←b^−ki(φacc−φ^)T\hat b \leftarrow \hat b - k_i(\varphi_\text{acc} - \hat\varphi)T. Dlaczego b^\hat b dąży do prawdziwego biasu?

    Odpowiedź

    W stanie ustalonym średnia różnica φacc−φ^\varphi_\text{acc} - \hat\varphi musi być zerowa (inaczej b^\hat b dalej się zmienia). Wtedy estymata kąta nie dryfuje, co jest możliwe tylko przy b^=b\hat b = b. To ten sam mechanizm co całka w PI usuwająca uchyb od stałego zakłócenia (lekcja 1.3).

5 Metoda i przykłady rozwiązane

Metoda: EKF krok po kroku

  1. Stan: wielkości szukane + błędy czujników (biasy).
  2. Model: ff (szybkie czujniki jako wejście), hh (wolne czujniki), oba nieliniowe dopuszczalne.
  3. Jakobiany FF, HH analitycznie; sprawdź numerycznie różnicami skończonymi.
  4. Szumy QQ, RR jak w lekcji 4.4; przy manewrach zwiększ RR akcelerometru.
  5. Sprawdzenie: spójność błędu z PP, obserwowalność biasów, zachowanie przy dużych kątach.

Przykład 5.1 Dobór τ filtru komplementarnego

Bias 0,05 rad/s, szum akcelerometru 0,5 m/s², T=0,01T = 0{,}01 s. Porównaj τ=0,09\tau = 0{,}09; 0,49; 2; 10 s.

  1. τ=0,09\tau = 0{,}09 s (α=0,9\alpha = 0{,}9): RMS 1,0°, ale szczyty do 5,6° (szum akcelerometru).
  2. τ=0,49\tau = 0{,}49 s (α=0,98\alpha = 0{,}98): RMS 1,5°, największy 3,0° (błąd od biasu bτ=1,4∘b\tau = 1{,}4^\circ plus szum).
  3. τ=2\tau = 2 s: RMS 5,7° (bias: 5,7°); τ=10\tau = 10 s: 28° (bias: 29°).

Wynik: optimum ok. 0,1–0,5 s; przy dużym biasie krótsze, przy dużym szumie dłuższe.

Sprawdzenie: dla długich τ\tau błąd jest prawie dokładnie bτb\tau ✓.

Przykład 5.2 Jeden krok EKF

φ^=0,5\hat\varphi = 0{,}5 rad, b^=0\hat b = 0, P=diag⁡(0,01; 10−4)P = \operatorname{diag}(0{,}01;\ 10^{-4}) (po predykcji). Pomiar ay=5,0a_y = 5{,}0 m/s², R=0,25R = 0{,}25. Policz korektę.

  1. h(φ^)=9,81sin⁡0,5=4,70h(\hat\varphi) = 9{,}81\sin 0{,}5 = 4{,}70; innowacja 5,0−4,70=0,305{,}0 - 4{,}70 = 0{,}30.
  2. H=9,81cos⁡0,5=8,61H = 9{,}81\cos 0{,}5 = 8{,}61; S=8,612⋅0,01+0,25=0,991S = 8{,}61^2\cdot 0{,}01 + 0{,}25 = 0{,}991.
  3. K=[0,01⋅8,61/0,991; 0]=[0,0869; 0]K = [0{,}01\cdot 8{,}61/0{,}991;\ 0] = [0{,}0869;\ 0] (bias nie jest jeszcze skorelowany z kątem).
  4. φ^=0,5+0,0869⋅0,30=0,526\hat\varphi = 0{,}5 + 0{,}0869\cdot 0{,}30 = 0{,}526 rad; Pφφ=(1−0,0869⋅8,61)⋅0,01=0,0025P_{\varphi\varphi} = (1 - 0{,}0869\cdot 8{,}61)\cdot 0{,}01 = 0{,}0025.

Wynik: kąt poprawiony o 1,5°, niepewność kąta spadła z 0,1 do 0,05 rad.

Sprawdzenie: arcsin⁡(5,0/9,81)=0,535\arcsin(5{,}0/9{,}81) = 0{,}535 rad: filtr przesunął się w stronę pomiaru, ale nie całkiem, bo pomiar ma swoją niepewność ✓.

6 Błędne intuicje, których trzeba się oduczyć

„Akcelerometr mierzy kąt.”
Mierzy siłę właściwą: przyspieszenie minus przyspieszenie grawitacyjne (w spoczynku pokazuje 1 g w górę). Kąt wynika z niego tylko wtedy, gdy obiekt nie przyspiesza.
„EKF jest optymalny.”
Jest przybliżeniem: linearyzacja w estymacie bywa zła przy dużych niepewnościach. Przy silnej nieliniowości lepsze bywają filtry UKF albo cząsteczkowe.
„Filtr komplementarny to tylko filtr dolnoprzepustowy na akcelerometrze.”
To para filtrów sumujących się do jedności: górnoprzepustowy na żyroskopie jest równie ważny.
„Lepszy żyroskop rozwiąże problem dryfu.”
Zmniejszy go, ale każdy żyroskop ma bias zmienny z temperaturą. Bez czujnika absolutnego (akcelerometr, magnetometr, GPS) całka zawsze ucieknie.
„Jakobian liczy się raz.”
W EKF liczy się go w każdym kroku, w bieżącej estymacie. To właśnie odróżnia EKF od filtru Kalmana dla modelu zlinearyzowanego raz w punkcie pracy.

7 Zadania do przemyślenia

Te zadania wymagają więcej myślenia niż liczenia.

Zadania do przemyślenia

5 zadań · ok. 1,5 godziny

  1. Zakręt samolotu. Samolot w skoordynowanym zakręcie z przechyleniem 30°. Co pokazuje akcelerometr w osi poprzecznej? Co z tego wynika dla filtru komplementarnego?

    Rozwiązanie
    1. W zakręcie skoordynowanym wypadkowa grawitacji i siły odśrodkowej leży w płaszczyźnie symetrii samolotu: akcelerometr poprzeczny pokazuje zero, jak w locie poziomym.
    2. Filtr komplementarny powoli „prostuje” estymatę do 0°, choć samolot jest przechylony o 30°. W samolotach korekcję akcelerometrem wyłącza się albo kompensuje przyspieszenie z prędkości i prędkości kątowej (lekcja 3 Fuzja czujników w 3D: estymacja orientacji na kwaternionach).

  2. Magnetometr dla odchylenia. Akcelerometr nie mówi nic o kursie (obrót wokół pionu). Jaki czujnik pełni tę rolę i jakie ma wady?

    Rozwiązanie
    1. Magnetometr: kierunek pola magnetycznego Ziemi. Fuzja z żyroskopem osi pionowej, tym samym schematem.
    2. Wady: zakłócenia od silników i przewodów z prądem, metalowych konstrukcji, deklinacja magnetyczna. Dlatego drony często używają też kursu z GPS (z ruchu).

  3. Kiedy linearyzacja zawodzi. EKF startuje z φ^=0\hat\varphi = 0, a dron jest przechylony o 150° (prawie do góry nogami). Co się może stać?

    Rozwiązanie
    1. ay=gsin⁡150∘=gsin⁡30∘a_y = g\sin 150^\circ = g\sin 30^\circ: ten sam pomiar co przy 30°. Filtr zbiegnie do 30°, bo to rozwiązanie bliższe startowi. Pomiar jednej składowej nie rozróżnia φ\varphi i 180∘−φ180^\circ - \varphi.
    2. Potrzebny drugi pomiar (az=gcos⁡φa_z = g\cos\varphi, zmienia znak) i ostrożna inicjalizacja. Ogólnie: EKF działa lokalnie, wymaga dobrego startu.

  4. Drgania śmigieł. Silniki dają drgania 150–300 Hz o amplitudzie przyspieszenia rzędu gg. Jak to wpływa na fuzję i co z tym robić? (Por. aliasing w lekcji 3.4.)

    Rozwiązanie
    1. Drgania złożą się w pobliże zera, jeśli ich częstotliwość leży blisko wielokrotności częstotliwości próbkowania: np. przy próbkowaniu 200 Hz drgania 190–210 Hz wyglądają jak powolne zmiany kąta o częstotliwości 0–10 Hz i filtr komplementarny ich nie odfiltruje (przy 500 Hz drgania 260–300 Hz złożą się tylko do 200–240 Hz).
    2. Rozwiązania: mechaniczne tłumienie czujnika, filtr antyaliasingowy w czujniku, szybkie próbkowanie z filtrami cyfrowymi, filtry wycinające na częstotliwości obrotów silników (znanej z ESC).

  5. Dlaczego kwaterniony? W 3D orientację można opisać trzema kątami (przechylenie, pochylenie, odchylenie). Dlaczego kontrolery lotu wolą cztery liczby (kwaternion)?

    Rozwiązanie
    1. Kąty Eulera mają osobliwość (przy pochyleniu 90° przechylenie i odchylenie stają się nieodróżnialne: blokada przegubu, ang. gimbal lock), a ich równania kinematyki zawierają funkcje trygonometryczne.
    2. Kwaternion nie ma osobliwości, a kinematyka jest liniowa w kwaternionie. Cena: więzy (norma 1) i ostrożność w EKF. Szczegóły w lekcji 3 Fuzja czujników w 3D: estymacja orientacji na kwaternionach.

8 Sprawdź się

Odpowiedz w myślach, zanim rozwiniesz odpowiedź.

1 Jakie wady mają żyroskop i akcelerometr?

Żyroskop: dryf całki od biasu. Akcelerometr: szum i fałszywy kąt przy przyspieszeniach.

2 Jak działa filtr komplementarny?

φ^=α(φ^+ωmT)+(1−α)φacc\hat\varphi = \alpha(\hat\varphi + \omega_mT) + (1 - \alpha)\varphi_\text{acc}: górnoprzepustowo żyroskop, dolnoprzepustowo akcelerometr.

3 Jaki błąd daje bias w filtrze komplementarnym?

Stały, równy bτb\tau.

4 Czym EKF różni się od KF?

Modele ff, hh mogą być nieliniowe; FF, HH to jakobiany liczone w bieżącej estymacie.

5 Ile wynosi HH dla pomiaru gsin⁡φg\sin\varphi?

gcos⁡φ^g\cos\hat\varphi.

6 Jak filtr komplementarny może szacować bias?

Dodać człon całkujący w pętli korekty (filtr Mahony’ego).

9 Karta wzorów i co dalej

Filtr komplementarny

φ^k=α(φ^k−1+ωmT)+(1−α)φacc\hat\varphi_k = \alpha(\hat\varphi_{k-1} + \omega_mT) + (1 - \alpha)\varphi_\text{acc}

α=τ/(τ+T)\alpha = \tau/(\tau + T);   τsτs+1+1τs+1=1\;\frac{\tau s}{\tau s + 1} + \frac{1}{\tau s + 1} = 1

błąd od biasu: bτb\tau

φacc=arcsin⁡(ay/g)\varphi_\text{acc} = \arcsin(a_y/g)

EKF

x^−=f(x^,u)\hat{\mathbf x}^- = f(\hat{\mathbf x}, u), P−=FPFT+QP^- = FPF^T + Q

K=P−HT(HP−HT+R)−1K = P^-H^T(HP^-H^T + R)^{-1}, x^=x^−+K(z−h(x^−))\hat{\mathbf x} = \hat{\mathbf x}^- + K(z - h(\hat{\mathbf x}^-))

przechylenie: F=[1−T01]F = \begin{bmatrix}1 & -T\\ 0 & 1\end{bmatrix}, H=[gcos⁡φ^  0]H = [g\cos\hat\varphi\ \ 0]

Do przeczytania

  • Obowiązkowo: G. Welch, G. Bishop, An Introduction to the Kalman Filter, rozdz. 2 (EKF, kilka stron).
  • Dla chętnych: R. Mahony, T. Hamel, J.-M. Pflimlin, „Nonlinear complementary filters on the special orthogonal group”, IEEE Trans. Automatic Control 53 (2008); S. Madgwick, raport o filtrze orientacji (2010), oba szeroko używane w dronach.

Mamy komplet narzędzi liniowych: sprzężenie od stanu, obserwatory, LQR, filtr Kalmana i jego nieliniowe rozszerzenie. Ostatnia lekcja części IV, dla chętnych, wychodzi poza świat linearyzacji: jak udowodnić stabilność układu nieliniowego bez rozwiązywania równań (Lapunow) i jak sterować, gdy model jest nieliniowy w całym zakresie pracy.

Korzystają z tej lekcji Dynamika lotu 3 Fuzja czujników w 3D: estymacja orientacji na kwaternionach