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
 
I corsi di Scienze Matematiche Fisiche e Naturali
 
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