4.2 Obserwator stanu i obserwowalność · Matematyka i fizyka od podstaw

0 Prolog: model, który patrzy przez ramię

Nawigator żaglowca nie miał GPS. Znał prędkość statku (z logu), kurs (z kompasu) i czas, więc co godzinę nanosił na mapę przewidywaną pozycję. Metoda nazywała się zliczeniem (ang. dead reckoning). Błędy się kumulowały, więc gdy tylko pokazał się znany ląd albo udał się pomiar wysokości gwiazdy, nawigator poprawiał pozycję na mapie, ale nie o całą różnicę, tylko rozsądnie, bo pomiar też był niedokładny.

W doktoracie na Stanfordzie (1963, artykuł w 1964 r.) David Luenberger zapisał tę samą ideę dla dowolnego układu liniowego: model przewiduje, pomiar koryguje. Dziś każdy dron, samolot i samochód autonomiczny ma w środku taki „model, który patrzy przez ramię” prawdziwego obiektu.

Pytanie na start. Dron ma tylko czujnik prędkości pionowej (np. radar dopplerowski skierowany w dół). Czy da się z niego odtworzyć wysokość? Odpowiedź w części 3.

1 Obserwator: model plus korekta

Obiekt $\dot{\mathbf x} = A\mathbf x + Bu$ , pomiar $y = C\mathbf x$ . Pomysł pierwszy: puścić równolegle kopię modelu $\dot{\hat{\mathbf x}} = A\hat{\mathbf x} + Bu$ z tym samym sterowaniem. Błąd $\mathbf e = \mathbf x - \hat{\mathbf x}$ spełnia wtedy $\dot{\mathbf e} = A\mathbf e$ . Dla obiektu z całkowaniem (wysokość) zła wartość początkowa nigdy nie zniknie, a każde nieznane zakłócenie odkłada się na zawsze.

Pomysł drugi (Luenberger): dodać korektę proporcjonalną do tego, o ile przewidywany pomiar różni się od prawdziwego:

Obserwator Luenbergera

\dot{\hat{\mathbf x}} = A\hat{\mathbf x} + Bu + L\,(y - C\hat{\mathbf x}),\qquad \dot{\mathbf e} = (A - LC)\,\mathbf e .

Błąd estymacji gaśnie z biegunami $A - LC$ . Wybór $L$ to lokowanie biegunów, tylko dla macierzy $A - LC$ zamiast $A - BK$ .

Dla wysokości drona ( $C = [1\ 0]$ , mierzymy $z$ ): $A - LC = \begin{bmatrix}-l_1 & 1\\ -l_2 & -0{,}5\end{bmatrix}$ , wielomian $s^2 + (0{,}5 + l_1)s + (0{,}5l_1 + l_2)$ . Dla $\omega_0 = 6$ rad/s, $\zeta = 0{,}7$ : $l_1 = 7{,}9$ , $l_2 = 32{,}05$ . W kodzie to kilka linijek:

e = y - zh                      # innowacja: pomiar minus przewidywanie
zh += dt * (vh + l1 * e)
vh += dt * (-0.5 * vh + u + l2 * e)

Zadania · część 1

3 zadania · ok. 20 min

Wyprowadź $\dot{\mathbf e} = (A - LC)\mathbf e$ .

Odpowiedź
$\dot{\mathbf e} = A\mathbf x + Bu - A\hat{\mathbf x} - Bu - L(C\mathbf x - C\hat{\mathbf x}) = (A - LC)(\mathbf x - \hat{\mathbf x})$ . Sterowanie się skraca, więc błąd nie zależy od $u$ .
Dobierz $L$ dla obserwatora wysokości z biegunami $-4$ i $-5$ .

Odpowiedź
$s^2 + 9s + 20$ : $0{,}5 + l_1 = 9$ , $l_1 = 8{,}5$ ; $0{,}5\cdot 8{,}5 + l_2 = 20$ , $l_2 = 15{,}75$ .
Porównaj lokowanie biegunów regulatora ( $A - BK$ ) i obserwatora ( $A - LC$ ). Jakim przekształceniem jedno zamienia się w drugie?

Odpowiedź
Wartości własne $A - LC$ i $(A - LC)^T = A^T - C^TL^T$ są te same. To lokowanie biegunów dla „obiektu” $A^T$ , $B' = C^T$ ze wzmocnieniem $L^T$ . Dlatego w Pythonie L = control.place(A.T, C.T, p).T. Ta symetria nazywa się dualnością.

2 Szybki czy gładki?

Jak szybkie powinny być bieguny obserwatora? Duże $L$ to szybka korekta: estymata szybko nadąża za zakłóceniami, o których model nie wie (podmuch, paczka). Ale duże $L$ to też silna reakcja na szum pomiaru. Małe $L$ daje estymatę gładką, ale spóźnioną. To znowu kompromis z lekcji 2.5: obserwator jest filtrem, a jego pasmo trzeba dobrać do szumu i zakłóceń.

Typowa reguła: bieguny obserwatora 2–5 razy szybsze od biegunów regulatora, a potem sprawdzenie w symulacji z szumem. Optymalny wybór $L$ , gdy znamy statystykę szumu pomiaru i zakłóceń, daje filtr Kalmana (lekcja 4.4 Filtr Kalmana).

Zadania · część 2

3 zadania · ok. 25 min

Prędkość z różnicy dwóch pomiarów barometru ( $\sigma = 0{,}1$ m, co 0,02 s) ma odchylenie ok. 7 m/s. Obserwator z $\omega_0 = 6$ rad/s ma błąd prędkości ok. 0,2 m/s. Skąd taka różnica?

Odpowiedź
Różnica dwóch pomiarów korzysta tylko z dwóch próbek i dzieli szum przez małe $\Delta t$ . Obserwator uśrednia wiele pomiarów (jest filtrem dolnoprzepustowym o paśmie ok. 6 rad/s) i dodatkowo wie z modelu, jak prędkość może się zmieniać.
W symulacji 1 porównaj obserwatory 2, 6, 20 i 60 rad/s. Który ma najmniejszy błąd prędkości? Który najlepiej radzi sobie z podmuchem?

Odpowiedź
Błąd prędkości (python-control, inne ziarno szumu): ok. 0,50; 0,21; 0,62; 2,9 m/s, czyli najmniejszy przy ok. 6 rad/s. Podmuch: im szybszy obserwator, tym bliżej regulatora z prawdziwym stanem (najniższy punkt 0,48 m). Obserwator 2 rad/s jest tak spóźniony, że dron opada poniżej zera. Najlepszy kompromis leży pośrodku.
Dlaczego wolny obserwator tak źle znosi podmuch, skoro szum nie jest tu przyczyną?

Odpowiedź
Model w obserwatorze nie zna siły podmuchu, więc przewiduje, że dron leci zgodnie z poleceniem. Rozbieżność z pomiarem koryguje dopiero człon $L(y - C\hat{\mathbf x})$ , a przy małym $L$ trwa to sekundy. Regulator przez ten czas steruje według złego obrazu.

3 Obserwowalność

Kiedy obserwator się nie uda? Wtedy, gdy jakaś zmiana stanu nie zmienia pomiarów, ani teraz, ani później. Test jest dualny do sterowalności. Układ jest obserwowalny, gdy pomiar i jego kolejne pochodne ( $C\mathbf x$ , $CA\mathbf x$ , $CA^2\mathbf x$ , …) wyznaczają cały stan:Åström–Murray 8.1

W_o = \begin{bmatrix}C\\ CA\\ \vdots\\ CA^{n-1}\end{bmatrix},\qquad \text{obserwowalny} \iff \operatorname{rank} W_o = n .

Odpowiedź na pytanie z prologu. Barometr ( $C = [1\ 0]$ ): $W_o = \begin{bmatrix}1 & 0\\ 0 & 1\end{bmatrix}$ , rząd 2, wysokość i prędkość są obserwowalne. Sam czujnik prędkości ( $C = [0\ 1]$ ): $W_o = \begin{bmatrix}0 & 1\\ 0 & -0{,}5\end{bmatrix}$ , rząd 1. Przesunięcie całego toru lotu o stałą wysokość nie zmienia żadnego pomiaru prędkości, więc nie ma sposobu, by je wykryć. Dlatego dron z przepływem optycznym potrzebuje jeszcze dalmierza albo barometru.

Zadania · część 3

3 zadania · ok. 20 min

Dron w płaszczyźnie (lekcja 4.1) ma tylko żyroskop ( $y = \dot\theta$ ). Czy kąt $\theta$ jest obserwowalny? A położenie $x$ ?

Odpowiedź
$C = [0\ 0\ 0\ 1]$ , $CA = 0$ (bo $\dot\theta$ zależy tylko od $u$ ). $W_o$ ma rząd 1: ani kąt, ani położenie nie są obserwowalne. Żyroskop mierzy tylko tempo obrotu, a stały kąt jest dla niego niewidoczny. Stąd dryf całkowania żyroskopu (lekcja 4.5 Rozszerzony filtr Kalmana i fuzja czujników w jednej osi).
Ten sam dron z hipotetycznym czujnikiem poziomego przyspieszenia w układzie ziemskim, $y = \ddot x = g\theta$ (np. z kamery śledzącej drona; zwykły akcelerometr na pokładzie mierzy co innego, lekcja 4.5). Co jest obserwowalne?

Odpowiedź
$C = [0\ 0\ g\ 0]$ , $CA = [0\ 0\ 0\ g]$ , $CA^2 = 0$ . Rząd 2: obserwowalne są $\theta$ i $\dot\theta$ , nie $x$ ani $\dot x$ . Do położenia potrzeba czujnika położenia (GPS, kamera).
Czy obiekt nieobserwowalny może mieć sensowny obserwator? (Por. stabilizowalność w lekcji 4.1.)

Odpowiedź
Tak, jeśli nieobserwowalne mody są stabilne: błąd w nich zgaśnie sam, choć w tempie narzuconym przez obiekt. To wykrywalność (ang. detectability). Całkowanie (biegun w zerze) nie jest stabilne, dlatego wysokość z samej prędkości jest beznadziejna.

4 Zasada separacji

Regulator $u = -K\hat{\mathbf x}$ steruje estymatą, a nie prawdziwym stanem. Czy to nie psuje biegunów, które tak starannie ustawiliśmy? Zapiszmy cały układ w zmiennych $\mathbf x$ i $\mathbf e = \mathbf x - \hat{\mathbf x}$ :

\frac{\mathrm d}{\mathrm dt}\begin{bmatrix}\mathbf x\\ \mathbf e\end{bmatrix} = \begin{bmatrix}A - BK & BK\\ 0 & A - LC\end{bmatrix}\begin{bmatrix}\mathbf x\\ \mathbf e\end{bmatrix} .

Macierz jest blokowo trójkątna, więc jej wartości własne to wartości własne $A - BK$ i $A - LC$ : bieguny regulatora i obserwatora po prostu się sumują. Regulator i obserwator można więc projektować osobno. To zasada separacji. Uwaga: dotyczy biegunów przy dokładnym modelu. Odporność całości na błędy modelu to osobna sprawa, do której wrócimy w lekcji 5.2 LQG i odporność.

Zadania · część 4

2 zadania · ok. 15 min

Regulator wysokości $K = [4;\ 2{,}3]$ , obserwator z $\omega_0 = 6$ , $\zeta = 0{,}7$ . Gdzie są bieguny całego układu?

Odpowiedź
$-1{,}4 \pm 1{,}43j$ (regulator) i $-4{,}2 \pm 4{,}29j$ (obserwator).
Wyprowadź równanie $\dot{\mathbf x} = (A - BK)\mathbf x + BK\mathbf e$ .

Odpowiedź
$u = -K\hat{\mathbf x} = -K(\mathbf x - \mathbf e)$ , więc $\dot{\mathbf x} = A\mathbf x - BK\mathbf x + BK\mathbf e$ .

5 Metoda i przykłady rozwiązane

Metoda: regulator z obserwatorem

Obserwowalność: $\operatorname{rank} W_o = n$ dla dostępnych czujników; jeśli nie, dołóż czujnik.
Regulator: $K$ z lokowania biegunów (lekcja 4.1) albo LQR (lekcja 4.3).
Obserwator: bieguny $A - LC$ 2–5 razy szybsze od regulatora; L = place(A.T, C.T, p).T.
Separacja: bieguny całości to suma obu zestawów.
Sprawdzenie: symulacja z szumem pomiaru i zakłóceniami nieznanymi modelowi.

Przykład 5.1 Prędkość z barometru

Wysokość drona, barometr $\sigma = 0{,}1$ m co 0,02 s. Zaprojektuj obserwator 3–4 razy szybszy niż regulator PD z biegunami $\omega_0 = 2$ .

Regulator: $\omega_0 = 2$ rad/s; obserwator $\omega_0 = 6$ rad/s, $\zeta = 0{,}7$ : $s^2 + 8{,}4s + 36$ .
$l_1 = 8{,}4 - 0{,}5 = 7{,}9$ , $l_2 = 36 - 0{,}5\cdot 7{,}9 = 32{,}05$ .
Symulacja: błąd prędkości ok. 0,2 m/s (RMS), różniczkowanie barometru dałoby ok. 7 m/s.

Wynik: $L = [7{,}9;\ 32{,}05]$ ; estymata prędkości ok. 35 razy dokładniejsza niż różnica pomiarów.

Sprawdzenie: wartości własne $A - LC$ to $-4{,}2 \pm 4{,}29j$ ✓.

Przykład 5.2 Za wolny obserwator

Ten sam układ z obserwatorem $\omega_0 = 2$ rad/s ( $L = [2{,}3;\ 2{,}85]$ ). Co się dzieje przy podmuchu −2 N przez 2 s?

Regulator z prawdziwym stanem: dron opada z 1 m do ok. 0,48 m.
Z wolnym obserwatorem: do ok. −0,66 m, czyli ponad 1,6 m spadku (dron uderzyłby w ziemię).
Z obserwatorem 6 rad/s: do ok. 0,17 m; 20 rad/s: do ok. 0,40 m.

Wynik: obserwator nie szybszy od regulatora psuje regulację mocniej niż szum.

Sprawdzenie: zasada separacji mówi o biegunach, nie o zakłóceniach nieznanych modelowi; tutaj liczy się szybkość korekty ✓.

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

„Obserwator to filtr, który wygładza pomiar.”: Robi więcej: odtwarza wielkości, których w ogóle nie mierzymy (prędkość z wysokości), korzystając z modelu.
„Im szybszy obserwator, tym lepiej.”: Szybki obserwator przepuszcza szum pomiaru. Optimum zależy od szumu i zakłóceń.
„Zasada separacji gwarantuje, że regulator z obserwatorem działa tak samo jak z pełnym stanem.”: Gwarantuje te same bieguny przy idealnym modelu. Odpowiedź na zakłócenia i odporność na błędy modelu mogą być gorsze.
„Więcej czujników zawsze pomaga.”: Czujnik, który mierzy tylko kombinację już obserwowalnych stanów, nie poprawia obserwowalności. Liczy się, co mierzy, a nie ile ich jest.
„Skoro obserwator ma model, nie potrzebuje sterowania $u$ .”: Potrzebuje: bez $Bu$ w modelu każda zmiana sterowania wyglądałaby jak zakłócenie i obserwator byłby stale spóźniony.

7 Zadania do przemyślenia

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

Zadania do przemyślenia

5 zadań · ok. 1,5 godziny

Estymowanie zakłócenia. Dołóż do modelu wysokości trzeci stan: nieznaną stałą siłę $F$ ( $\dot F = 0$ ). Czy rozszerzony układ jest obserwowalny z barometru? Do czego to się przyda?
Rozwiązanie
1. $\dot z = v$ , $\dot v = -0{,}5v + u + F$ , $\dot F = 0$ , $y = z$ . $W_o$ : $C = [1\ 0\ 0]$ , $CA = [0\ 1\ 0]$ , $CA^2 = [0\ -0{,}5\ 1]$ : rząd 3, obserwowalny.
2. Obserwator estymuje siłę paczki czy wiatru. Regulator może ją skompensować wprost: $u = -K\hat{\mathbf x} - \hat F$ . To alternatywa dla członu całkującego (ang. disturbance observer).
Nawigator i dryf. Zliczanie drogi (dead reckoning) to obserwator z $L = 0$ między pomiarami lądu. Dlaczego błąd pozycji rośnie, a nie zostaje stały?
Rozwiązanie
1. Pozycja to całka prędkości; błąd prędkości (log, prądy) całkuje się w rosnący błąd pozycji. Przy $L = 0$ błąd spełnia $\dot e = Ae$ plus niemodelowane wymuszenia; całkowanie nie gasi niczego.
2. Każdy pomiar lądu to chwilowe $L \ne 0$ , które ściąga błąd w dół. Nowoczesna wersja: GPS koryguje nawigację inercjalną (lekcja 4.4 Filtr Kalmana).
Obserwator zredukowanego rzędu. Barometr mierzy $z$ dokładnie (pomiń szum). Czy trzeba estymować obie zmienne? Zaproponuj estymator tylko prędkości.
Rozwiązanie
1. Nie: $z$ znamy, estymujemy tylko $v$ . Z $\dot z = v$ mamy „pomiar” $v$ jako pochodną $z$ , ale unikamy różniczkowania podstawieniem $w = \hat v - lz$ .
2. Chcemy $\dot{\hat v} = -0{,}5\hat v + u + l(\dot z - \hat v)$ . Z $w = \hat v - lz$ pochodna $\dot z$ się skraca: $\dot w = -(0{,}5 + l)\hat v + u$ , a $\hat v = w + lz$ . Błąd $v - \hat v$ gaśnie z biegunem $-(0{,}5 + l)$ . To obserwator Luenbergera zredukowanego rzędu: przydatny przy dokładnych czujnikach, wrażliwy na szum.
Obserwowalność orbity. Radar mierzy tylko odległość do satelity, nie kierunek. Czy z serii pomiarów odległości da się wyznaczyć orbitę? Od czego to zależy?
Rozwiązanie
1. Pojedynczy pomiar nie, ale ruch po orbicie zmienia odległość w sposób zależny od wszystkich elementów orbity: z serii pomiarów (i modelu dynamiki) orbita jest zwykle obserwowalna, z wyjątkiem symetrii (np. obrót orbity wokół linii radar–Ziemia daje te same odległości).
2. Obserwowalność zależy od geometrii i czasu obserwacji. Dlatego używa się kilku stacji albo pomiaru prędkości radialnej (Dopplera). Temat wraca w kursie astronautyki.
Czy obserwator może zdestabilizować pętlę? Według zasady separacji nie. Podaj realistyczny powód, dla którego w praktyce pętla z obserwatorem może się rozkołysać.
Rozwiązanie
1. Błąd modelu: jeśli obiekt różni się od modelu w obserwatorze (masa, opóźnienie, nieuwzględniona dynamika), macierz przestaje być trójkątna i bieguny się „mieszają”.
2. Opóźnienie pomiaru (barometr co 20 ms z filtrem) też psuje założenia. Zapas odporności regulatora z obserwatorem bywa dużo mniejszy niż z pełnym stanem: słynny kontrprzykład Doyle’a w lekcji 5.2 LQG i odporność.

8 Sprawdź się

Odpowiedz w myślach, zanim rozwiniesz odpowiedź.

1 Jak wygląda równanie obserwatora?

$\dot{\hat{\mathbf x}} = A\hat{\mathbf x} + Bu + L(y - C\hat{\mathbf x})$ .

2 Jaką dynamikę ma błąd estymacji?

$\dot{\mathbf e} = (A - LC)\mathbf e$ .

3 Jak sprawdzić obserwowalność?

$\operatorname{rank}[C;\ CA;\ \dots;\ CA^{n-1}] = n$ .

4 Czy wysokość jest obserwowalna z samego czujnika prędkości?

Nie: stałe przesunięcie wysokości nie zmienia żadnego pomiaru.

5 Co to zasada separacji?

Bieguny układu z regulatorem i obserwatorem to suma biegunów $A - BK$ i $A - LC$ .

6 Jaki kompromis rządzi wyborem

L

Szybkość nadążania za zakłóceniami kontra przepuszczanie szumu pomiaru.

7 Co to dualność?

Obserwator dla $(A, C)$ to lokowanie biegunów dla $(A^T, C^T)$ .

9 Karta wzorów i co dalej

Obserwator

$\dot{\hat{\mathbf x}} = A\hat{\mathbf x} + Bu + L(y - C\hat{\mathbf x})$

$\dot{\mathbf e} = (A - LC)\mathbf e$ ; $\;L$ = place(A.T, C.T, p).T

wysokość drona: $s^2 + (0{,}5 + l_1)s + (0{,}5l_1 + l_2)$

bieguny obserwatora 2–5× szybsze od regulatora

Obserwowalność i separacja

$W_o = [C;\ CA;\ \dots;\ CA^{n-1}]$ , obserwowalny $\iff \operatorname{rank} W_o = n$

$u = -K\hat{\mathbf x}$ : bieguny $= \operatorname{eig}(A - BK) \cup \operatorname{eig}(A - LC)$

rozszerzenie o zakłócenie: $\dot F = 0$

Do przeczytania

Obowiązkowo: K. J. Åström, R. M. Murray, Feedback Systems, rozdz. 8 (obserwowalność i estymacja stanu).
Dla chętnych: A. Tewari, Modern Control Design with MATLAB and SIMULINK, rozdz. 5.3 (obserwatory, przykłady lotnicze); D. G. Luenberger, „Observing the state of a linear system”, IEEE Trans. Military Electronics 8 (1964).

Wybór biegunów, zarówno regulatora, jak i obserwatora, był dotąd sztuką: „2–5 razy szybciej”, „para dominująca z wymagań”. Następna lekcja zastępuje te reguły kciuka jednym pytaniem: ile kosztuje uchyb, a ile sterowanie? Odpowiedź daje regulator LQR, a dualne pytanie o obserwator prowadzi do filtru Kalmana.