Vai alla Home Page About me Courseware Federica Living Library Federica Federica Podstudio Virtual Campus 3D Le Miniguide all'orientamento Gli eBook di Federica La Corte in Rete
 
 
Il Corso Le lezioni del Corso La Cattedra
 
Materiali di approfondimento Risorse Web Il Podcast di questa lezione

Alberto Finzi » 8.Robotica Probabilistica - Filtri Gaussiani


Filtri Bayesiani

Predizione

\overline{bel}(x_t)=\int p(x_t | u_t, x_{t-1})bel(x_{t-1})dx_{t-1}

Correzione

bel(x_t)=\eta p(z_t |x_t)\overline {bel}(x_t)

Gaussiane


Propertà delle Gaussiane

\left\begin{array}{ll} X\sim N(\mu, \sigma^2) \\ Y=aX+b\end{array}\right\}\Rightarrow Y\sim N(a\mu+b,a^2\sigma^2)

\left\begin{array}{ll} X_1 \sim N(\mu_1,\sigma_1^2)\\ X_2 \sim N(\mu_2,\sigma_2^2)\end{array}\right\}\Rightarrow p(X_1)\cdot p(X_2)\sim N \left( \frac{\sigma_2^2}{\sigma_1^2+\sigma_2^2}\mu_1+\frac {\sigma_1^2}{\sigma_1^2+\sigma_2^2}\mu_2, \frac 1 {\sigma_1^{-2}+\sigma_2^{-2}}\right)

Multivariate

Si rimane nel “mondo gaussiano” con trasformazioni lineari.

\left\begin{array}{ll}X\sim N(\mu,\Sigma)\\ Y=AX+B\end{array}\right\}\Rightarrow Y\sim N(A\mu+B, A\Sigma A^T)

\left\begin{array}{ll} X_1\sim N(\mu_1,\Sigma_1) \\ X_2 \sim N(\mu_2,\Sigma_2)\end{array}\right\} p(X_1)\cdot p(X_2)\sim N \left(\frac{\Sigma_2}{\Sigma_1+\Sigma_2}\mu_1+\frac{\Sigma_1}{\Sigma_1+\Sigma_2}\mu_2,\frac 1{\Sigma_1{-1}+\Sigma_2^{-1}}\right)

Filtro di Kalman Discreto

Stima lo stato x di un processo a tempo discreto governato da una equazione alle differenze stocastica lineare

x_t=A_tx_{t-1}+B_tu_t+\varepsilon_t

con una misura

z_t=C_tx_t+\delta_t

Componenti di un Filtro di Kalman

At → Matrice (nxn) che descrive l’evoluzione dello stato da t a t-1 senza controllo o rumore.
Bt → Matrice (nxl) che descrive come il controllo ut cambia lo stato da t a t-1.
Ct → Matrice (kxn) che descrive come mappare lo stato xt in un’osservazione zt.
εt e δt  → Variabili random che rappresentano il rumore di processo e misura. Assunte independenti e con distribuzione normale con covarianza Rt e Qt rispettivamente.

Filtro di Kalman in 1D


Aggiornamento Filtro di Kalman 1D

bel(x_t)=\left\{\begin{array}{ll}\mu_1=\bar \mu_t+K_t(z_t-\bar\mu_t)\\ \sigma_t^2=(1-K_T)\bar\sigma_t^2\end{array}\right \;\;\;\text{with}\;\;\;K_t=\frac{\bar\sigma_t^2}{\sigma_t^2+\bar\sigma_{obs,t}^2}

bel (x_t)=\left\{\begin{array}{ll}\mu_t=\bar\mu_t+K_t(z_t-C_t\bar\mu_t)\\ \Sigma_t=(I-K_tC_t)\bar\Sigma_t\end{array}\right\;\;\;\;\;\;\;\text{with}\;K_t=\bar\Sigma_tC_t^T(C_t\bar\Sigma_tC_t^T+Q_t)^{-1}

Update.

Update.


Aggiornamento Filtro di Kalman 1D

Spostamento

\overline{bel}(x_t)=\left\{\begin{array}{ll}\bar\mu_t=a_t\mu_{t-1}+b_tu_t\\ \\ \bar\sigma_t^2=a_t^2\sigma_t^2+\sigma^2_{act,t}\end{array}\right


Aggiornamento Filtro Kalman


Gaussiana Lineare: Initialization

Belief iniziale con distribuzione normale:

bel(x_0)=N(x_0; \mu_0, \Sigma_0)

Gaussiana Lineare: Dinamica

La dinamica è funzione lineare dello stato e del controllo più rumore additivo:

x_t=A_tx_{t-1}+B_tu_t+\varepsilon_t

p(x_t | u_t, x_{t-1})=N(x_t; A_tx_{t-1}+B_tu_t, R_t)

\overline{bel}(x_t)=\int p(x_t | u_t, x_{t-1})\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;bel(x_{t-1}dx_{t-1})

\Downarrow\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\Downarrow

\sim N(x_t; A_tx_{t-1}+B_tu_t, R_t)\;\;\;\sim N(x_{t-1}; \mu_{t-1}, \Sigma_{t-1})

Gaussiana Lineare: Dinamica


Gaussiana Lineare: Osservazioni

Le osservazioni sono funzioni lineari dello stato più rumore additivo:

z_t=C_tx_t+\delta_t

p(z_t | x_t)=N(z_t; C_tx_t, Q)

bel(x_t)=\;\;\;\;\eta\;\;\;\; p(z_t | x_t)\;\;\;\;\;\;\;\;\;\;\;\;\;\;\; \overline {bel}(x_t)

\Downarrow\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\Downarrow

\sim N(z_t;C_tx_t,Q_t)\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\sim N(x_T;\bar\mu_t,\bar\Sigma_t)

Sistemi a Gaussiana Lineare: Osservazioni


Kalman Filter Algorithm

  1. Algoritmo Kalman_filter (\mu_{t-1}, \Sigma_{t-1}, u_t, z_t)
  2. Predizione:
  3. \bar\mu_t=A_t\mu_{t-1}+B_tu_t
  4. \bar\Sigma_t=A_t\Sigma_{t-1}A_t^T+R_t
  5. Correzione:
  6. K_t=\bar\Sigma_tC_t^T(C_t\Sigma_tC_t^T+Q_t^T)^{-1}
  7. \mu_t=\bar\mu_t+K_t(z_t-C_t\bar\mu_t)
  8. \Sigma_t=(I-K_tC_t)\bar\Sigma_t
  9. Risultato \mu_t,\Sigma_t

Il ciclo Prediction-Correction


The Prediction-Correction-Cycle


Il ciclo Prediction-Correction


Sommario: Filtro di Kalman

Efficiente: Polinomiale nella dim delle misure k e nella dim dello stato n:

O(k2.376 + n2)

Ottimale per sistemi lineari Gaussiani!

Molti sistemi robotici sono nonlineari!

Sistemi Dinamici Nonlineari

Molti problemi robotici realistici richiedono funzioni nonlineari

x_t=g(u_t,x_{t-1})

z_t=h(x_t)

Assunzione lineare rivisitata


Funzione Non-lineare


Linearizazione EKF


Linearizazione EKF (segue)


Linearizazione EKF (segue)


Linearizazione EKF (segue)


Algoritmo EKF

  1. Extended_Kalman_filter (\mu_{t-1},\Sigma_{t-1},u_t z_t):
  2. Predizione:
  3. \bar \mu_t=g(u_t,\mu_{t-1})\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\longleftarrow \bar\mu_t=A_t\mu_{t-1}+B_tu_t
  4. \bar\Sigma_t=G_t\Sigma_{t-1}G_t^T+R_t\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\longleftarrow \bar\sigma_t=A_t\Sigma_{t-1}A_t^T+R_t
  5. Correzione:
  6. K_t=\bar\Sigma_tH_t^T(H_t\bar\Sigma_tH_t^T+Q_t)-1\;\;\;\;\;\;\longleftarrow K_t=\bar\Sigma_tC_t^T(C_t\bar\Sigma_tC_t^T+Q_t)^{-1}
  7. \mu_t=\bar\mu_t+K_t(z_t-h(\bar\mu_t))\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\longleftarrow \mu_t=\bar\mu_t+K_t(z_t-C_t\bar\mu_t)
  8. \Sigma_t=(I-K_tH_t)\bar\Sigma_t\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\; \longleftarrow \Sigma_t=(I-K_tC_t)\bar\Sigma_t
  9. Risultato \mu_t,\Sigma_t\;\;\;H_t=\frac{\partial h(\bar\mu_t)}{\partial x_t}\;\;\;\;\;G_t=\frac{\partial g(u_t,\mu_{t-1})}{\partial x_{t-1}}
  • Contenuti protetti da Creative Commons
  • Feed RSS
  • Condividi su FriendFeed
  • Condividi su Facebook
  • Segnala su Twitter
  • Condividi su LinkedIn
Progetto "Campus Virtuale" dell'Università degli Studi di Napoli Federico II, realizzato con il cofinanziamento dell'Unione europea. Asse V - Società dell'informazione - Obiettivo Operativo 5.1 e-Government ed e-Inclusion