Monitorowanie i Diagnostyka w Systemach Sterowania (MiDwSS) Podstawowe sposoby opisu niepewności, wybrane zagadnienia zastosowania estymacji rekursywnej dla potrzeb monitorowania i diagnostyki w systemach sterowania Politechnika Gdańska Wydział Elektrotechniki i Automatyki Kierunek: Specjalność: Studia stacjonarne II stopnia: Automatyka i Robotyka Systemy Sterowania i Wspomagania Decyzji rok I, semestr II Opracowanie: dr inż. Tomasz Rutkowski MiDwSS 03
Plan prezentacji Podstawowe sposoby opisu niepewności Liniowe i rekursywne postaci obserwatorów Luenbergera Filtr RLS (ang. Recursive Least Squares) Filtr WRLS (ang. Weighted Recursive Least Squares) Filtr Kalmana (ang. Kalman Filter, KF ) Nieliniowe i rekursywne postaci obserwatorów Filtr RLS i WRLS Rozszerzony Filtr Kalmana (ang. Etended Kalman Filter, EKF) Sformułowanie zadań estymacji punktowej (RLS, WRLS) oraz przedziałowej (ang. set bounded) zmiennych i parametrów obiektów dynamicznych (liniowych/nieliniowych) na ruchomym oknie pomiarowym zadania optymalizacji MiDwSS 03
Schemat ideowy układu diagnostyki PRZYKŁADOWE ESTYMATORY MiDwSS 03 3
Schemat ideowy układu diagnostyki Estymacja parametrów Filtr RLS Filtr WRLS Rozszerzony Filtr Kalmana estymatory oparte o metody optymalizacji (rozwiązanie odpowiednich zadań optymalizacji prowadzących do estymat punktowych lub przedziałowych) Estymacja wyjść/stanu: Obserwator Laundbergera Filtr Kalmana, Rozszerzony Filtr Kalmana estymatory oparte o metody optymalizacji (rozwiązanie odpowiednich zadań optymalizacji prowadzących do estymat punktowych lub przedziałowych) MiDwSS 03 4
Schemat ideowy układu diagnostyki estymacja parametrów X OB Y Θˆ Θ OB - Obiekt X - sygnał wejściowy Y - sygnał wyjściowy Θ - parametry obiektu Θˆ - estymata parametrów obiektu MiDwSS 03 5
Schemat ideowy układu diagnostyki estymacja wyjść/stanu X OB Y Y ) OB OB - Obiekt X - sygnał wejściowy Y - sygnał wyjściowy Y ) ) e Y Y - residuum - estymata sygnału wyjściowego z obiektu MiDwSS 03 6
Schemat ideowy układu diagnostyki estymacja parametrów i/lub wyjścia/stanu X OB Y Y ), Θˆ Θ, Y OB - Obiekt X - sygnał wejściowy Y - sygnał wyjściowy Y ) - estymata sygnału wyjściowego z obiektu Θ - parametry obiektu Θˆ - estymata parametrów obiektu MiDwSS 03 7
Estymacja, Obserwatory, Filtry Estymacja to proces podejmowania decyzji lub wydawania sądu, co do przybliżonej wartości pewnych parametrów czy zmiennych charakteryzujących dany obiekt, na podstawie dostępnej informacji o obiekcie, łącznie z danymi o procesach w nim zachodzących, uporządkowane w danym zbiorze obserwacji. Idea obserwatora stanu polega na wykorzystaniu sygnałów wejściowych i wyjściowych systemu dynamicznego do estymacji (śledzenia zmienności) zmiennych stanu. Idea filtracji polega na takim przekształceniu sygnału wejściowego, przez filtr o odpowiedniej strukturze, aby wynik filtracji jak najmniej różnił się od sygnału odniesienia przy założonym kryterium błędu. MiDwSS 03 8
PODSTAWOWE MODELE NIEPEWNOŚCI MiDwSS 03 9
Modele niepewności W praktyce każdy zbiór rzeczywistych obserwacji zawiera informacje obarczone błędami, co związane jest to głównie z niepewnością, niedeterministycznym charakterem obiektu. Uwzględniając fakt, iż nie zawsze dysponujemy pełną informacją pomiarową (chociażby ze względów finansowych) problem estymacji można zdefiniować jako szacowanie wielkości niemierzonych (nieznanych) parametrów i zmiennych charakteryzujących obiekt na podstawie dostępnych obserwacji (pomiarów) innych parametrów i zmiennych badanego obiektu oraz na bazie modelu matematycznego obiektu opisującego relacje pomiędzy charakteryzującymi go zmiennymi i parametrami. MiDwSS 03 0
Modele niepewności Wykorzystanie modeli matematycznych jak i danych pomiarowych, w celu estymacji nieznanych parametrów i zmiennych charakteryzujących obiekt, jednoznacznie wiąże się z niepewnością. Źródła niepewności wynikają przede wszystkim z: błędów struktury modelu matematycznego, niepewnych parametrów modelu matematycznego, błędów pomiarowych obserwowanych wielkości, błędów metod numerycznych i symulacji (dokładność obliczeń, błędy zaokrągleń itp.), wiedzy a priori oraz heurystyk wykorzystanych przy budowie modelu systemu (niepewność w tym przypadku jest bardzo trudna do oszacowania). MiDwSS 03
Modele niepewności Trudno jest wskazać uniwersalną metodę opisu niepewności. Przy wyborze jej odpowiedniej reprezentacji należy wziąć pod uwagę między innymi następujące aspekty związane z jej opisem: przyczyny niepewności, ilość jak i jakość dostępnej informacji, typ dostępnej informacji, metody przetwarzania dostępnych informacji, itp. MiDwSS 03
Modele niepewności Wyróżnia się wiele sposobów opisu niepewności, z których najczęściej wykorzystuje się następujące modele: probabilistyczny model niepewności, rozmyty model niepewności, model niepewności wyrażony w postaci zbiorów ograniczonych (ang. set-bounded). MiDwSS 03 3
Modele niepewności Probabilistyczny model niepewności: nieznane wielkości reprezentowane są przez wartości zmiennych losowych wylosowanych ze ściśle określonych zbiorów, niepewność opisana jest przez rozkład prawdopodobieństwa; trajektorie tych wielkości są reprezentowane przez procesy stochastyczne. MiDwSS 03 4
Modele niepewności Probabilistyczny model niepewności Zmienna losowa: {,, } X, K Prawdopodobieństwo że zmienna losowa X przyjmie wartość ze zbioru [a, b]: ( X [ a b] ) P, n MiDwSS 03 5
Modele niepewności Probabilistyczny model niepewności Średnia n n i i Wartość oczekiwana (najbardziej prawdopodobna w sensie statystycznym) n E( X ) lim i n n i ( X ) X µ m m E inne oznaczenia MiDwSS 03 6
Modele niepewności Probabilistyczny model niepewności Wariancja υ υ E E [( X E( X )) ] E ( X m) [ ] n lim ( i m) [( ( )) ] ( X E X E X ) E( X ) n n i wartość rozrzutu realizacji wokół wartości średniej Odchylenie standardowe σ υ wartość koncentracji realizacji i w przedziale [m-σ, m+σ] MiDwSS 03 7
Modele niepewności Probabilistyczny model niepewności Dystrybuanta zmiennej losowej X: F n( ) ( ) lim P ( X ) n n Przykład: rozkład Gaussa F() Wyraża prawdopodobieństwo tego, że zmienna losowa X przyjmie wartość mniejszą lub równą MiDwSS 03 8
Modele niepewności Probabilistyczny model niepewności Gęstość prawdopodobieństwa ( ) df f ( ) Przykład: rozkład Gaussa d f ( ) X σ m σ π e ( m, ) ~ N σ f() MiDwSS 03 9
Modele niepewności Probabilistyczny model niepewności Rozkład Gaussa podstawowe właściwości X ( m, ) ~ N σ Y ax + b Jeżeli a to ( Y ~ N am + b, aσ ) f ( y) aσ y ( am + b ) aσ π e X X ( m ) N, ~ σ Jeżeli i są niezależne, oraz i to f ( + ) π X ( σ + σ ) ( ( m + m ) σ e ( σ + ) ) X ( m ) N, ~ σ MiDwSS 03 0
Modele niepewności Probabilistyczny model niepewności Kowariancja {( y ), (, y ),, (, )} F, K n y n (, y) P( X i Y y) m E ( X ) lim i my E( Y ) lim n n n i n n n i y i υ E [( ) ] n X m lim ( m ) n n i i υ y E [( ) ] n X my lim ( i my ) n n i MiDwSS 03
Modele niepewności Probabilistyczny model niepewności Kowariancja (cd.) c y E [( X m )( )] ( )( ) Y m lim y i m y i m y n n n n i ρ y σ c y σ y -gdy zmienne niezależne to: ρ y 0 - gdy zmienne zależne to: - ρ y MiDwSS 03
Modele niepewności Modele niepewności Probabilistyczny model niepewności Macierz wariacyjno-kowarjacyjna dla n zmiennych losowych c c υ L MiDwSS 03 3 n n n n n c c c c c c C υ υ υ L M M M L L
Modele niepewności Probabilistyczny model niepewności Autokorelacja ( t, t ) E[ X ( t ) X ( t )] R, t Jeżeli proces jest stacjonarny to: τ t ( τ ) E[ X ( t) X ( t τ )] R, MiDwSS 03 4
Modele niepewności Probabilistyczny model niepewności Biały szum R( τ ) τ τ MiDwSS 03 5
Modele niepewności Model niepewności w postaci zbiorów ograniczonych (ang. set-bounded): niepewne wielkości reprezentowane są w postaci ograniczeń dolnych i górnych, wewnątrz których na pewno znajdują się nieznane wielkości, trajektorie ograniczeń są modelowane przez ograniczone zbiory w przestrzeni trajektorii. Pomiar: y( t) y( t) + ε ( t) ε m m m + ( t) ε ( t) ε ( t) m znane a priori m y y m (t)-ε+ m (t) y m (t) y m (t)-ε - m (t) t MiDwSS 03 6
Modele niepewności Model niepewności w postaci zbiorów ograniczonych (ang. set-bounded): niepewne wielkości reprezentowane są w postaci ograniczeń dolnych i górnych, wewnątrz których na pewno znajdują się nieznane wielkości, trajektorie ograniczeń są modelowane przez ograniczone zbiory w przestrzeni trajektorii. Estymaty zmiennej stanu (t): ma (t) Xˆ ( t ) [ min ˆ : arg min ma Xˆ ( t), z Xˆ ma ( t)] z min (t) ^ (t) (t) centrum Czybyszewa t MiDwSS 03 7
Modele niepewności Rozmyty model niepewności: bazuje na teorii zbiorów rozmytych, niepewne wielkości reprezentowane są przez wartości zmiennych rozmytych, zdefiniowanych bezpośrednio przez funkcję przynależności. Zbiór rozmyty błędu prędkości e V µ( µ ( ) e V e V m s 0 0 0 ujemny duży bliski zera dodatni duży MiDwSS 03 8
ESTYMACJA PARAMETRÓW MiDwSS 03 9
Schemat ideowy układu diagnostyki estymacja parametrów X OB Y Θˆ Θ OB - Obiekt X - sygnał wejściowy Y - sygnał wyjściowy Θ - parametry obiektu Θˆ - estymata parametrów obiektu MiDwSS 03 30
Schemat ideowy układu estymacji Źródła błędów systemu Sterowanie System/ Obiekt Stan Urządzenia pomiarowe Źródła błędów pomiarowych Urządzenia pomiarowe Pomiary wyjść Pomiary wejść Estymator Estymaty stanu/ parametrów Źródła błędów pomiarowych MiDwSS 03 3
Podstawowy model pomiarowy Model pomiarowy ~ y y + e~ e ~ ~ y y błąd pomiaru wartość mierzona wartość prawdziwa ~ y y ˆ + ) e błąd resztkowy (residuum) ) e ~ y yˆ wartość mierzona wartość estymowana e~ - wartość praktycznie nigdy nieznana; mechanizm generujący ten błąd zwykle jest aproksymowany przez pewien znany proces (np. szum gaussowski o zerowej wartości średniej i znanej wariancji σ lub zbiór ograniczony o granicach znanych a priori ) e ) - wartość znana w momencie wyznaczenia wartości estymowanej zmiennej MiDwSS 03 3
ESTYMACJA PARAMETRÓW LRS, WRLS MiDwSS 03 33
Estymacja parametrów filtr RLS (ang. Recursive Least Squares) Idea RLS Zakładamy, że pewien obiekt (proces) generuje wektor składający się z N zmiennych (), (),, (N) Zakładamy niezmienność procesu Nie możemy ich zmierzyć bezpośrednio ale dysponujemy aparaturą pomiarową, która umożliwia pomiar M- elementowego wektora z: z(), z(),, z(m), M>N: z H + gdzie H to macierz układu pomiarowego o wymiarach MN v to addytywny szum pomiarowy Estymatę oznaczamy jako ˆ a odpowiadający jej błąd z Hˆ + MiDwSS 03 34 v vˆ vˆ
Estymacja parametrów filtr RLS (ang. Recursive Least Squares) Idea RLS (cd.) Estymata ˆ będzie tym lepsza im mniejszy będzie błąd sumy kwadratów związany z vˆ podstawiając: J vˆ T vˆ M i v i J ( ) T z Hˆ ( z Hˆ ) J osiąga minimum gdy: J ˆ H T ( z Hˆ ) 0 MiDwSS 03 35
Estymacja parametrów filtr RLS (ang. Recursive Least Squares) Idea RLS (cd.) Po kolejnych przekształceniach H T Hˆ H T z ˆ ( T ) T H H H z Czy jest to rozwiązanie efektywne? MiDwSS 03 36
Estymacja parametrów Estymacja parametrów filtr RLS (ang. filtr RLS (ang. Recursive Recursive Least Least Squares Squares) Przykład rekurencji Rekurencyjne wyznaczanie średniej: n i n i n + + n n n MiDwSS 03 37 + + i i n i i n n n n n n n ( ) + + n n n n n n n n n
Estymacja parametrów filtr RLS (ang. Recursive Least Squares) Standard rekursywnej, adaptacyjnej estymaty parametrów nowa estymata jej prognoza + korekta korekta wzmocnienie * (pomiar-prognoza pomiaru) MiDwSS 03 38
Estymacja parametrów filtr RLS (ang. Recursive Least Squares) Wyprowadzenie RLS ˆ Wykorzystując poprzednie wyprowadzenie dla metody najmniejszych kwadratów: ˆ ( T ) T H H H z i wprowadzając oznaczenie bieżącego pomiaru k: ˆ ( T ) T H ( k) z( k) ( k) H ( k) H ( k) Otrzymuje się ostatecznie rekursywną postać filtru RLS: [ ] ( ) ( ) ( ) ( ) T k + ˆ k + K k z k + h ( k + ) ˆ ( k) ( k) + K( k) [ z( k + ) zˆ ( ) ] ˆ k + MiDwSS 03 39
Estymacja parametrów filtr RLS (ang. Recursive Least Squares) Wyprowadzenie RLS (cd.) ( k + ) ˆ ( k) + K( k) [ z( k + ) zˆ ( k ) ] ˆ + nowa estymata jej prognoza błąd pomiędzy pomiarem a jego prognozą korekta ( ) T k + h ( k ) ˆ ( k) zˆ + nowy, znany stara wektor estymata MiDwSS 03 40
Estymacja parametrów filtr RLS i WRLS Algorytm estymacji (dla RLS λ, dla WRLS λ<). Inicjalizacja: estymata N-elementowego wektora na podstawie N pierwszych pomiarów W P ˆ diag N N ( λ, λ,..., λ,) T ( H N W H ) ( N ) ( ) ( N ) T ( N ) P( N ) H ( N ) W z( N ) k N. Nowy pomiar z(k+), {nowe h(k+) i nowy szum v(k+)}: T ( k + ) h ( k + ) ˆ ( k + ) + v( k ) zˆ + 3. Modyfikacja wzmocnienia: c K ( ) T ( k + ) λ + h ( k + ) P( k) h( k + ) ( k + ) P( k ) h( k + ) c( k + ) MiDwSS 03 4
Estymacja parametrów filtr RLS (ang. Recursive Least Squares) 4. Predykcja parametru korekta estymaty wielkości szukanej: ˆ T ) ( ) ( k + ) ˆ ( k) + K( k + ) z( k + ) h ( k + ) ( k ) 5. Modyfikacja macierzy P: T P k + Ι K k + h k + λ 6. Następna iteracja: ( ) P( N ) ( ) ( ) ( ) k k + Skok do kroku MiDwSS 03 4
Estymacja parametrów filtr WRLS (ang. Weighted Recursive Least Squares) Idea WRLS Estymata ˆ będzie tym lepsza im mniejszy będzie błąd sumy kwadratów związany z vˆ, przy uwzględnieniu diagonalnej macierzy wag W: podstawiając: ˆ J vˆ T Wvˆ M i wiv i ( T ) T H WH H Wz przy czym: [ v] 0 E [ vv ] R E T W R MiDwSS 03 43
Przykłady estymacji parametrów filtry RLS, WRLS Przykładowe zadanie: Estymacja parametrów modelu matematycznego opisanego równaniem: y( k) n 0 ( n k) h( k) + v( k) metodami RLS i WRLS, na podstawie znajmości sygnału wejściowego (n) i wyjściowego y(n) zaszumionego szumem v(n) Przy czym jego parametry to [-0.987;,345] Zaczerpnięty z [] MiDwSS 03 44
Przykłady estymacji parametrów filtry RLS, WRLS MiDwSS 03 45
Przykłady estymacji parametrów filtry RLS, WRLS Parametry stałe [-0.987;,345] 0 POMIAR 8 6 4 z(k) 0 - -4-6 -8-0 0 00 400 600 800 000 00 400 600 800 000 k MiDwSS 03 46
Przykłady estymacji parametrów filtry RLS, WRLS -0.6 ESTYMATA h RLS.8 ESTYMATA h -0.65.75.7-0.7.65-0.75.6 he(k) -0.8 he(k).55-0.85.5.45-0.9.4-0.95.35-0 00 400 600 800 000 00 400 600 800 000 k.3 0 00 400 600 800 000 00 400 600 800 000 k WRLS -0.7 ESTYMATA h.9 ESTYMATA h -0.75.8-0.8.7-0.85-0.9.6 he(k) -0.95 he(k).5 -.4 -.05 -..3 -.5 0 00 400 600 800 000 00 400 600 800 000. 0 00 400 600 800 000 00 400 600 800 000 k k MiDwSS 03 47
Przykłady estymacji parametrów filtry RLS, WRLS Parametry zmienne [±0.987; ±,345] 0 POMIAR 8 6 4 z(k) 0 - -4-6 -8-0 0 00 400 600 800 000 00 400 600 800 000 k MiDwSS 03 48
Przykłady estymacji parametrów filtry RLS, WRLS ESTYMATA h RLS 3 ESTYMATA h 0.8 0.6 0.4 0. he(k) 0 he(k) 0-0. -0.4 - -0.6 - -0.8-0 00 400 600 800 000 00 400 600 800 000 k -3 0 00 400 600 800 000 00 400 600 800 000 k.5 ESTYMATA h WRLS 3 ESTYMATA h 0.5 he(k) 0 he(k) 0-0.5 - - - -.5 0 00 400 600 800 000 00 400 600 800 000 k -3 0 00 400 600 800 000 00 400 600 800 000 k MiDwSS 03 49
Nieliniowe filtry RLS, WRLS Bardziej szczegółowe informacje na temat liniowych i nieliniowych filtrów RLS i WRLS można znaleźć między innymi w materiałach wykładowych z przedmiotu: Modelowanie i Identyfikacja (wykłady 3, 4 i 5, sem. I studiów stacjonarnych I stopnia) MiDwSS 03 50
ESTYMACJA WYJŚĆ/STANU Obserwator Luenbergera Kalmana, Rozszerzony Filtr Kalamna MiDwSS 03 5
Schemat ideowy układu diagnostyki estymacja wyjść/stanu U OB Y Y ) OB OB - Obiekt U - sygnał wejściowy Y - sygnał wyjściowy Y ) e Y - estymata sygnału wyjściowego z obiektu ) Y MiDwSS 03 5
Estymacja wyjść/stanu Idea obserwatora Luenbergera wymaga znajomości wszystkich zmiennych stanu, wymagane są idealne czujniki pomiarowe o nieograniczonym paśmie znane wszystkie parametry systemu (opis w przestrzeni stanu, macierze A, B, C, D) Obserwator Luenbergera rzędu zredukowanego Więcej szczegółowych informacji o obserwatorach pełnym i zredukowanym Luenbergera można znaleźć między innymi w materiałach wykładowych z przedmiotu: Teoria Sterowania (sem. I studiów stacjonarnych I stopnia) MiDwSS 03 53
Estymacja wyjść/stanu Idea obserwatora Luenbergera Realny system dynamiczny u(t) (t) y(t) A, B C A, B ) ( t) Numeryczna kopia systemu Realny system dynamiczny ( k + ) A( k) Bu( k) ˆ( k + ) Aˆ ( k) + Bu( k) + Błąd odtworzenia stanu Numeryczna kopia systemu ~ ~ ( k) ( k) ˆ ( k) A( k) + Bu( k) Aˆ ( k) Bu( k) A( k) ~ ( 0) ( 0) ˆ( 0) MiDwSS 03 54
Estymacja wyjść/stanu Idea obserwatora Luenbergera Realny system dynamiczny u(t) (t) y(t) A, B C A, [B L] ) ( t) C ŷ( t) Korekta estymaty: ˆ Numeryczna kopia systemu ( k + ) Aˆ ( k) + Bu( k) + L( y( k) ˆ ( k) ) Błąd odtworzenia stanu ~ k k ˆ k A k + Bu k Aˆ k Bu k L y k ˆ k A LC ~ ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ( ) ( )) ( ) ( k) ~ ( 0) ( A LC) ( 0) ˆ( 0) MiDwSS 03 55
Estymacja wyjść/stanu Przykład zastosowania obserwatora Luenbergera MiDwSS 03 56
Estymacja wyjść/stanu Przykład zastosowania obserwatora Luenbergera 70 60 prawdziwy stan obserwator L obserwator L obserwator L3 50 40 (k) 30 0 0 0-0 0 0 0 30 40 50 60 70 80 k MiDwSS 03 57
Estymacja wyjść/stanu Przykład zastosowania obserwatora Luenbergera 40 30 0 0 (k) 0-0 odchylenie standardowe szumu pomiarowego 0.05, 0.5, 0.5-0 -30-40 -50 0 0 0 30 40 50 60 70 80 k MiDwSS 03 58
Estymacja wyjść/stanu Idea Filtru Kalmana (estymacja minimalno-średniokwadratowa, MMS, Minimum Mean Square) Filtr Kalmana stosuje się do estymacji wartości obserwowanych zmiennych, minimalizując błędu estymacji stanu ~ ˆ w sensie statystycznym. Filtr Kalmana uwzględnia informacje o procesie z dwóch źródeł: urządzenia pomiarowe (wiedza a posteriori), model systemu (wiedza a priori). Filtr Kalmana działa zgodnie ze schematem predyktor-korektor Rudolph. E. Kalman, "A New Approach to Linear Filtering and Prediction Problems", Trasactions of the ASME: Journal of Basic Engineering, vol 8, Series D, pp. 35-45, 960. MiDwSS 03 59
Estymacja wyjść/stanu Idea Filtru Kalmana (zagubieni na morzu) 0.6 0.4 0. 0. 0.08 0.06 z Określenie pozycji na podstawie pomiaru z wykonanego w chwili czasu t : średnia: z odchylenie standardowe: σ z najlepsza estymata położenia: (t ) z wariancja błędu estymaty: σ y (t ) σ z ˆ 0.04 0.0 σ z 0 0 0 0 30 40 50 60 70 80 90 00 z może być predykcją pozycji dla kolejnej chwili czasu t MiDwSS 03 Zaczerpnięto z [3] 60
Estymacja wyjść/stanu Idea Filtru Kalmana (zagubieni na morzu) z z 0.6 0.4 0. 0. 0.08 0.06 Ponowne, po krótkiej chwili określenie pozycji na podstawie pomiaru (pomiar dokładniejszy niż poprzednim razem) z w chwili czasu t t : średnia - z odchylenie standardowe - σ z 0.04 0.0 σ z konieczna korekcja estymaty położenia dokonanej sekstansem, wyznaczenie - (t ) ˆ 0 0 0 0 30 40 50 60 70 80 90 00 Jak dokonać fuzji informacji z dwóch pomiarów o różnej dokładności? Założyliśmy rozkład Gaussa Zaczerpnięto z [3] MiDwSS 03 6
Estymacja wyjść/stanu Idea Filtru Kalmana (zagubieni na morzu) Z właściwości rozkładu normalnego: Zatem najlepszą estymatą położenia w chwili czasu t będzie: ˆ (t ) µ ˆ ( t ) z ; Zaczerpnięto z [3] MiDwSS 03 6
Estymacja wyjść/stanu Idea Filtru Kalmana (zagubieni na morzu) 0.6 0.4 0. 0. µ z z Skorygowana pozycja w chwili czasu t : optymalna estymata położenia: (t ) µ wariancja błędu estymaty : σ (t ) (poprzedni slajd) ˆ 0.08 0.06 0.04 0.0 0 0 0 0 30 40 50 60 70 80 90 00 MiDwSS 03 Zaczerpnięto z [3] 63
Estymacja wyjść/stanu Idea Filtru Kalmana Działanie zgodnie ze schematem Predykcja-Korekcja Optymalna estymata jej prognoza + korekta korekta wzmocnienie Kalmana * (pomiar - prognoza pomiaru) Wariancja estymaty Wariancja prognozy*( - wzmocnienie Kalmana) MiDwSS 03 64
Estymacja wyjść/stanu Idea Filtru Kalmana (estymacja minimalno-średniokwadratowa, MMS, Minimum Mean Square) Minimalizuje się funkcję jakości w postaci: J E [( ) ( ) ] T ˆ ˆ Rozważa się model: z ( k + ) A ( k) + B u( k) + G w( k) ( k + ) H ( k + ) + v( k + ) - model procesu - model pomiaru proces dynamiczny, niestacjonarny Szczegółowe informacje można znaleźć np. w [] MiDwSS 03 65
Estymacja wyjść/stanu Idea Filtru Kalmana (estymacja MMS) MiDwSS 03 66
Estymacja wyjść/stanu Idea Filtru Kalmana (estymacja MMS) MiDwSS 03 67
Estymacja wyjść/stanu Idea Filtru Kalmana Błąd estymacji a priori (model): e ( k + k) ( k + ) ˆ ( k + k) Błąd estymacji a posteriori (pomiar): e ( k + k + ) ( k + ) ˆ ( k + k + ) Estymata błędu kowariancji a priori: [ ] T ( k + k) E e( k + k) e ( k k) P + Estymata błędu kowariancji a posteriori: [ ] T ( k + k + ) E e( k + k + ) e ( k + k ) P + MiDwSS 03 68
Estymacja wyjść/stanu Idea Filtru Kalman Celem filtru Kalmana jest wyznaczenie estymaty a posteriori ˆ ( k + k + ) jako liniowej kombinacji estymaty a priori ˆ k + k i ważonej różnicy pomiędzy aktualnym pomiarem z( k +) a predykcją pomiaru H ˆ ( k + k) : ( k + k + ) ˆ ( k + k) + K( k + ) ( z( k + ) H ˆ ( k k) ) ˆ + ( ) ( ) Wzmocnienie Kalmana K k + dobiera się tak aby estymacja kowariancji błędu aposteriori P k + k + była jak najmniejsza ( ) [ ] T H + R ( ) ( ) T + P k + k H H P( k + k) K k MiDwSS 03 69
Estymacja wyjść/stanu Idea Filtru Kalmana K ( k + ) H P P ( ) T k + k H ( k k) T + H + R lim K k R 0 ( + ) H Większe zaufanie do pomiaru niż do predykcji pomiaru ( ) 0 lim K k + P (k ) 0 + Większe zaufanie do predykcji pomiaru niż pomiaru MiDwSS 03 70
Estymacja wyjść/stanu Algorytm Filtru Kalmana MiDwSS 03 7
Estymacja wyjść/stanu Przykład : zastosowania Filtru Kalmana Odszumianie (filtrowanie) próbkowanych sygnałów zebranych z systemu GPS (0 razy na sekundę) zamontowanego w pojeździe autonomicznym poruszającym się ruchem jednostajnie przyspieszonym ( m/s ) po prostej drodze. Jakie jest położenie pojazdu? Pomiar położenia odbywa się z dokładnością 0 m. Prędkość mierzy się z dokładnością 0. m/s. MiDwSS 03 7
Estymacja wyjść/stanu Estymacja wyjść/stanu Przykład : zastosowania Filtru Kalmana Jak określić macierze R i Q? [ ] T k k E R ) ( ) ( µ µ 00 0 ) ( s R σ sv s s MiDwSS 03 73 [ ] T k w k w E Q ) ( ) ( [ ] v vs sv s E v s v s E Q ) ( ) ( ) ( ) ( T T T T T T Q v v v v σ σ σ σ 0. T 0. σ v
Estymacja wyjść/stanu Przykład : zastosowania Filtru Kalmana 400 00 000 Pozycja pojazdu prawdziwa mierzona estymowana 800 Pozy ycja [m] 600 400 00 0-00 0 5 0 5 0 5 30 35 40 45 50 Czas [s] MiDwSS 03 74
Estymacja wyjść/stanu Przykład : zastosowania Filtru Kalmana 40 30 Blad pomiaru pozycji i Blad estymacji pozycji pomiaru estymaty 0 Blad Po ozycji [m] 0 0-0 -0-30 0 5 0 5 0 5 30 35 40 45 50 Czas [s] MiDwSS 03 75
Estymacja wyjść/stanu Przykład : zastosowania Filtru Kalmana 60 50 Predkosc pojazdu prawdziwa estymowana 40 Predko osc [m/s] 30 0 0 0 0 5 0 5 0 5 30 35 40 45 50 Czas [s] MiDwSS 03 76
Estymacja wyjść/stanu Przykład : zastosowania Filtru Kalmana 0.4 Blad estymacji predkosci 0.3 0. Blad pred dkosci [m/s] 0. 0-0. -0. -0.3 0 5 0 5 0 5 30 35 40 45 50 Czas [s] MiDwSS 03 77
Estymacja wyjść/stanu Przykład : zastosowania Filtru Kalmana Odszumianie (filtrowanie) próbkowanych sygnałów zebranych z analogowego czwórnika RLC. Napięcie zasilania u we (t) to napięcie o przebiegu bipolarnym prostokątnym z przedziału 0- V, R5 kω, L.5 H i C 0. µf. Napięcie na kondensatorze mierzone jest co T0-4 sekundy. Odchylenie standardowe szumu pomiarowego i procesu przyjęto na poziomie 0.. Zaczerpnięty z [] MiDwSS 03 78
Estymacja wyjść/stanu Przykład : zastosowania Filtru Kalmana ( t) u ( t) c ( ) t d dt du dt c d dt d dt ( t) LC R L LC ( t) ( t) + ( t) u d d dt dt 0 LC R L ( t) ( t) + LC u ( t) + w( t) LC MiDwSS 03 79
Estymacja wyjść/stanu Przykład : zastosowania Filtru Kalmana do estymacji stanu d ( t) u( t) w( t) d dt 0 + + ( t) 6 6 6 6 4 0 0 4 0 4 0 dt A d e AT B d ( AT ) e Ι A B z 0.983 360.3 0.000 0.803 0.087 360.3 0.087 360.3 ( k + ) ( k) + u( k) + w( k) ( k + ) [ 0] ( k + ) + v( k + ) MiDwSS 03 80
Estymacja wyjść/stanu Przykład : zastosowania Filtru Kalmana do estymacji stanu MiDwSS 03 8
Estymacja wyjść/stanu Przykład : zastosowania Filtru Kalmana do estymacji stanu X(K) (CZERW) I JEGO POMIAR Z(K) (NIEB) 0-0 50 00 50 00 50 300 k X(K) (CZERW) I JEGO ESTYMATA XE(K/K)(NIEB) 0-0 50 00 50 00 50 300 k WSPÓŁCZYNNIK WZMOCNIENIA KA(k) 0. 0. 0 0 50 00 50 00 50 300 k MiDwSS 03 8
Estymacja wyjść/stanu Przykład : zastosowania Filtru Kalmana do estymacji stanu 000 X(K) (CZERW) I JEGO POMIAR Z(K) (NIEB) 0-000 0 50 00 50 00 50 300 k X(K) (CZERW) I JEGO ESTYMATA XE(K/K)(NIEB) 000 0-000 0 50 00 50 00 50 300 k WSPÓŁCZYNNIK WZMOCNIENIA KA(k) 50 0-50 -00 0 50 00 50 00 50 300 k MiDwSS 03 83
Estymacja wyjść/stanu Przykład 3: zastosowania Filtru Kalmana do estymacji parametrów Rozważamy model postaci: ( k + ) A( Θ) ( k) + B( Θ) u( k) + G w( k) z( k) H ( Θ) ( k) + v( k) gdzie wektor parametrów jest nieznany. Wprowadzając nowy wektor zmiennych stanu: y ( k) przy czym ( k) ( ) Θ k Θ Θk ( ) Θ MiDwSS 03 84
Estymacja wyjść/stanu Przykład 3: zastosowania Filtru Kalmana do estymacji parametrów Otrzymuje się nieliniowy model w przestrzeni stanów: y z + ( k + ) f ( y( k), u( k) ) ( k ) h ( y ( k ) ) + v ( k ) + ( ) w k 0 Rozwiązanie: Rozszerzony Filtr Kalmana (dalsza część wykładu) przy czym: f ( y( k ) u( k) ) h ( Θ) ( k ) + B( Θ) u( k ) Θ( k ) A, ( y( k) ) H ( Θ) ( k) MiDwSS 03 85
Estymacja wyjść/stanu Idea Rozszerzonego Filtru Kalmana Rozszerzony Filtr Kalmana przeznaczony jest dla obiektów opisanych modelem nieliniowym i/lub nieliniowym równaniem pomiarowym. Równania takiego nieliniowego systemu można zapisać jako: Rozszerzony Filtr Kalmana jest Filtrem Kalamana linearyzowanym wokół aktualnej średniej i kowariancji każdej zmiennej stanu. f(), h() funkcje nieliniowe Inna notacja niż poprzednio MiDwSS 03 86
Estymacja wyjść/stanu Idea Rozszerzonego Filtru Kalmana Macierze Jacobiego pochodnych cząstkowych funkcji f() względem i w: Macierze Jacobiego pochodnych cząstkowych funkcji h() względem i v: Inna notacja niż poprzednio MiDwSS 03 87
Estymacja wyjść/stanu Algorytm Rozszerzonego Filtru Kalmana MiDwSS 03 Inna notacja niż poprzednio 88
Estymacja wyjść/stanu Przykład 4: zastosowania Rozszerzonego Filtru Kalmana Nieliniowa dynamika obiektu Nieliniowe równanie pomiarowe d spadające ciało g radar r Zaczerpnięto z [4] MiDwSS 03 r 89
Estymacja wyjść/stanu Estymacja wyjść/stanu Przykład 4: zastosowania Rozszerzonego Filtru Kalmana Model systemu (ciągły) + w w g d & & β 3 & MiDwSS 03 90 Model pomiaru (ciągły) 3 3 3 0 w & ( ) v r r z + + β 3 0 e d k ρ ρ
Estymacja wyjść/stanu Przykład 4: zastosowania Rozszerzonego Filtru Kalmana 5 Rozszerzony Filtr Kalmana 5 600 4 4 3 3 400 Błąd estymacji wysokoś ści (ft) 0 - - Błąd estymacji prędkośc ci (ft/s) 0 - - Błąd estymacji wsp. β (lb/ft ) 00 0-00 -3-3 -400-4 -4-5 0 5 0 5-5 0 5 0 5-600 0 5 0 5 czas (s) czas (s) czas (s) MiDwSS 03 9
Estymacja wyjść/stanu Przykład 4: zastosowania Rozszerzonego Filtru Kalmana 0 04 Pomiar, Prawdziwa pozycja, Estymat 8 elevation (ft) 6 4 0-4 6 8 0 4 6 czas (s) Prawdziwa pozycja - Estymata 0-000 -000-3000 -4000 4 6 8 0 4 6 czas (s) MiDwSS 03 9
Estymacja wyjść/stanu Przykład 4: zastosowania Rozszerzonego Filtru Kalmana 0 0 Filtr Kalmana 6000 00 00 80 80 5000 Błąd estymacji wysoko ości (ft) 60 40 0 0-0 -40 Błąd estymacji prędkoś ści (ft/s) 60 40 0 0-0 -40 (lb/ft ) Błąd estymacji wsp. β 4000 3000 000 000-60 -60-80 -80 0-00 0 5 0 5-00 0 5 0 5 0 5 0 5 czas (s) czas (s) czas (s) MiDwSS 03 93
Bibliografia Materiały wykładowe z przedmiotów (sem. I; studiów I stopnia) : Modelowanie i Identyfikacja Teoria Sterowania [] Tomasz P.Zieliński, Cyfrowe przetwarzanie sygnałów od teorii do zastosowań. WKŁ, 007. [] Rudolph. E. Kalman, "A New Approach to Linear Filtering and Prediction Problems", Trasactions of the ASME: Journal of Basic Engineering, vol 8, Series D, pp. 35-45, 960. [3] Peter S. Maybeck, Stochastic models, estimation and control, Academic Press, 979. [4] Arthur Gelb, Applied Optimal Estimation. The M.I.T Press, 974. MiDwSS 03 94
Zakończenie ;) Koniec części I Dziękuję za uwagę!!! MiDwSS 03 95