Moduł 09 · estymacja

Obserwatory stanu

Luenberger, Kalman, EKF/UKF, separation principle, observer + controller. Estymacja zmiennych nie-mierzonych.

1. Po co obserwator — gdy nie wiemy wszystkiego

Analogia. Wyobraź sobie kierowcę auta z zasłoniętym prędkościomierzem — widzi tylko, że jedzie. Z odczytów spidometra rowerowego (impulsy z koła co 50 cm) próbuje wnioskować o prędkości chwilowej. Pomiary są dyskretne i hałaśliwe, ale ma dobry model fizyczny: ile koło się obróci, taką drogę przejedzie. Jego mózg łączy pomiar z modelem żeby estymować prędkość. Dokładnie to robi obserwator stanu.

Typowy plant dynamiczny:

x˙=Ax+Bu,y=Cx.\dot x = A x + B u, \qquad y = C x.

Stan xRnx \in \mathbb{R}^n — wszystkie zmienne opisujące układ. Wyjście yRmy \in \mathbb{R}^m — to, co mierzymy. Zwykle m<nm < n, 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:

  1. Model dynamiczny: skoro znamy A,BA, B, możemy całkować x^˙=Ax^+Bu\dot{\hat x} = A\hat x + Bu i to też daje nam ewolucję stanu.
  2. Pomiar: dostajemy y=Cxy = Cx w każdej chwili — możemy je porównać z y^=Cx^\hat y = C\hat x i korygować estymatę jeśli są rozbieżne.

Klasyczna struktura:

x^˙=Ax^+Busymulacja modelu+L(yCx^)korekta z pomiaru\dot{\hat x} = \underbrace{A\hat x + Bu}_{\text{symulacja modelu}} + \underbrace{L(y - C\hat x)}_{\text{korekta z pomiaru}}

Pierwszy człon — predykcja z modelu. Drugi — aktualizacja z pomiaru, ważona przez wzmocnienie LL. Każdy obserwator (Luenberger, Kalman, EKF, SMO) ma tę strukturę; różnią się tylko sposobem doboru LL.

2. Obserwowalność — kiedy w ogóle da się estymować

Analogia. Wyobraź sobie pokój z lampką i wentylatorem. Mierzysz tylko prąd całkowity. Lampka pobiera 100 W, wentylator 30 W. Jeśli prąd = 130 W — może to być oboje włączone, albo lampka + lekko działający wentylator z 30 W poboru — nie wiesz. Pomiar prądu nie jest wystarczający do rozróżnienia stanów. Plant jest nieobserwowalny z tym pomiarem.

Obserwowalność = formalny warunek, że znajomość y(t)y(t) przez skończony czas wystarcza do wyznaczenia x(t)x(t). Kalman (1960) podał algebraiczny test.

Kryterium Kalmana

Układ x˙=Ax, y=Cx\dot x = Ax,\ y = Cx jest obserwowalny wtedy i tylko wtedy gdy macierz obserwowalności:

O=[CCACA2CAn1]\mathcal{O} = \begin{bmatrix} C \\ CA \\ CA^2 \\ \vdots \\ CA^{n-1} \end{bmatrix}

ma rząd nn (pełny rząd kolumnowy). Czyli kolumny O\mathcal{O} rozpinają całą przestrzeń Rn\mathbb{R}^n.

Intuicja

CC mówi: co bezpośrednio mierzymy. CACA mówi: co możemy zobaczyć przez pierwszą pochodną y˙\dot y. CA2CA^2 — 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.

Przykład: MSD z modułu 1

A=(01k/mc/m)A = \begin{pmatrix} 0 & 1 \\ -k/m & -c/m \end{pmatrix}, C=(10)C = \begin{pmatrix} 1 & 0 \end{pmatrix} (pomiar pozycji). O=(CCA)=(1001)\mathcal{O} = \begin{pmatrix} C \\ CA \end{pmatrix} = \begin{pmatrix} 1 & 0 \\ 0 & 1 \end{pmatrix} — pełny rząd. Plant jest obserwowalny: można odzyskać prędkość z samego pomiaru pozycji.

Detectability — słabszy warunek

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).

3. Obserwator Luenbergera — klasyk z 1964

Analogia. Wyobraź sobie dwóch biegaczy: jeden (plant) biegnie po prawdziwej trasie, drugi (observer) ma mapę i biegnie wzdłuż niej w wirtualnym świecie. Co kilka sekund pierwszy patrzy na siebie i podaje przez radio: „jestem przy drzewie X". Drugi sprawdza, gdzie on by miał być według mapy, i jeśli różnica jest duża — koryguje swój kurs (sam pchnie się trochę w stronę X). Po kilku takich poprawkach obaj biegacze są synchroniczni. Wzmocnienie tej korekty to L.

Luenberger (1964) podał wzór:

x^˙=Ax^+Bu+L(yCx^)\dot{\hat x} = A\hat x + Bu + L(y - C\hat x)

Wybór LL: tak, by macierz dynamiki błędu ALCA - LC 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).

Dynamika błędu estymacji

Niech x~=xx^\tilde x = x - \hat x — błąd estymacji. Wtedy:

x~˙=x˙x^˙=(Ax+Bu)(Ax^+Bu+LC(xx^))=(ALC)x~.\dot{\tilde x} = \dot x - \dot{\hat x} = (Ax + Bu) - (A\hat x + Bu + LC(x - \hat x)) = (A - LC)\,\tilde x.

Linearny układ autonomiczny. Jeśli ALCA - LC ma wszystkie wartości własne w lewej półpłaszczyźnie, x~0\tilde x \to 0 eksponencjalnie, niezależnie od warunku początkowego. Szybkość zbieżności kontrolujemy wyborem LL.

wzmocnienie obserwatora L

L₁ (gain na x̂₁)4.0
L₂ (gain na x̂₂)6.0
błąd estymacji w t=00.50
dynamika błędu A − LC
trace-6.00det18.00wartości własneλ=-3.00 ± 3.00istabilne?tak

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.

-1.5-0.7500.751.5012345x₁ (rzeczywista pozycja — mierzona)x̂₁ (estymata)t [s]x₁ (pozycja)-3-1.501.53012345x₂ (rzeczywista prędkość — NIE mierzona)x̂₂ (estymata)t [s]x₂ = ẋ₁ (prędkość, ESTYMOWANA)

Co tu się dzieje. Plant — MSD (m=1, c=2, k=4) — startuje od stanu [1,0][1, 0]. Obserwator startuje z błędem [1+0.50,0.50][1 + 0.50, 0.50] — czyli zaczyna „nieprawidłowo". Ale dostaje tylko pomiar y=x1y = x_1 i powinien sam dojść do prawidłowego x2x_2.

Sprawdź: estymata x^1\hat x_1 (czerwona, górny wykres) szybko zbiega do prawdziwej pozycji x1x_1 (niebieska). Co WAŻNIEJSZE — estymata x^2\hat x_2 (czerwona, dolny wykres) ZNAJDUJE prawdziwą prędkość x2x_2 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.

4. Filtr Kalmana — optymalny pod szumem

Analogia. Wyobraź sobie dwa źródła informacji o swojej pozycji: GPS (pomiar, dokładność ±5 m) i licznik kroków + kompas (model, kumulujący błąd ~2% drogi). W każdej chwili masz dwie estymaty pozycji — która jest prawdziwa? Najlepsza odpowiedź: ważona kombinacja, gdzie waga zależy od pewności każdej. Po dłuższym chodzeniu błąd modelu rośnie, więc waga GPS rośnie. Tuż po GPS-fix, waga modelu rośnie (bo GPS zazwyczaj nie zmienia się szybciej niż chodzimy). Filtr Kalmana robi to automatycznie.

Kalman (1960) wprowadził obserwator dla układu ze znanym szumem gaussowskim:

xk+1=Axk+Buk+wk,yk=Cxk+vkx_{k+1} = A x_k + B u_k + w_k, \quad y_k = C x_k + v_k
wkN(0,Q),vkN(0,R).w_k \sim \mathcal{N}(0, Q), \quad v_k \sim \mathcal{N}(0, R).

QQ to kowariancja szumu procesowego (jak bardzo ufamy modelowi). RR to kowariancja szumu pomiaru (jak bardzo ufamy sensorom). Filtr utrzymuje nie tylko estymatę średnią x^\hat x, ale też pełną kowariancję P=E[(xx^)(xx^)T]P = E[(x - \hat x)(x - \hat x)^T]. Kowariancja mówi: „jak pewna jest moja estymata, w którym kierunku jestem najbardziej niepewny".

Dwa kroki na sample

predict:  x^Ax^+Bu,PAPAT+Q\textbf{predict:}\; \hat x \leftarrow A\hat x + Bu, \quad P \leftarrow APA^T + Q
update:  K=PCT(CPCT+R)1,x^x^+K(yCx^),P(IKC)P\textbf{update:}\; K = PC^T(CPC^T + R)^{-1}, \quad \hat x \leftarrow \hat x + K(y - C\hat x), \quad P \leftarrow (I - KC)P

KK = wzmocnienie Kalmana — jest dynamicznie wyliczane w każdej chwili z bieżącej PP. Dla układów stabilnych PP zbiega do stanu ustalonego, wtedy KK też się stabilizuje — to steady-state Kalman filter, z gainem K przedrobionym offline. Liczbowo to identyczne jak Luenberger z optymalnym L.

Optymalność

Filtr Kalmana minimalizuje błąd średniokwadratowy E[xx^2]E[\|x - \hat x\|^2] 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).

parametry filtra

Q (zaufanie do modelu)0.010
σ pomiaru (szum)0.10
P₀ (początkowa niepewność)1.0

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 RR 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.

-1.5-0.670.150.971.801.22.43.64.86x₁ (rzeczywista)y (pomiar z szumem)x̂₁ (Kalman)t [s]x₁ (pozycja)
przestrzeń stanu z elipsami kowariancji 2σ
x₁x₂
trajektoria rzeczywista, estymata Kalmana, elipsy kowariancji w wybranych chwilach — kurczą się w czasie jak filtr „uczy się" pomiarów.

Filtr Kalmana = obserwator z formalną gwarancją optymalności pod szumem gaussowskim. Utrzymuje nie tylko estymatę średnią x^\hat x, ale też kowariancję PP — miarę pewności estymaty. Elipsy w przestrzeni stanu to poziomice rozkładu gaussa N(x^,P)N(\hat x, P).

x^Ax^+Bu,    PAPAT+Qpredict,K=PCT(CPCT+R)1,    x^x^+K(yCx^),    P(IKC)Pupdate\underbrace{\hat x \leftarrow A\hat x + Bu, \;\; P \leftarrow APA^T + Q}_{\text{predict}}, \qquad \underbrace{K = PC^T(CPC^T + R)^{-1}, \;\; \hat x \leftarrow \hat x + K(y - C\hat x), \;\; P \leftarrow (I - KC)P}_{\text{update}}

Wzmocnienie Kalmana KK jest optymalnym kompromisem między zaufaniem do modelu (Q) a pomiarów (R). Jeśli pomiary są bardzo dokładne (R małe), KK rośnie i estymata szybko trafia w pomiary. Jeśli pomiary szumowe (R duże), KK 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.

wzorzec liczbowy

Skalarny Kalman — dwie iteracje krok po kroku

scenariusz
Model: xk+1=axk+wkx_{k+1} = a\,x_k + w_k, yk=cxk+vky_k = c\,x_k + v_k z a=1a = 1, c=1c = 1, Q=0.1Q = 0{.}1 (proces), R=1R = 1 (pomiar). Inicjalizacja: x^0=0\hat x_0 = 0, P0=1P_0 = 1. Pomiary: y1=1.2y_1 = 1{.}2, y2=0.9y_2 = 0{.}9.

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ę).

k = 1 · predict
x^10=ax^0=10=0\hat x_{1|0} = a \hat x_0 = 1 \cdot 0 = 0
P10=a2P0+Q=1+0.1=1.1P_{1|0} = a^2 P_0 + Q = 1 + 0{.}1 = 1{.}1
k = 1 · update (y₁ = 1.2)
ν1=y1cx^10=1.2\nu_1 = y_1 - c \hat x_{1|0} = 1{.}2
S1=c2P10+R=2.1S_1 = c^2 P_{1|0} + R = 2{.}1
K1=P10c/S1=0.5238K_1 = P_{1|0} c / S_1 = 0{.}5238
x^11=0+0.52381.2=0.6286\hat x_{1|1} = 0 + 0{.}5238 \cdot 1{.}2 = 0{.}6286
P11=(1K1c)P10=0.5238P_{1|1} = (1 - K_1 c) P_{1|0} = 0{.}5238
k = 2 · predict
x^21=10.6286=0.6286\hat x_{2|1} = 1 \cdot 0{.}6286 = 0{.}6286
P21=0.5238+0.1=0.6238P_{2|1} = 0{.}5238 + 0{.}1 = 0{.}6238
k = 2 · update (y₂ = 0.9)
ν2=0.90.6286=0.2714\nu_2 = 0{.}9 - 0{.}6286 = 0{.}2714
S2=0.6238+1=1.6238S_2 = 0{.}6238 + 1 = 1{.}6238
K2=0.6238/1.6238=0.3841K_2 = 0{.}6238 / 1{.}6238 = 0{.}3841
x^22=0.6286+0.38410.2714=0.7329\hat x_{2|2} = 0{.}6286 + 0{.}3841 \cdot 0{.}2714 = 0{.}7329
P22=(10.3841)0.6238=0.3841P_{2|2} = (1 - 0{.}3841) \cdot 0{.}6238 = 0{.}3841

Obserwacja: KK i PP maleją — filtr coraz bardziej ufa swojej estymacie i mniej kolejnym pomiarom. Asymptotycznie zbiegają do steady-state (rozwiązanie DARE), na którym KK_\infty jest stałe. Dla naszych parametrów steady-state P0.3702P_\infty \approx 0{.}3702, K0.2702K_\infty \approx 0{.}2702 (z równania DARE P2=0.1P+0.1P^2 = 0{.}1\,P + 0{.}1).

Wszystkie liczby wyliczone ręcznie i zaokrąglone do 4 miejsc. Wpisz te wartości startowe do swojego kodu — jeśli kolejne kroki masz inne, masz błąd w implementacji.

5. EKF, UKF, particle filter — dla nieliniowych

Analogia. Standardowy Kalman zakłada, że wszystko jest liniowe i rozkłady są gaussowskie. W praktyce prawie nic nie jest liniowe — manipulator z trygonometrią, wahadło z sin/cos. Trzy strategie podejścia: linearyzuj wokół estymaty (EKF — Extended Kalman Filter), próbkuj wokół estymaty (UKF — Unscented), reprezentuj jako chmurę cząstek (Particle Filter). Każde z innym kompromisem.

EKF — najprostsze podejście

Plant: x˙=f(x,u), y=h(x)\dot x = f(x, u), \ y = h(x) z gaussowskim szumem. Idea EKF: w każdym kroku linearyzuj f,hf, h wokół bieżącej estymaty, potem stosuj klasyczne równania Kalmana:

Fk=fxx^k,Hk=hxx^kF_k = \frac{\partial f}{\partial x}\bigg|_{\hat x_k}, \quad H_k = \frac{\partial h}{\partial x}\bigg|_{\hat x_k}
x^k+1k=f(x^k,uk),Pk+1k=FkPkFkT+Q\hat x_{k+1|k} = f(\hat x_k, u_k), \quad P_{k+1|k} = F_k P_k F_k^T + Q

+ update krok jak w Kalmanie, z HkH_k zamiast CC.

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 x^\hat x daleko od prawdy, FF jest wyliczony „w złym miejscu" — korekta idzie w złą stronę.

UKF — sigma points

Unscented Kalman Filter (Julier & Uhlmann 1997): zamiast linearyzować ff, propaguje specjalnie dobrany zestaw punktów (sigma points) przez nieliniową funkcję. Każdy punkt to przykład z rozkładu N(x^,P)N(\hat x, P), po przejściu przez ff 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 ff, koszt podobny (3 sigma + 1 = 4 punkty dla 2D, ogólnie 2n+1).

Particle filter — dowolne rozkłady

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 ff, 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ę).

Kiedy co

filtrzłożonośćkiedy stosować
Linear KalmanO(n³) per stepplant liniowy, szum gaussowski
EKFO(n³)łagodnie nieliniowy plant, szum gauss
UKFO(n³)silnie nieliniowy plant, szum gauss
Particle FilterO(N·n) per step, N>>ndowolnie nieliniowy plant, dowolny szum

6. Separation principle — observer + controller razem

Analogia. Wyobraź sobie pilota lecącego we mgle. Wpierw używa instrumentów żeby zorientować się gdzie jest (observer). Potem decyduje, jak skorygować kurs (controller). Te dwie czynności są logicznie niezależne — pilot najpierw ufa instrumentom, potem ufa swojej decyzji. Separation principle mówi, że taki podział jest matematycznie poprawny dla układów liniowych: można zaprojektować estymator i regulator osobno, a połączony układ będzie stabilny.

Twierdzenie o separacji (LQG)

Dla układu liniowego z gaussowskim szumem:

  1. Zaprojektuj regulator u=Kx^u = -K\hat x tak, jakbyś znał stan idealnie (LQR z DARE);
  2. Zaprojektuj obserwator Kalmana niezależnie;
  3. Złóż: u=Kx^u = -K\hat x, gdzie x^\hat x pochodzi z observera.

Dynamika układu zamkniętego ma wartości własne będące unią: wartości własne regulatora ABKA - BK i obserwatora ALCA - LC. 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.

Granice twierdzenia

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.

Lektury kanoniczne

  • Anderson & Moore, „Optimal Filtering" (1979) — kanon Kalmana i jego rozszerzeń.
  • Simon, „Optimal State Estimation" (2006) — najlepszy dydaktyczny przegląd: Kalman, EKF, UKF, PF, H∞ filtering, śmietanka.
  • Thrun, Burgard, Fox „Probabilistic Robotics" (2005) — probabilistyczne podejście, szczególnie dla SLAM i lokalizacji.
  • Edwards & Spurgeon, „Sliding Mode Control" (1998), rozdz. 6 — sliding mode observers.
  • Kwakernaak & Sivan, „Linear Optimal Control Systems" (1972) — LQG i separation principle.