Algorytmy estymacji stanu (filtry)

Transkrypt

1 Algorytmy estymacji stanu (filtry) Na podstawie: AIMA ch15, Udacity (S. Thrun) Wojciech Jaśkowski Instytut Informatyki, Politechnika Poznańska 21 kwietnia 2014

2 Problem lokalizacji Obserwowalność? Determinizm?

3 Problem lokalizacji

4 Filtr Histogramowy

5 Filtr Histogramowy

6 Filtr Histogramowy

7 Filtr Histogramowy

8 Filtr Histogramowy

9 Pomiar

10 Pomiar

11 Pomiar [zadanie 1]

12 Pomiar wynik

13 Ruch agenta Jak wygląda stan wiedzy (ang. belief) po ruchu o 1 pozycję w prawo?[zadanie 2]

14 Ruch agenta

15 (Niedokładny) ruch agenta P(X i+2 X i ) = 0.8 P(X i+1 X i ) = 0.1 P(X i+3 X i ) = 0.1 Jak wygląda rozkład po wykonaniu ruchu o 2 pola w prawo? [zadanie3]

16 (Niedokładny) ruch agenta P(X i+2 X i ) = 0.8 P(X i+1 X i ) = 0.1 P(X i+3 X i ) = 0.1 Jak wygląda rozkład po wykonaniu ruchu o 2 pola w prawo? [zadanie4]

17 Ruch agenta Jak wygląda rozkład w? [zadanie 5]

18 Lokalizacja

19 Lokalizacja

20 Lokalizacja podsumowanie Stan wiedzy: rozkład prawdopodobieństwa Obserwacja: iloczyn (stan wiedzy, pomiar) + normalizacja Ruch: konwolucja (stan wiedzy, model ruchu)

21 Lokalizacja podstawy matematyczne (obserwacja) Zmienne losowe: X robot jest na danym polu (X = x i znaczy, że jest na polu i-tym) Z wynik pomiaru P(X Z) = P(Z X )P(X ) P(Z) P(X ) rozkład a priori P(Z X ) rozkład prawd. pomiaru P(Z) normalizacja P(Z) = i P(Z xi)p(xi) = α

22 Lokalizacja podstawy matematyczne (ruch) Konwolucja: P(x t i ) = j P(x t 1 j )P(x i x j ) P(X t 1 ) rozkład prawd. a priori (np. P(x t 1 i ) P(X X ) rozkład prawd. związanych z wiedzą na temat ruchu (np. P(x i x j ))

23 Porównanie estymacji stanu Metoda stanowość rozkład Filtr histogramowy dyskretny multimodalny Filtr Kalmana ciągły unimodalny

24 Funkcja gaussowska

25 Funkcja gaussowska μ= 0, μ= 0, μ= 0, μ= 2, 2 σ = 0.2, 2 σ = 1.0, 2 σ = 5.0, 2 σ = 0.5, φ μ,σ 2( x) x f (x) = 1 1 (x µ) 2 2πσ 2 e 2 σ 2

26 Pomiar i ruch Pomiar: (iloczyn/bayes czy konwolucja/prawd. całkowite?)[zadanie 6] Ruch: (iloczyn/bayes czy konwolucja/prawd. całkowite?)

27 Pomiar rozkład a priori oraz pomiar

28 Pomiar Gdzie jest średnia f. gausowskiej (stanu wiedzy) po pomiarze? [zadanie 7]

29 Pomiar Gdzie jest amplituda f. gausowskiej (stanu wiedzy) po pomiarze? [zadanie 8]

30 Pomiar

31 Pomiar wzory Aktualizacja stanu wiedzy po pomiarze (mnożenie f. gaussowskich): µ = r 2 µ + σ 2 ν r 2 + σ 2

32 Zadanie na pobudkę µ = 10 ν = 12 σ 2 = 4 r 2 = 4 [zadanie 9] µ = r 2 µ + σ 2 ν r 2 + σ 2 σ 2 = 1 1 r σ 2

33 Pomiar wzory P(X ) rozkład a priori P(Z X ) pomiar P(X Z) rozkład a posteriori

34 Pomiar pytanie na koniec Jak będzie wyglądał rozkład a posteriori?[zadanie 10]

35 Ruch wzory Aktualizacja stanu wiedzy po ruchu: µ = µ + ν σ 2 = σ 2 + r 2

36 Wielowymiarowy filtr Kalmana Potrafi przewidywać pozycję na podstawie wiedzy o prędkości (bez jej pomiarów)

37 Wielowymiarowa f. gaussowska µ =... Σ = µ 0. µ D f (x) = (2π) D 2 Σ 1 2 exp( 1 2 (x µ)t Σ 1 (x µ))

38 Wielowymiarowa f. gaussowska

39 Wielowymiarowy filtr Kalmana

40 Wielowymiarowy filtr Kalmana Gdzie byśmy byli, gdyby prędkość wynosiła 0? A jeśli 1? A jeśli 2?

41 Wielowymiarowy filtr Kalmana (predykcja)

42 Wielowymiarowy filtr Kalmana x - obserwowane

43 Projektowanie filtru Kalmana F funkcja przejścia H funkcja pomiaru ( co obserwujemy )

44 Projektowanie filtru Kalmana równania x estymata P macierz kowariancji (niepewność) F funkcja przejścia u wektor ruchu z pomiar H funkcja pomiaru R niepewnośc pomiaru 1 macierz jednostkowa Ruch: x = Fx + u P = FPF T Pomiar: y = z Hx S = HPH T + R K = PH T S 1 x = x + (Ky) P = (1 KH)P

45 Porównanie Metoda przestrzeń stanów rozkład efektywność Filtr histogramowy dyskretna multimodalny Filtr Kalmana ciągła unimodalny

46 Porównanie Metoda przestrzeń stanów rozkład efektywność Filtr histogramowy dyskretna multimodalny Filtr Kalmana ciągła unimodalny Filtr cząsteczkowy ciągła multimodalny

47 Porównanie Metoda przestrzeń stanów rozkład efektywność Filtr histogramowy dyskretna multimodalny wykładniczy Filtr Kalmana ciągła unimodalny O(D 2 ) Filtr cząsteczkowy ciągła multimodalny szybki

48 Zalety filtru cząsteczkowego Filtr cząsteczkowy: bardzo prosty w implementacji bardzo elastyczny Przykład działania: (autor: Szymon Kaliski)

49 Robot

50 Robot

51 Filtr cząsteczkowy Krok algorytmu 1 (inicjalizacja) Inicjalizuj N cząstek losowo

52 Filtr cząsteczkowy Krok algorytmu 2 (ruch robota) Wykonaj ruch zgodnie z (zamierzonym) ruchem robota. Do ruchu dodaj trochę szumu, aby algorytm nie zbiegł do punktu. Szum odgrywa rolę mutacji.

53 Filtr cząsteczkowy

54 Filtr cząsteczkowy

55 Filtr cząsteczkowy (wagi cząsteczek) w i = Π L j=1f µi,σ(dist(p i, L j ))

56 Filtr cząsteczkowy Krok algorytmu 3 (przydział wag) Przydziel wagi cząsteczkom (prawd. pomiaru). Dla każdej cząsteczki p i : gdzie: f funkcja gaussowska w i = Π L j=1f µi,σ(dist(p i, L j )) µ i pomiar dla punktu orientacyjnego i σ niepewność pomiaru (sensorów) dist(p i, L j ) odległość pomiędzy pozycją i-tej cząsteczki a j-tym punktem orientacyjnym

57 Selekcja (ponowne próbkowanie) Krok algorytmu 4 (próbkowanie) Próbkuj populację cząstek

58 Selekcja przykład N = 5 w = {w 1 = 0.6, w 2 = 1.2, w 3 = 2.4, w 4 = 0.6, w 5 = 1.2} Jakie jest prawd niewylosowania p 3? [zadanie 11] Jakie jest prawd niewylosowania p 1? [zadanie 12]

59 Selekcja ruletkowa

60 Selekcja (szybsza) mx = max(w) b = 0 idx = randint(0,n) for i in range(n): b += random(0, 2*mx) while w[idx] < b: b -= w[idx] idx = (idx + 1) % N resampled.append(idx)

61 Podsumowanie

62 Zadanie Czy orientacja cząstek ma znaczenie?[zadanie 13]

63 Wady i zalety Przykład: (autor: Sebastian Thrun) Zalety: Wady: Proste w implementacji Zwykle działają bardzo dobrze Niewymagające obliczeniowo Działają dobrze nawet, gdy rozkłady: niemonotoniczne skomplikowane (niegaussowskie) Przekleństwo wymiarowości = rozszerzenia (np. filtry Rao-Blackwellized) Zbyt mało cząstek = kłopoty Mało szumów = kłopoty

64