Fuzja sygnałów i filtry bayesowskie

Fuzja sygnałów i filtry bayesowskie Roboty Manipulacyjne i Mobilne dr inż. Janusz Jakubiak Katedra Cybernetyki i Robotyki Wydział Elektroniki, Politechnika Wrocławska Wrocław, 10.03.2015

Dlaczego potrzebna jest fuzja sygnałów? Rodzaje informacji: rozłączne, komplementarne, substytucyjne. J. Jakubiak (KCIR) Fuzja sygnałów Wrocław, 10.03.2015 2 / 17

Dlaczego potrzebna jest fuzja sygnałów? Rodzaje informacji: rozłączne, komplementarne, substytucyjne. Problem: czujnik 1 z x _ 1 =a x z 2 czujnik 2 x _ =b x _ =??? J. Jakubiak (KCIR) Fuzja sygnałów Wrocław, 10.03.2015 2 / 17

Dlaczego potrzebna jest fuzja sygnałów? Fuzja sygnałów Przetworzenie sygnałów pochodzących z różnych czujników w taki sposób, że wynikowa informacja jest lepsza od informacji możliwych do uzyskania z każdego z czujników oddzielnie. J. Jakubiak (KCIR) Fuzja sygnałów Wrocław, 10.03.2015 3 / 17

Dlaczego potrzebna jest fuzja sygnałów? Fuzja sygnałów Przetworzenie sygnałów pochodzących z różnych czujników w taki sposób, że wynikowa informacja jest lepsza od informacji możliwych do uzyskania z każdego z czujników oddzielnie. Rozróżnia się fuzje bezpośrednią, nazywaną także fuzją sensorów, w której przetwarzane są wyłącznie dane z czujników oraz pośrednią (fuzja informacji), w której wykorzystywane są także informacje z innych źródeł (np. wiedza a priori, informacje od użytkownika). J. Jakubiak (KCIR) Fuzja sygnałów Wrocław, 10.03.2015 3 / 17

Notacja x stan rzeczywisty układu (R n ), x estymata stanu, z obserwacja (R m ), J. Jakubiak (KCIR) Fuzja sygnałów Wrocław, 10.03.2015 4 / 17

Notacja x stan rzeczywisty układu (R n ), x estymata stanu, z obserwacja (R m ), p(a B) prawdopodobieństwo zdarzenia A pod warunkiem B, N (µ, Σ) wielowymiarowy rozkład normalny ze średnią µ i macierzą kowariancji Σ (parametrami: momenty) N (µ, Σ) p(x ) = { 1 (2π) n det Σ exp 1 } 2 (x µ)t Σ 1 (x µ) J. Jakubiak (KCIR) Fuzja sygnałów Wrocław, 10.03.2015 4 / 17

Notacja x stan rzeczywisty układu (R n ), x estymata stanu, z obserwacja (R m ), p(a B) prawdopodobieństwo zdarzenia A pod warunkiem B, N (µ, Σ) wielowymiarowy rozkład normalny ze średnią µ i macierzą kowariancji Σ (parametrami: momenty) N (µ, Σ) p(x ) = { 1 (2π) n det Σ exp 1 } 2 (x µ)t Σ 1 (x µ) Twierdzenie Bayesa p(x Z) = p(z X )p(x ) p(z) = ηp(z X )p(x ), gdzie η jest współczynnikiem normalizującym. J. Jakubiak (KCIR) Fuzja sygnałów Wrocław, 10.03.2015 4 / 17

Notacja (2) Zmiany momentów dystrybucji gaussowskich przy przekształceniach liniowym (Y = AX + B) p(x ) = N (µ, Σ) p(y ) = N (Aµ + B, AΣA T ) J. Jakubiak (KCIR) Fuzja sygnałów Wrocław, 10.03.2015 5 / 17

Notacja (2) Zmiany momentów dystrybucji gaussowskich przy przekształceniach liniowym (Y = AX + B) p(x ) = N (µ, Σ) p(y ) = N (Aµ + B, AΣA T ) sumie (Y = X 1 + X 2 ) p(x 1 ) = N (µ 1, Σ 1 ), p(x 2 ) = N (µ 2, Σ 2 ), p(y ) = N (µ 1 + µ 2, Σ 1 + Σ 2 ) J. Jakubiak (KCIR) Fuzja sygnałów Wrocław, 10.03.2015 5 / 17

Notacja (2) Zmiany momentów dystrybucji gaussowskich przy przekształceniach liniowym (Y = AX + B) p(x ) = N (µ, Σ) p(y ) = N (Aµ + B, AΣA T ) sumie (Y = X 1 + X 2 ) p(x 1 ) = N (µ 1, Σ 1 ), p(x 2 ) = N (µ 2, Σ 2 ), iloczyn p(y ) = N (µ 1 + µ 2, Σ 1 + Σ 2 ) p(x 1 )p(x 2 ) = N (µ 1, Σ 1 )N (µ 2, Σ 2 ) N (µ Y, Σ Y ), Σ Y = ( ) Σ 1 1 + Σ 1 1 2 µ Y = Σ Y Σ 1 1 µ 1 + Σ Y Σ 1 2 µ 2 J. Jakubiak (KCIR) Fuzja sygnałów Wrocław, 10.03.2015 5 / 17

Niezależne pomiary czujnik 1 z 1 x x _ z 2 czujnik 2 J. Jakubiak (KCIR) Fuzja sygnałów Wrocław, 10.03.2015 6 / 17

Niezależne pomiary p(x z 1 z 2 ) = p(x z 1 )p(x z 2 ) = ηp(z 1 x)p(z 2 x) czujnik 1 z 1 x x _ z 2 czujnik 2 J. Jakubiak (KCIR) Fuzja sygnałów Wrocław, 10.03.2015 6 / 17

Niezależne pomiary przykład Załóżmy, że mamy dwa czujniki A i B, dające niezależne pomiary. Czujnik A ma dokładność ±0.8, czunik B: ±0.5. Rozkłady prawdopodobieństwa są jednorodne w podanym zakresie dokładności. J. Jakubiak (KCIR) Fuzja sygnałów Wrocław, 10.03.2015 7 / 17

Niezależne pomiary przykład Załóżmy, że mamy dwa czujniki A i B, dające niezależne pomiary. Czujnik A ma dokładność ±0.8, czunik B: ±0.5. Rozkłady prawdopodobieństwa są jednorodne w podanym zakresie dokładności. Przy pomiarze czujniki zwróciły A:2.0, a B:2.9. 1 Jak wyglądają rozkłady prawdopodobieństwa pomiarów z obu czujników? J. Jakubiak (KCIR) Fuzja sygnałów Wrocław, 10.03.2015 7 / 17

Niezależne pomiary przykład Załóżmy, że mamy dwa czujniki A i B, dające niezależne pomiary. Czujnik A ma dokładność ±0.8, czunik B: ±0.5. Rozkłady prawdopodobieństwa są jednorodne w podanym zakresie dokładności. Przy pomiarze czujniki zwróciły A:2.0, a B:2.9. 1 Jak wyglądają rozkłady prawdopodobieństwa pomiarów z obu czujników? 2 Jak wygląda rozkład gęstości pomiaru łącznego, jaka jest jego dokładność? J. Jakubiak (KCIR) Fuzja sygnałów Wrocław, 10.03.2015 7 / 17

Niezależne pomiary przykład Załóżmy, że mamy dwa czujniki A i B, dające niezależne pomiary. Czujnik A ma dokładność ±0.8, czunik B: ±0.5. Rozkłady prawdopodobieństwa są jednorodne w podanym zakresie dokładności. Przy pomiarze czujniki zwróciły A:2.0, a B:2.9. 1 Jak wyglądają rozkłady prawdopodobieństwa pomiarów z obu czujników? 2 Jak wygląda rozkład gęstości pomiaru łącznego, jaka jest jego dokładność? 3 (*) Jak dokładność zależy od różnicy między odczytami z sensorów A i B? J. Jakubiak (KCIR) Fuzja sygnałów Wrocław, 10.03.2015 7 / 17

Niezależne pomiary c.d. p(x z 1 z 2 ) = p(x z 1 )p(x z 2 ) = ηp(z 1 x)p(z 2 x) J. Jakubiak (KCIR) Fuzja sygnałów Wrocław, 10.03.2015 8 / 17

Procesy Markowa Proces ma własność Markowa, jeśli warunkowe rozkłady przyszłych stanów procesu zależą tylko od stanu bieżącego, a nie od stanów przeszłych J. Jakubiak (KCIR) Fuzja sygnałów Wrocław, 10.03.2015 9 / 17

Procesy Markowa Proces ma własność Markowa, jeśli warunkowe rozkłady przyszłych stanów procesu zależą tylko od stanu bieżącego, a nie od stanów przeszłych Własność ta w robotyce mobilnej często nie jest spełniona np. z powodu następujących czynników: model nie uwzględnia dynamiki otoczenia (np. poruszający się ludzie i ich wpływ na pomiary czujników), niedokładności modeli probabilistycznych p(z t x t ), p(x t x t 1, u t ), błędy przybliżonych reprezentacji (np. rozkładu prawdopodobieństwa). Niektóre z nich mogą być uwzględnione w modelu, jednak kosztem jego znacznej komplikacji. J. Jakubiak (KCIR) Fuzja sygnałów Wrocław, 10.03.2015 9 / 17

Filtry gaussowskie Wszystkie wykorzystują ideę wielowymiarowej dystrybucji Gaussa, z parametryzacją za pomocą momentów, lub kanoniczną. Zakładają zatem unimodalność, z czego wynika: + dobrze sprawdzają się w zadaniach ze zmienną losową w pobliżu stanu rzeczywistego - działają gorzej lub wcale w zadaniach z wieloma równoprawnymi hipotezami J. Jakubiak (KCIR) Fuzja sygnałów Wrocław, 10.03.2015 10 / 17

Filtr Kalmana (KF) (Swerling 1958, Kalman1960) Stosowany dla układów liniowych, ciągłych. W chwili t szacujemy rozkład prawdopodobieństwa X przez N (µ t, Σ t ). Zakładamy, że spełnione są założenia Markowa oraz: 1 prawdopodobieństwo dla funkcji przejścia jest f.liniową x t = A t x t 1 + B t u t + ɛ t, ɛ t N (0, R t ), 2 prawdopodobieństwo pomiarów f. liniowa 3 prawdopodobieństwo początkowe z t = C t x t + δ t, δ t N (0, Q t ), p(x 0 ) = N (µ 0, Σ 0 ). Wtedy także prawdopodobieństwo a posteriori jest gaussowskie. J. Jakubiak (KCIR) Fuzja sygnałów Wrocław, 10.03.2015 11 / 17

Filtr Kalmana algorytm µ t = A t µ t 1 + B t u t Σ t = A t Σ t 1 A T t + R t K t = Σ t C T t (C t Σt C T t + Q t ) 1 µ t = µ t + K t (z t C t µ t ) Σ t = (I K t C t ) Σ t Obliczenia przewidywanego rozkładu nowego stanu po zastosowaniu u t. Wyznaczenie tzw. wzmocnienia Kalmana. Wyznaczenie wypadkowego rozkładu nowego stanu. (jeśli wzmocnienie Kalmana wynosi 0 nowy rozkład wyliczany jest wyłącznie na podstawie wiedzy a priori, jeśli zaś K t = Ct 1 wyłącznie na podstawie pomiarów) J. Jakubiak (KCIR) Fuzja sygnałów Wrocław, 10.03.2015 12 / 17

KF przykład Rozpoczynamy od rozkładu w poprzedniej iteracji x t 1. J. Jakubiak (KCIR) Fuzja sygnałów Wrocław, 10.03.2015 13 / 17

KF przykład Wyznaczamy przewidywany rozkład x t po przemieszczeniu. J. Jakubiak (KCIR) Fuzja sygnałów Wrocław, 10.03.2015 13 / 17

KF przykład Jednocześnie dokonujemy pomiaru z t. J. Jakubiak (KCIR) Fuzja sygnałów Wrocław, 10.03.2015 13 / 17

KF przykład Wyliczamy rozkład bieżący z t. J. Jakubiak (KCIR) Fuzja sygnałów Wrocław, 10.03.2015 13 / 17

Rozszerzony filtr Kalmana (EKF) EKF Stosowany dla układów nieliniowych postaci x t = g(u t, x t 1 ) + ɛ t z t = h(x t ) + δ t J. Jakubiak (KCIR) Fuzja sygnałów Wrocław, 10.03.2015 14 / 17

EKF algorytm µ t = g(u t, µ t 1 ) Σ t = G t Σ t 1 Gt T + R t K t = Σ t H T t (H t Σt H T t + Q t ) 1 µ t = µ t + K t (z t h t ( µ t )) Σ t = (I K t H t ) Σ t Obliczane jakobiany: G t = g (u t, µ t 1 ), g (u t, x t 1 ) = g(u t, t 1 ) x t 1 H t = h (µ t ) h (x t ) = h(x t) x t J. Jakubiak (KCIR) Fuzja sygnałów Wrocław, 10.03.2015 15 / 17

Podsumowanie EKF Złożoność obliczeniowa jednej iteracji algorytmu O(k 2.4 + n 2 ). Założenie o unimodalności (jednak w wielu praktycznych przypadkach, mimo niespełnienia tego założenia, okazuje się skuteczny). Jakość estymacji zależy od nieliniowości funkcji w pobliżu punktu przybliżenia oraz od niepewności początkowej. J. Jakubiak (KCIR) Fuzja sygnałów Wrocław, 10.03.2015 16 / 17

Modyfikacje MHEKF (multi-hypothesis), ADF (assumed density) dopasowanie momentów a posteriori, UKF (unscented) analiza stochastyczna z użyciem tzw. punktów σ, IF/EIF ((extended) information filter) dualny do f. Kalmana, wykorzystujący reprezentację kanoniczną rozkładu, czyli macierz informacji Ω = Σ 1 oraz wektor informacji ξ = Σ 1 µ. (E)IF (E)KF + reprezentacja globalnej niepewności + mniejsza złożoność + lepsza stabilność numeryczna obliczeniowa + w środowiskach wielorobotowych J. Jakubiak (KCIR) Fuzja sygnałów Wrocław, 10.03.2015 17 / 17