Filtr Kalmana. Zaawansowane Techniki Sterowania. Wydział Mechatroniki Politechniki Warszawskiej. Anna Sztyber

Transkrypt

1 Filtr Kalmana Zaawansowane Techniki Sterowania Wydział Mechatroniki Politechniki Warszawskiej Anna Sztyber ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 1 / 32

2 Plan wykładu 1 Sformułowanie problemu 2 Niestacjonarny Filtr Kalmana Jak to działa? Przykład - zbiornik z mieszaniem 3 Stacjonarny Filtr Kalmana Przykład - odwrócone wahadło na wózku 4 Rozszerzenia ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 2 / 32

3 Filtr Kalmana Dla układu: x(k + 1) = Ax(k) + Bu + w(k) y(k) = Cx(k) + v(k), gdzie: w(k) N(0, Q k ) v(k) N(0, R k ) x 0, v i w są niezależne Szukamy oszacowania ˆx minimalizującego: E ( (x ˆx)(x ˆx) ) T ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 3 / 32

4 Kowariancja Σ ij = cov(x i, x j ) = E ((x i µ i )(x j µ j )) = E(x i x j ) µ i µ j cov(ax) = A cov(x)a T ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 4 / 32

5 Filtr Kalmana Narzędzie do łączenia informacji przy występowaniu niepewności i szumów dysponujemy zaszumionymi danymi pomiarowymi model procesu pozwala na przewidywanie kolejnych stanów systemu oszacowanie stanu aktualnego sygnały sterujące Rudolf E. Kalman został w 2008 laureatem Charles Stark Draper Prize za filtr Kalmana Inne przykładowe wyróżnione rozwiązania: Internet, GPS, CCD, LCD, LED, c++ ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 5 / 32

6 Zastosowania [3] automatyka i robotyka diagnostyka procesów przemysłowych ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 6 / 32

7 Zastosowania [3] automatyka i robotyka diagnostyka procesów przemysłowych Misja Apollo prognozy pogody GPS F-111 amerykański samolot dalekiego zasięgu Minuteman amerykański międzykontynentalny pocisk balistyczny ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 6 / 32

8 Łatwe wprowadzenie [2, 1] Modelujemy niepewność jako rozkład Gaussa: ˆx(k) - najlepsze oszacowanie, średnia P k - macierz kowariancji ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 7 / 32

9 Łatwe wprowadzenie [2, 1] Modelujemy niepewność jako rozkład Gaussa: ˆx(k) - najlepsze oszacowanie, średnia P k - macierz kowariancji Predykcja stanu kolejnego: ˆx(k) = Aˆx(k 1) + Bu(k 1) P k = AP k 1 A T x 2 (k) x 2 (k) x 1 (k) x 1 (k) ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 7 / 32

10 Wpływ niemierzalnych zakłóceń na stan (w(k) N(0, Q k )): x(k) = A k x(k 1) + B k u(k 1) + w(k) P k = AP k 1 A T + Q k Oczekiwane wyniki pomiarów: ŷ(k) = Cˆx(k) cov(ŷ(k)) = CP k C T ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 8 / 32

11 Łączenie informacji z modelu i pomiarów Wynik pomiaru (v(k) N(0, R k )): y(k) = Cx(k) + v(k) x 2 (k) x 1 (k) ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 9 / 32

12 Łączenie informacji z modelu i pomiarów Wynik pomiaru (v(k) N(0, R k )): y(k) = Cx(k) + v(k) x 2 (k) x 1 (k) W celu uzyskania wspólnego rozkładu prawdopodobieństwa mnożymy uzyskane rozkłady ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 9 / 32

13 Iloczyn rozkładów Gaussa 1-D Iloczyn funkcji Gaussa jest funkcją Gaussa ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 10 / 32

14 Iloczyn rozkładów Gaussa 1-D Iloczyn funkcji Gaussa jest funkcją Gaussa ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 11 / 32

15 Iloczyn rozkładów Gaussa 1-D Otrzymujemy rozkład Gaussa: µ = µ 1σ 2 2 +µ 2σ 2 1 σ 2 1 +σ2 2 σ 2 = σ2 1 σ2 2 σ 2 1 +σ2 2 Przekształcając: µ = µ 1 + σ2 1(µ 2 µ 1 ) σ σ 2 2 σ 2 = σ 2 1 σ4 1 σ σ 2 2 ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 12 / 32

16 Wzmocnienie Kalmana Wprowadzamy k: Otrzymujemy: k = σ2 1 σ σ 2 2 µ = µ 1 + k(µ 2 µ 1 ) W zapisie macierzowym: σ 2 = σ 2 1 kσ 2 1 K = Σ 1 (Σ 1 + Σ 2 ) 1 µ = µ 1 + K(µ 2 µ 1 ) Σ = Σ 1 KΣ 1 ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 13 / 32

17 Korekta oczekiwany wynik pomiaru: µ 1 = Cˆx(k), zarejestrowany wynik pomiaru: µ 2 = y(k), Σ 1 = CP k C T Σ 2 = R Cˆx (k) = Cˆx + K(y(k) Cx(k)) CP k C T = CP k C T KCP k C T K = CP k C T (CP k C T + R k ) 1 Pozbywamy się C (to nie jest formalny krok): ˆx (k) = ˆx + K(y(k) Cx(k)) P k = P k KCP k K = P k C T (CP k C T + R) 1 ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 14 / 32

18 Niestacjonarny Filtr Kalmana - podsumowanie Predykcja stanu ˆx(k) = Aˆx(k 1) + Bu(k 1) P k = AP k 1 A T + Q k Korekta ˆx (k) = ˆx + K(y(k) Cx(k)) P k = P k KCP k K = P k C T (CP k C T + R k ) 1 ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 15 / 32

19 Dla układów niestacjonarnych Predykcja stanu ˆx(k) = A kˆx(k 1) + B k u(k 1) P k = A k P k 1 A k T + Q k Korekta ˆx (k) = ˆx + K(y(k) C k x(k)) P k = P k KC k P k K = P k C k T (C k P k C T + R k ) 1 ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 16 / 32

20 Niestacjonarny Filtr Kalmana for k =1: length ( tspan ) u = -K*z; [,y] = ode15s (@(t,y) tank_derivatives_u_hold (...); x = y(end,:) ' - [h0;t0 ]; ym = x + 1* randn (2,1) ; % predict z = AD*z + BD*u; P = AD*P*AD ' + Qf; % update Kf = P*C '/( C*P*C' + Rf); z = z + Kf *( ym - z); P = P - Kf*C*P; end ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 17 / 32

21 Rf = eye (2) ; Qf = eye (2) ; d= cumsum (1* randn (Tend,1) ); ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 18 / 32

22 Rf = eye (2) ; Qf = 0.01* eye (2) ; d= cumsum (1* randn (Tend,1) ); ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 19 / 32

23 Rf = 0.01* eye (2) ; Qf = eye (2) ; d= cumsum (1* randn (Tend,1) ); ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 20 / 32

24 Rf = eye (2) ; Qf = 0.01* eye (2) ; d =10* ones (Tend,1) ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 21 / 32

25 Rf = eye (2) ; Qf = Bd '* eye (2) *Bd; d = randn (Tend,1) ; ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 22 / 32

26 Model z uwzględnieniem macierzy zakłóceń Dla układu: Stosujemy: x(k + 1) = A k x(k) + B k u + Γw(k) y(k) = C k x(k) + v(k), Q Γ T QΓ ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 23 / 32

27 Stacjonarny filtr Kalmana P k = AP k 1 A T + Q P k = P k KCP k K = P k C T (CP k C T + R) 1 Zmiany P k nie zależą od x ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 24 / 32

28 Stacjonarny filtr Kalmana P k = AP k 1 A T + Q P k = P k KCP k K = P k C T (CP k C T + R) 1 Zmiany P k nie zależą od x P k = P k 1 P = Q + APA T APC T (CP k C T + R) 1 CP k A T ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 24 / 32

29 Analogie ˆx (k) = ˆx + K(y(k) Cx(k)) Otrzymujemy obserwator stanu P = Q + APA T APC T (CP k C T + R) 1 CP k A T ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 25 / 32

30 Analogie ˆx (k) = ˆx + K(y(k) Cx(k)) Otrzymujemy obserwator stanu Dla LQR: P = Q + APA T APC T (CP k C T + R) 1 CP k A T P = Q + A T (P P B(R + B T P B) 1 B T P )A ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 25 / 32

31 Wahadło na wózku - Filtr Kalmana %% Kalman Filter %Qf = 10ˆ( -4) * eye (4) ; Qf = [10ˆ( -6) 0 0 0; 0 10ˆ( -6) 0 0; ˆ( -4) 0; ˆ( -2) ]; Rf = 0.001* eye (2) ; [P,Lf,Gf] = dare (Ad ',C',Qf,Rf); eigs (Ad - Gf '*C) ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 26 / 32

32 Filtr Kalmana - obliczenia iteracyjne function K = kalman_filter (Ad,C,Qf,Rf) % initial values P = eye (4) ; K = P*C '/( C*P*C' + Rf); for i =1:100 P = Ad*P*Ad ' + Qf; K = P*C '/( C*P*C' + Rf); P = P - K*C*P; end ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 27 / 32

33 Regulator LQG ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 28 / 32

34 Regulator LQG ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 29 / 32

35 Jak dobrać macierze Q i R? R jest kowariancją szumów pomiarowych oszacowanie na podstawie danych pomiarowych w wersji niestacjonarnej możliwe oszacowanie on-line dane producentów czujników jest miarą naszej wiary w jakość pomiarów Q - kowariancja zakłóceń o zerowej średniej i rozkładzie Gaussa trudna interpretacja założenie rzadko spełnione w praktyce jest miarą naszej wiary w jakość modelu należy uważać z wstawianiem wartości 0 heurystyka: obserwator powinien być szybszy niż regulator (o rząd wielkości) ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 30 / 32

36 Warianty Filtr Kalmana układy liniowe możliwa wydajna implementacja dla układów wbudowanych Filtr Kalmana-Bucy ego układy z czasem ciągłym Rozszerzony Filtr Kalmana (EKF) Jakobiany zamiast macierzy przekształceń założenie gładkości funkcji ˆx stanowi centrum niepewności oszacowania Unscented Kalman Filter (Sigma-point filter) Filtr cząsteczkowy (Particle Filter) Dla zainteresowanych: [4] ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 31 / 32

37 Literatura Tim Babb. How a kalman filter works, in pictures [Online; accessed ]. R. Faragher. Understanding the basis of the kalman filter via a simple and intuitive derivation [lecture notes]. IEEE Signal Processing Magazine, 29(5): , Sept M. S. Grewal and A. P. Andrews. Applications of kalman filtering in aerospace 1960 to the present [historical perspectives]. IEEE Control Systems, 30(3):69 78, June Tucker McClure. How kalman filters work [Online; accessed ]. ZTS (IAiR PW) Filtr Kalmana Anna Sztyber 32 / 32