Przejdź do głównej zawartości

Moduł 3 — Sensory i estymacja stanu

Kontroler lotu nie widzi drona — wnioskuje o nim, 8000 razy na sekundę, z zaszumionych układów, z których każdy kłamie inaczej.

🟢 Fundamenty. Podstawowe czujniki: żyroskop (prędkość obrotowa, °/s — serce lotu acro), akcelerometr (siła właściwa — mówi „gdzie jest dół", gdy nie przyspieszasz), magnetometr (kompas), barometr (wysokość z ciśnienia), GNSS (pozycja, ~1–3 m). Każdy jest niedoskonały: żyroskopy dryfują, akcelerometry trzęsą się, kompasy kłamią przy przewodach z prądem, barometry czują strumień śmigieł, GPS błądzi. Estymacja = takie ich mieszanie, żeby błędy się znosiły.

🟡 Praktyk. Klasyczna mieszanka to filtr komplementarny — krótkoterminowo ufaj żyroskopowi, długoterminowo akcelerometrowi:

θ^k=α(θ^k−1+ωk Δt)+(1−α) θacc,k,α≈0,98\hat\theta_k = \alpha\left(\hat\theta_{k-1} + \omega_k\,\Delta t\right) + (1-\alpha)\,\theta_{\text{acc},k},\qquad \alpha \approx 0{,}98

Dziesięć linijek kodu, działa wszędzie — i to jest tryb angle w prostych firmware'ach. Naucz się dyscypliny próbkowania: żyroskop próbkowany 8 kHz reprezentuje tylko drgania poniżej częstotliwości Nyquista 4 kHz; wszystko szybsze aliasuje się w fałszywy wolnozmienny sygnał, za którym pętla PID zacznie gonić. Dlatego istnieją miękkie mocowania i filtrowanie (moduł 9).

🔴 Zaawansowany. Przemysłowa odpowiedź to filtr Kalmana — statystycznie optymalne połączenie modelu ruchu i pomiarów, ważonych własnymi niepewnościami:

Predykcja:x^−=Fx^,P−=FPF⊤+QWzmocnienie:K=P−H⊤(HP−H⊤+R)−1Korekcja:x^=x^−+K(z−Hx^−),P=(I−KH)P−\begin{aligned} \textbf{Predykcja:}\quad & \hat{\mathbf x}^- = \mathbf F\hat{\mathbf x},\qquad \mathbf P^- = \mathbf F\mathbf P\mathbf F^\top + \mathbf Q\\[2pt] \textbf{Wzmocnienie:}\quad & \mathbf K = \mathbf P^-\mathbf H^\top(\mathbf H\mathbf P^-\mathbf H^\top + \mathbf R)^{-1}\\[2pt] \textbf{Korekcja:}\quad & \hat{\mathbf x} = \hat{\mathbf x}^- + \mathbf K(\mathbf z - \mathbf H\hat{\mathbf x}^-),\qquad \mathbf P = (\mathbf I - \mathbf K\mathbf H)\mathbf P^- \end{aligned}

Małe R\mathbf R (zaufany czujnik) → duża korekcja; duże Q\mathbf Q (niezaufany model) → szybka adaptacja. EKF3 ArduPilota i EKF2 PX4 to rozszerzone filtry Kalmana łączące IMU + magnetometr + baro + GNSS (+ optical flow, dalmierz, wizję) w pozycję, prędkość i orientację — orientację trzymaną jako kwaternion, by uniknąć gimbal locka (moduł 12).

⚫ Mistrz. Charakteryzujesz żyroskop wariancją Allana (oddzielając szum biały od niestabilności biasu), stroisz macierze szumów EKF z logów zamiast kopiować domyślne, rozumiesz obserwowalność (czemu yaw jest nieobserwowalny bez magnetometru lub ruchu) i potrafisz samodzielnie zaimplementować działający EKF na danych z symulacji.

Lista mistrzowska

  • Zaimplementuj filtr komplementarny na dowolnym mikrokontrolerze i pokaż stabilne kąty.
  • Odczytaj log innowacji EKF i wskaż, który czujnik zawodzi.
  • Wyjaśnij aliasing narysowaną sinusoidą — i czemu wymusza filtrowanie żyroskopu.

🖼️ Pomysły na zdjęcia: makro układu IMU na Twoim FC; schematy blokowe „Kalman filter" z Wikimedia Commons (część PD).

📚 Darmowe źródła: „Kalman and Bayesian Filters in Python" R. Labbe'a (darmowa książka na licencji CC — najlepsze pojedyncze źródło); dokumentacja EKF ArduPilota; dokumentacja ECL/EKF PX4.