PSC: Progettazione di sistemi di controllo  I Sem. 2010

Lezione 23 — 07 Dicembre

Docente: Luca Schenato  Stesore: Alberton R., Ausserer M., Barazzuol A.

23.1 Unscented Kalman filter

Si continua ora con l’unscented Kalman filter (UKF) visto la lezione scorsa. Il procedimento dell’UKF consta di tre passaggi. Nel primo la variabile casuale x viene approssimata da 2N + 1 punti v_{i} pesati da dei pesi w_{i} scelti in modo tale che media e varianza coicidano con quelle originali di x. Nel secondo passo si mappano questi punti attraverso la funzione non lineare g(x). Infine i punti mappati vengono approssimati con un opportuna gaussiana. Si ha:

p_{x}(·) ∼ N (x̄, P_{x}) y = g(x)

e si approssima la p_{y} con una gaussiana ottenendo:

p_{y}(·) ≃ N (ȳ, P_{y})

Figura 23.1. Procedimento dell’Unscented Kalman Filter

1. Da p_{x}(·) ∼ N (x̄, P_{x}) si generano 2N + 1 punti (dove x ∈ ℝ^{N}) nel modo seguente:

• v_{0} = x̄

• v_{1} = x̄ + ασ_{1}

• v_{2} = x̄ − ασ_{1} ⋮

PSC  Lezione 23 — 07 Dicembre  I Sem. 2010

23-1 • v_{2N−1} = x̄ + ασ_{N}

• v_{2N} = x̄ + ασ_{N}

dove α è un fattore di scala che viene imposto in modo che la media campionaria degli v_{i} risulta pari a x̄ e la varianza campionaria degli v_{i} pari a P_{x}. Il calcolo di α si comincia fattorizzando la varianza di x P_{x} = UDU^{T}, con U = [u_{1}, …, u_{N}] una matrice di vettori u_{i} ortonormali e D la matrice diagonale

D = [\begin{matrix}λ_{1} \\ & . & . & . \\ & & & & λ_{N} \end{matrix} ] ≥ 0

Si puó scrivere la P_{x} nel modo seguente:

P_{x} = UDU^{T}

= [u_{1} … u_{N}] [\begin{matrix}√ & \overline{λ_{1}} \\ & . & . & . \\ √ & \overline{λ_{N}} \end{matrix} ] [\begin{matrix}√ & \overline{λ_{1}} \\ & . & . & . \\ √ & \overline{λ_{N}} \end{matrix} ] [u_{1}^{T} ; ⋮ ; u_{N}^{T} ]

= [\sqrt{λ_{1}}u_{1} … \sqrt{λ_{N}}u_{N}] [ \sqrt{λ_{1}}u_{1}^{T} ; ⋮ ; \sqrt{λ_{N}}u_{N}^{T} ]

= λ_{1}u_{1}u_{1}^{T} + … + λ_{N}u_{N}u_{N}^{T}.

Chiamando σ_{i} = \sqrt{λ_{i}}u_{i} (vedi vettore rosso in Figura 23.1) si ha che P_{x} = ∑_{i=1}^{N} σ_{i}σ_{i}^{T}. La densitá di probabilitá di x puó essere approssimato nel modo seguente:

p_{x}(·) ≃ ∑_{i=0}^{2N} w_{i}δ(x − v_{i}) = p̂_{x}

con i pesi

w_{1} = w_{2} = … = w_{2N} = \frac{1}{2(N + K)}

w_{0} = 1 − ∑_{i=1}^{2N} w_{i} = 1 − \frac{N}{N + K} = \frac{K}{N + K} .

Vengono ora calcolate la media e la varianza di x relative alla densitá di probablitá approssimata:

23-2

𝔼p̂_{x}[x] = ∫_{x} ∑_{i=0}^{2N} xw_{i}δ(x − v_{i})dx = ∑_{i=0}^{2N} w_{i}v_{i} = x̄ ∑_{i=0}^{2N} w_{i} = x̄

V arp̂_{x}(x) = 𝔼p̂_{x}[(x − x̄)(x − x̄)^{T}] = ∫_{x} ∑_{i=0}^{2N}(x − x̄)(x − x̄)^{T} w_{i}δ(x − v_{i})dx,

= ∫_{ξ} ∑_{i=0}^{2N} ξξ^{T} w_{i}δ(ξ + x̄ − v_{i})dξ = ∑_{i=1}^{N} \frac{1}{2(N + K)}2α^{2}σ_{i}σ_{i}^{T}

= \frac{α^{2}}{N + K} ∑_{i=1}^{N} σ_{i}σ_{i}^{T} = \frac{α^{2}}{N + K} P_{x}

avendo effettuato il cambio di variabili ξ = x − x̄. Poiché V arp̂_{x}(x) = P_{x} si determina α = \sqrt{N + K}.

2. Al secondo passo si devono mappare le v_{i} usando la funzione non lineare g(·) ottenendo così y_{i} = g(v_{i}) con i = 0, …, 2N. La densitá di probabilitá approssimata di y risulta essere

p_{y} ≃ ∑_{i=0}^{2N} w_{i}δ(y − y_{i}) = p̂_{y}

3. Al terzo passo si effettuano le seguenti approssimazioni:

ˆp̂_{y} = N (ȳ, P_{y}) ≅ p̂_{y}(·), ȳ:= 𝔼p̂_{y}[y] = ∑_{i=0}^{2N} w_{i}y_{i}

P_{y} := V arp̂_{y}(y) = 𝔼p̂_{y}[(y − ȳ)(y − ȳ)^{T}] = ∑_{i=0}^{N} w_{i}(y − ȳ)(y − ȳ)^{T} ≥ 0

e vorremo che almeno per i momenti del primo e secondo ordine valga

ˆp̂_{y}(·) ≅ p_{y}(·) .

Fino ad ora si sono mappate densità di probabilità in densità di probabilità e le abbiamo approssimate con delle gaussiane.

[\begin{matrix}x \\ y \end{matrix}] ∼ N ([\begin{matrix}μ_{x} \\ μ_{y} \end{matrix}] , [\begin{matrix}P_{x} & P_{xy} \\ P_{xy}^{T} & P_{y} \end{matrix}])

con

P_{xy} = cov(x, y) ≅ ∑_{i=1}^{N}w_{i}(v_{i} − x̄)(y_{i} − ȳ)

x|y ∼ N (μ_{x} + P_{xy}P_{y}^{−1}(y − μ_{y}), P_{x} − P_{xy}P_{y}^{−1}P_{xy}^{T})

23-3 Si cerca ora di applicare quanto visto al modello dinamico non lineare:

\{ \begin{matrix}x_{k+1} = f(x_{k}, u_{k}, w_{k}) \\ y_{k} = h(x_{k}, v_{k}) \end{matrix}

Non essendo necessariamente w_{k} e v_{k} gaussiani questi vengono approssimati nel modo seguente:

w_{k} ∼ p_{w}(·) ≅ N (w̄, P_{w})
v_{k} ∼ p_{v}(·) ≅ N (v̄, P_{v})
x_{k} ≅ N (x̂_{k|k}, P_{k|k})
x_{k} ⊥ v_{k} ⊥ w_{k}

e dunque si può scrivere y = g(ξ) con

ξ = [\begin{matrix}x_{k} \\ w_{k} \\ v_{k} \end{matrix}] ∼ N ([x̂_{k|k} ; w̄ ; v̄] , [\begin{matrix}P_{k|k} & 0 & 0 \\ 0 & P_{w} & 0 \\ 0 & 0 & P_{v} \end{matrix}]) , \{ \begin{matrix}x ∈ ℝ^{n} \\ y ∈ ℝ^{m} \\ w ∈ ℝ^{p} \\ v ∈ ℝ^{q} \\ ξ ∈ ℝ^{n+p+q} = ℝ^{N} \end{matrix}

Si dovranno dunque generare 2N + 1 punti v_{i} = 0, 1, …, 2N con i pesi

w_{i} = \frac{1}{2(N + K)}  i ≠ 0
w_{0} = \frac{K}{N + K} .

E ora per calcolare x̂_{k+1|k+1} ho bisogno di x̂_{k+1|k}, ŷ_{k+1|k}, P_{k+1|k}^{x}, P_{k+1|k}^{y}, P_{k+1|k}^{xy}, infatti:

x̂_{k+1|k+1} = x̂_{k+1|k} − P_{k+1|k}^{xy}(P_{k+1|k}^{y})^{−1}(y_{k+1} − ŷ_{k+1|k})  (23.1)

e calcolando:

x̂_{k+1|k} = ∑_{i=0}^{2N} w_{i}f(v_{i}^{x}, u_{k}^{x}, w_{i}^{w})

P_{k+1|k}^{x} = ∑_{i=0}^{2N} w_{i}(f(v_{i}^{x}, u_{k}, w_{i}^{w}) − x̂_{k+1|k})(f(v_{i}^{x}, u_{k}, w_{i}^{w}) − x̂_{k+1|k})^{T}

ŷ_{k+1|k} = ∑_{i=0}^{2N} w_{i}h(x̂_{k+1|k}^{i}, v_{i}^{v})

P_{k+1|k}^{y} = ∑_{i=0}^{2N} w_{i}(ŷ_{k+1|k}^{i} − ŷ_{k+1|k})(ŷ_{k+1|k}^{i} − ŷ_{k+1|k})^{T}

P_{k+1|k}^{xy} = ∑_{i=0}^{2N} w_{i}(x̂_{k+1|k}^{i} − x̂_{k+1|k})(ŷ_{k+1|k}^{i} − ŷ_{k+1|k})^{T}

23-4 si hanno tutti i termini necessari per implementare la (23.1).

Si noti come in questo caso non è necessario linearizzare ad ogni passo la matrice come nel caso del filtro esteso e si approssimano distribuzioni piuttosto che non linearità. Inoltre con questo metodo si ottiene una precisione del secondo ordine senza calcolare hessiane o jacobiane. I pesi possono poi essere scelti in modo tale da catturare statistiche di ordine più elevato. Tuttavia qui si rende necessario diagonalizzare la matrice di varianza di ξ al variare di P_{k|k}. Per rendere meno oneroso questo calcolo si usa la Squared UKF (che utilizza la fattorizzazione di Cholesky), chiamando Q la radice di P si può scrivere:

P = UDU^{T} = (UD^{\frac{1}{2}} U^{T})(UD^{\frac{1}{2}} U^{T}) = \sqrt{P} \sqrt{P}^{T} , \sqrt{P} = Q = [v_{1}| … |v_{N}]

P = QQ^{T} = [v_{1} … v_{N}] [v_{1}^{T} ; ⋮ ; v_{N}^{T} ] = ∑_{i=0}^{N} v_{i}v_{i}^{T}

Segue un esempio in cui si mettono a confronto il Filtro di Kalman Unscented con il filtro di Kalman Esteso. Esempio:

y = x^{2}, x ∈ ℝ, x ∼ N (x̄, σ_{x}^{2})

Figura 23.2. Procedimento del filtro UKF

23-5

Rappresentando x come x := x̄+δx con δx ∼ N (0, σ_{x}^{2}) si calcolano la media e la varianza esatta di y:

ȳ_{T} = 𝔼_{y}[y] = 𝔼_{x}[x^{2}] = 𝔼_{x}[(x̄ + δx)^{2}]

= 𝔼[x̄^{2} + 2x̄δx + (δx)^{2}] = x̄^{2} + σ_{x}^{2}

(σ_{y}^{2})_{T} = V ar(y) = 𝔼[(x^{2} − (x̄^{2} + σ_{x}^{2})^{2})^{2}]

= 𝔼[4x̄^{2}(δx) + (δx)^{4} + σ_{x}^{4} + 4x̄(δx)^{3} − 4x̄σ_{x}^{2}δx − 2(δx)^{2}σ_{x}^{2}] = 4x̄^{2}σ_{x}^{2} + 𝔼[(δx)^{4}] + σ_{x}^{4} − 2σ_{x}^{4} = 2σ_{x}^{4} + 4x̄^{2}σ_{x}^{2}

dove nell’ultimo passaggio si è sfruttato il fatto che 𝔼[(δx)^{4}] = 3σ_{x}^{4}. L’esempio prosegue nella prossima lezione.

23-6

Bibliografia

[1] Giorgio Picci. Fitraggio Statistico (Wiener, Levinson, Kalman) e Applicazioni. Libreria Progetto, 2006.

[2] J. Uhlmann S. Julier. A general method for approximating nonlinear transformations of probability distributions. Technical report, University of Oxford, 1996.

[3] E. Wan R. van der Merwe. Sigma-point kalman filters for integrated navigation. Proceedings of the 60th Annual of the Institute of Navigation, 2004.

[4] G. Oriolo P. Peliti T. Fiorenzani, C. Manes. Comparative study of unscented kalman filter and extended kalman filter for position/attitude estimation in unmanned aerial vehicles. Technical report, IASI CNR, 2002.