Filtro di Kalman Unscented

Nel caso di funzioni $f$ e $h$ estremamente non lineari, Unscented Kalman filter (UKF), anche chiamato Sigma Point Kalman Filter (SPKF), tende a fornire prestazioni migliori rispetto a EKF. Non solo lo stato, ma diversi punti intorno la media (chiamati sigma points) vengono propagati attraverso le varie funzioni che compongono l'aggiornamento dello stato di Kalman. Non richiede l'utilizzo di Jacobiani e si dimostra che fornisce una stima migliore di media e varianza del processo.

La trasformazione unscented (UT) [17] permette di stimare il valor medio e la varianza di una variabile casuale all'uscita di un sistema $f$ non lineare.

Per stimare valor medio e varianza, la variabile casuale $\mathbf{x} \in \mathbb{R}^{n}$ viene approssimata da $2n+1$ punti $\mathbf{\chi}_i$, chiamati sigma points, ognuno pesato con un peso $w_i$ in modo da rappresentare attraverso questi punti una distribuzione con media e varianza esattamente $\bar{\mathbf{x}}$ e $\mathbf{P}_{xx}$, uguali a quelli di $\mathbf{x}$. + Per ottenere dei punti con media e varianza uguali a quelli della distribuzione originale è necessario prendere i $2n+1$ sigma points e i rispettivi pesi nel modo seguente:

$$\begin{array}{ll} \boldsymbol{\chi}_0 = \bar{\mathbf{x}} & w_... ...t)_i & w_{i+n} = \frac{\lambda}{2 (n + \lambda)} \end{array}$$ (2.59)

dove $n$ è la dimensione dello stato e $\lambda$ è un numero definito come $\lambda = \alpha^2 (n + \kappa) - n$ con $\alpha \leq 1$ un numero piccolo positivo e $\kappa$ solitamente posto a 0 o $3-n$. In alcuni articoli viene posto $\alpha=1$ e $\kappa=3-n$ per le distribuzioni gaussiane.

A differenza dei metodi montecarlo, i sigma points sono scelti in maniera deterministica in modo da rappresentare al meglio le statistiche della variabile.

Ottenuti i sigma points, questi possono venire trasformati attraverso la funzione

$$\mathbf{y}_i = f(\boldsymbol{\chi}_i)$$ (2.60)

e viene calcolata media e varianza del risultato ottenuto (unscented transformation)

$$\bar{\mathbf{y}} \approx \sum_{0}^{2n} w_i f(\mathbf{x}_i) \quad \mathbf{P}_{yy} \approx \sum_{0}^{2n} w'_i (y_i - \bar{y})(y_i - \bar{y})^{\top}$$ (2.61)

per ogni punto $i=0, \ldots, 2n$. Questa media e varianza sono una buona approssimazione possibile della media e varianza della distribuzione in ingresso trasformata attraverso la funzione $f$. La differenza tra i pesi $w_i$ e $w'_i$ è solo nel primo termine dove si pone $w'_0 = \lambda/(n+\lambda) + (1-\alpha^2+\beta)$, con $\beta=2$ per le distribuzioni guassiane.

Nel caso dell'aggiornamento dello stato definiamo, per mantenere la sintassi uguale, una variabile chiamata stato aumentato $\mathbf{x}^{a} \in \mathbb{R}^{n^{a}}$ con $n^{a}=n+q$ formata dallo stato $\mathbf{x} \in \mathbb{R}^{n}$ e dal rumore di processo $w$, a media nulla, in modo da usare la funzione

$$\boldsymbol{\chi}^{-} = f(\mathbf{x}^{a}_{k-1}, \mathbf{u}_{k} )$$ (2.62)

di aggiornamento dello stato che tenga conto in maniera non lineare anche del contibuto del rumore di processo. Allo stesso modo definiamo la matrice di covarianza aumentata come:

$$\mathbf{P}_{xx}^{a} = \begin{bmatrix}\mathbf{P}_{xx} & 0 0 & \mathbf{Q} \end{bmatrix}$$ (2.63)

Attraverso questo procedimento è possibile generare il filtro di kalman unscented.

Nel caso in cui il rumore di processo sia additivo il sistema può ridiventare simile a quello di Kalman lineare nella forma

$$\mathbf{P}^{-}_{k} = \sum_{0}^{2n} w'_i (\boldsymbol{\chi}^{-}_{i... ...dsymbol{\chi}^{-}_{i} - \bar{\boldsymbol{\chi}}^{-}_{i} )^{\top} + \mathbf{Q}_k$$ (2.64)

Dai sigma points $\boldsymbol{\chi}^{-}_{i}$, proiettati attraverso $f$ e rappresentanti la distribuzione dello stato a priori, è possibile fare la stima dell'osservazione a priori:

$$Z_i = h(\mathbf{\chi}^{-}_{i})$$ (2.65)

da cui calcolare il valore più probabile dell'osservazione $\hat{z}$ pesando i risultati $Z_i$ con i pesi dei sigma point associati come dall'equazione 2.61. Anche in questo caso il rumore di osservazione può essere inserito come stato aumentato e se supposto additivo e indipendente può venire sommato alla matrice di covarianza.

Il guadagno di Kalman unscented diventa:

$$\mathbf{K} = \mathbf{P}_{xz} (\mathbf{P}_{zz} + \mathbf{R})^{-1}$$ (2.66)

con $\mathbf{P}_{zz}$ covarianza di $Z_i$ e $\mathbf{P}_{xz}$ cross covarianza tra $\mathbf{\chi}^{-}_{i}$ e $Z_i$.