Luenberger, Kalman, EKF/UKF, separation principle, observer + controller. Estymacja zmiennych nie-mierzonych.
Typowy plant dynamiczny:
Stan — wszystkie zmienne opisujące układ. Wyjście — to, co mierzymy. Zwykle , czyli mierzymy mniej niż potrzebujemy do sterowania.
Naiwne podejście — różniczkowanie: skoro mierzymy pozycję x₁, to prędkość x₂ = ẋ₁ można dostać przez numeryczne różniczkowanie. Pułapka: pomiar zawiera szum, a różniczka szumu jest jeszcze bardziej hałaśliwa. Pochodna nawet słabego szumu daje sygnał nieużywalny.
Mądre podejście — observer: zbudować „kopię" planta w komputerze, która biegnie równolegle z rzeczywistością. Korzystamy z dwóch źródeł informacji:
Klasyczna struktura:
Pierwszy człon — predykcja z modelu. Drugi — aktualizacja z pomiaru, ważona przez wzmocnienie . Każdy obserwator (Luenberger, Kalman, EKF, SMO) ma tę strukturę; różnią się tylko sposobem doboru .
Obserwowalność = formalny warunek, że znajomość przez skończony czas wystarcza do wyznaczenia . Kalman (1960) podał algebraiczny test.
Układ jest obserwowalny wtedy i tylko wtedy gdy macierz obserwowalności:
ma rząd (pełny rząd kolumnowy). Czyli kolumny rozpinają całą przestrzeń .
mówi: co bezpośrednio mierzymy. mówi: co możemy zobaczyć przez pierwszą pochodną . — przez drugą. Składanie pochodnych pomiaru ujawnia coraz więcej o ukrytych zmiennych. Jeśli ten ciąg „pokrywa" wszystkie n wymiarów stanu, układ jest obserwowalny.
, (pomiar pozycji). — pełny rząd. Plant jest obserwowalny: można odzyskać prędkość z samego pomiaru pozycji.
Nieobserwowalne nie zawsze znaczy „beznadziejne". Jeśli wszystkie nieobserwowalne mody są stabilne (zanikają same z siebie), plant jest detectable: te mody są nieistotne, a obserwator radzi sobie z resztą. Detectability to minimum, jakie wymagamy dla istnienia stabilnego obserwatora.
Pełny formalizm: Antsaklis & Michel „Linear Systems" rozdz. 4; Khalil „Nonlinear Systems" rozdz. 7 dla wersji nieliniowej (obserwowalność lokalna).
Luenberger (1964) podał wzór:
Wybór : tak, by macierz dynamiki błędu miała wartości własne w zadanych miejscach (lewa półpłaszczyzna zespolona). Jest to dual problem do pole placement dla regulatora — używamy tych samych metod (Ackermann, Bass-Gura).
Niech — błąd estymacji. Wtedy:
Linearny układ autonomiczny. Jeśli ma wszystkie wartości własne w lewej półpłaszczyźnie, eksponencjalnie, niezależnie od warunku początkowego. Szybkość zbieżności kontrolujemy wyborem .
Wybór L: wartości własne A − LC w lewej półpłaszczyźnie = błąd estymacji zbiega do zera. Większe |L| = szybsza zbieżność, ale większa wrażliwość na szum pomiarowy.
Co tu się dzieje. Plant — MSD (m=1, c=2, k=4) — startuje od stanu . Obserwator startuje z błędem — czyli zaczyna „nieprawidłowo". Ale dostaje tylko pomiar i powinien sam dojść do prawidłowego .
Sprawdź: estymata (czerwona, górny wykres) szybko zbiega do prawdziwej pozycji (niebieska). Co WAŻNIEJSZE — estymata (czerwona, dolny wykres) ZNAJDUJE prawdziwą prędkość mimo że prędkość NIE BYŁA MIERZONA. Tu jest cała magia obserwatora.
Spróbuj L₁ = 0, L₂ = 0: obserwator nie używa pomiaru — to po prostu „kopia" planta uruchomiona z błędnym początkiem. Estymata nie zbiega do prawdy, idą obok siebie z osobnymi dynamikami.
Spróbuj L₁ = 20: agresywne wzmocnienie, obserwator zbiega bardzo szybko. Wadą w praktyce: duże L wzmacnia szum pomiarowy proporcjonalnie. Standardowy kompromis: szybkość zbieżności vs odporność na szum.
Kalman (1960) wprowadził obserwator dla układu ze znanym szumem gaussowskim:
to kowariancja szumu procesowego (jak bardzo ufamy modelowi). to kowariancja szumu pomiaru (jak bardzo ufamy sensorom). Filtr utrzymuje nie tylko estymatę średnią , ale też pełną kowariancję . Kowariancja mówi: „jak pewna jest moja estymata, w którym kierunku jestem najbardziej niepewny".
= wzmocnienie Kalmana — jest dynamicznie wyliczane w każdej chwili z bieżącej . Dla układów stabilnych zbiega do stanu ustalonego, wtedy też się stabilizuje — to steady-state Kalman filter, z gainem K przedrobionym offline. Liczbowo to identyczne jak Luenberger z optymalnym L.
Filtr Kalmana minimalizuje błąd średniokwadratowy pod założeniem gaussowskiego szumu. W tym samym ujęciu jest BLUE — Best Linear Unbiased Estimator. Bez założenia gaussowskiego nadal jest najlepszym liniowym estymatorem (choć nie globalnie najlepszym ze wszystkich nieliniowych).
Q = wariancja szumu procesowego. Małe Q → filtr ufa modelowi, ignoruje pomiary z dużym opóźnieniem. Duże Q → filtr ufa pomiarom, szybko reaguje ale jest hałaśliwy.
σ_y = standardowe odchylenie szumu pomiaru. Wpływa na i decyduje, jak bardzo filtr „ucina" każdy pomiar.
P₀ = początkowa pewność estymacji. Duże = filtr „nie wie" gdzie jesteśmy, agresywnie korzysta z pierwszych pomiarów.
Filtr Kalmana = obserwator z formalną gwarancją optymalności pod szumem gaussowskim. Utrzymuje nie tylko estymatę średnią , ale też kowariancję — miarę pewności estymaty. Elipsy w przestrzeni stanu to poziomice rozkładu gaussa .
Wzmocnienie Kalmana jest optymalnym kompromisem między zaufaniem do modelu (Q) a pomiarów (R). Jeśli pomiary są bardzo dokładne (R małe), rośnie i estymata szybko trafia w pomiary. Jeśli pomiary szumowe (R duże), maleje i filtr „ignoruje" hałas.
Elipsy kurczą się w czasie = pewność rośnie. Po kilku pomiarach filtr „odkrył" prawdziwy stan i jego rozkład jest bardzo zlokalizowany. Dla układu stabilnego (jak nasz MSD) kowariancja P osiąga stan ustalony — to jest tzw. steady-state Kalman filter, z gainem K przedrobionym offline.
Iteracja Kalmana ma dwie fazy: predict (model dynamiki) i update (korekcja przez pomiar). Predict zwiększa wariancję (niepewność rośnie z czasem), update ją zmniejsza (pomiar przynosi informację).
Obserwacja: i maleją — filtr coraz bardziej ufa swojej estymacie i mniej kolejnym pomiarom. Asymptotycznie zbiegają do steady-state (rozwiązanie DARE), na którym jest stałe. Dla naszych parametrów steady-state , (z równania DARE ).
Plant: z gaussowskim szumem. Idea EKF: w każdym kroku linearyzuj wokół bieżącej estymaty, potem stosuj klasyczne równania Kalmana:
+ update krok jak w Kalmanie, z zamiast .
Plusy: prosty, identyczne wymagania jak Kalman + jacobiany. Standardowo używany w robotyce (SLAM, estymacja orientacji z IMU), nawigacji (GPS+IMU fusion), ekonomii.
Minusy: linearyzacja wokół estymaty wnosi błąd; jeśli plant jest mocno nieliniowy lub niepewność duża, EKF dryfuje albo dywerguje. Ekstremalny przypadek: gdy daleko od prawdy, jest wyliczony „w złym miejscu" — korekta idzie w złą stronę.
Unscented Kalman Filter (Julier & Uhlmann 1997): zamiast linearyzować , propaguje specjalnie dobrany zestaw punktów (sigma points) przez nieliniową funkcję. Każdy punkt to przykład z rozkładu , po przejściu przez reprezentuje nowy rozkład. Sigma points są dobrane analitycznie tak, że dokładnie reprezentują 2. moment (kowariancja) — bez przybliżenia.
Zaleta UKF: dokładniejszy niż EKF dla mocno nieliniowych , koszt podobny (3 sigma + 1 = 4 punkty dla 2D, ogólnie 2n+1).
Kiedy szum nie jest gaussowski (np. bias kamery, drop GPS, multi-modal probability) — sigma points nie wystarczą. Particle Filter (Sequential Monte Carlo): reprezentujemy rozkład jako chmurę cząstek (N cząstek, każda z wagą). W każdym kroku: cząstki ewoluują przez , wagi aktualizujemy zgodnie z prawdopodobieństwem pomiaru, resamplujemy.
Plusy: dowolne nieliniowości, dowolne rozkłady. Minusy: koszt obliczeniowy O(N) per sample, dobór N nietrywialny (typowo 100–10000), particle degeneracy (kilka cząstek absorbuje całą wagę).
| filtr | złożoność | kiedy stosować |
|---|---|---|
| Linear Kalman | O(n³) per step | plant liniowy, szum gaussowski |
| EKF | O(n³) | łagodnie nieliniowy plant, szum gauss |
| UKF | O(n³) | silnie nieliniowy plant, szum gauss |
| Particle Filter | O(N·n) per step, N>>n | dowolnie nieliniowy plant, dowolny szum |
Dla układu liniowego z gaussowskim szumem:
Dynamika układu zamkniętego ma wartości własne będące unią: wartości własne regulatora i obserwatora . Oba ustawione osobno = stabilne, więc kombinacja też. Ten wynik nazywa się LQG (Linear-Quadratic-Gaussian).
W praktyce: zaprojektuj LQR (jakbyś miał pełen stan), zaprojektuj filtr Kalmana (jakbyś nie miał regulatora), złóż. Cała maszyneria liniowa działa.
Separation principle nie zachodzi dla nieliniowych układów w ogólności. Można zaprojektować EKF i nieliniowy regulator osobno, ale połączony układ nie musi być stabilny — interakcja dwóch nieliniowych dynamik może destabilizować. Dla pewnych klas układów (np. z odsprzężeniem przez wyjście) istnieją wyniki częściowej separacji.
Dla SMO + SMC pokazaliśmy w module 4 §7, że separation zachodzi pod warunkami strukturalnymi (równość PB = C^T w klasycznej formie Walcotta-Żaka).
Pełna teoria LQG: Kwakernaak & Sivan „Linear Optimal Control Systems" rozdz. 6. Nieliniowy przypadek: Khalil „Nonlinear Systems" rozdz. 14.
Dwa obserwatory pominięte w głównej narracji — bo omówione gdzie indziej w platformie albo wykraczają poza zakres bazowy:
Pełna sekcja w module 4 §7 z interaktywnym demem. SMO daje:
Klasyczna konstrukcja dla nieliniowych układów w postaci kanonicznej (chain of integrators). Wzmocnienie jest „bardzo duże" — sprawia, że dynamika błędu obserwatora jest szybsza niż dynamika planta, więc dominuje i estymata szybko zbiega. Plusy: prosty do projektu, gwarancje teoretyczne. Minusy: wzmacnia szum proporcjonalnie do gain (peaking phenomenon), wrażliwy na niemodelowane dynamiki.
Dualny do MPC. Zamiast rozwiązywać rekurencyjne równania jak Kalman, MHE w każdej chwili rozwiązuje problem optymalizacji: znajdź taki ciąg i ciąg szumów , żeby najlepiej pasowały do ostatnich N pomiarów. To pozwala dodać ograniczenia (np. ) i pracować z nielinearnymi modelami w bezpiecznym setupie. Cena: koszt obliczeniowy — QP per krok, jak w MPC.
Estymator zaburzeń, nie stanu. Rozszerzony plant z dodatkową równą (zaburzenie jest „stałe" w modelu). Standardowy obserwator (Luenberger, Kalman) na rozszerzonym stanie dostarcza estymatę , którą można odjąć od sterowania — feedforward kompensujący zaburzenie.
DOB to industrial standard dla precyzyjnego pozycjonowania (CNC, dyski twarde): jakikolwiek nieznany moment obciążenia, tarcie suche, cogging moment silnika — DOB estymuje i kompensuje.