% -----------------------------*- LaTeX -*------------------------------
\documentclass[12pt,A4]{report}
\usepackage{PSC_class}

%titoli e sillabazione in italiano
\usepackage[italian]{babel}

%per poter usare le lettere accentate (à,ò,ì) senza problemi
\usepackage[latin1]{inputenc}

%per poter inserire le figure
\usepackage[dvips]{graphicx}

%impostazione dello stile della bibliografia
\bibliographystyle{unsrt}


\begin{document}

% \course{CS 497}           % optional
% \coursetitle{Geometric Data Structures} % optional
% \semester{Fall 1998}          % optional
% \lecturer{Jeff Erickson}      % optional
\scribe{Alberton R., Ausserer M., Barazzuol A.}        % required
\lecturenumber{23}           % required, must be a number  %CONTROLLARE
\lecturedate{07 Dicembre}       % required, omit year

\maketitle

% ----------------------------------------------------------------------


%\begin{danger}
%This is the danger environment.
%\end{danger}

\section{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(\cdot)\sim \mathcal{N}(\bar{x},P_x) \qquad y=g(x) $$
e si approssima la $p_y$ con una gaussiana ottenendo: $$ p_y(\cdot)\simeq \mathcal{N}(\bar{y},P_y)$$

\begin{figure}[htbp]
    \begin{center}
      \includegraphics[width=0.9\textwidth]{Grafica1}
    \caption[Grafica1]
    {\label{fig:Grafica1}
    Procedimento dell'Unscented Kalman Filter}
    \end{center}
\end{figure}


\begin{enumerate}
\item Da $p_x(\cdot)\sim \mathcal{N}(\bar{x},P_x)$ si generano $2N+1$ punti (dove $x\in\mathbb{R}^N$) nel modo seguente:	
	\begin{itemize}
		\item $v_0 = \bar{x}$
		\item $v_1 = \bar{x}+\alpha\sigma_1$
		\item $v_2 = \bar{x}-\alpha\sigma_1$ \\
			$\vdots$
		\item $v_{2N-1} = \bar{x}+\alpha\sigma_N$
		\item $v_{2N} = \bar{x}+\alpha\sigma_N$
	\end{itemize}
dove $\alpha$ è un fattore di scala che viene imposto in modo che la media campionaria degli $v_i$ risulta pari a $\bar{x}$ e la varianza campionaria degli $v_i$ pari a $P_x$. Il calcolo di $\alpha$ si comincia fattorizzando la varianza di $x$ $P_x=UDU^T$, con $U=[u_1,\ldots,u_N]$ una matrice di vettori $u_i$ ortonormali e $D$ la matrice diagonale
$$ D=
\begin{bmatrix}

\lambda _1      &  & \\
  & \ddots & \\
   &  & \lambda _N
\end{bmatrix} \geq 0
 $$
Si puó scrivere la $P_x$ nel modo seguente:
\begin{align*}
	P_x &= UDU^T \\
	    &=\begin{bmatrix}
u_1   & \ldots & u_N\\
\end{bmatrix}
\begin{bmatrix}
\sqrt{\lambda _1}      &  & \\
  & \ddots & \\
   &  & \sqrt{\lambda _N}
\end{bmatrix}
\begin{bmatrix}
\sqrt{\lambda _1}      &  & \\
  & \ddots & \\
   &  & \sqrt{\lambda _N}
\end{bmatrix}
\begin{bmatrix}
u_1^T \\
\vdots \\
u_N^T
\end{bmatrix}\\
	&=\begin{bmatrix}
\sqrt{\lambda _1} u_1   & \ldots & \sqrt{\lambda _N} u_N\\
\end{bmatrix}
\begin{bmatrix}
\sqrt{\lambda _1} u_1^T \\
\vdots \\
\sqrt{\lambda _N} u_N^T
\end{bmatrix}\\
	&=\lambda _1 u_1 u_1^T+\ldots+\lambda _N u_N u_N^T .
\end{align*}
Chiamando $\sigma_i=\sqrt{\lambda _i} u_i$ (vedi vettore rosso in Figura \ref{fig:Grafica1}) si ha che $P_x=\sum_{i=1}^{N}\sigma_i\sigma_i^T$.\\
La densitá di probabilitá di $x$ puó essere approssimato nel modo seguente:
$$ p_x(\cdot)\simeq\sum_{i=0}^{2N}w_i\delta(x-v_i)=\hat{p}_x $$
con i pesi
$$ w_1=w_2=\ldots=w_{2N}=\frac{1}{2(N+K)} $$
$$ w_0=1-\sum_{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:

\begin{align*}
\mathbb{E}_{\hat{p}_x}[x] &= \int_x \sum_{i=0}^{2N}xw_i\delta(x-v_i)dx=\sum_{i=0}^{2N}w_iv_i=\bar{x}\sum_{i=0}^{2N}w_i=\bar{x}\\
	Var_{\hat{p}_x}(x) &= \mathbb{E}_{\hat{p}_x}[(x-\bar{x})(x-\bar{x})^T]=\int_x \sum_{i=0}^{2N} (x-\bar{x})(x-\bar{x})^T w_i \delta(x-v_i) dx,\\
	&=\int_\xi \sum_{i=0}^{2N} \xi \xi^T w_i \delta(\xi+\bar{x}-v_i) d\xi=\sum_{i=1}^{N}\frac{1}{2(N+K)}2\alpha^2\sigma_i\sigma_i^T\\
	&=\frac{\alpha^2}{N+K}\sum_{i=1}^{N}\sigma_i\sigma_i^T=\frac{\alpha^2}{N+K}P_x
\end{align*}

avendo effettuato il cambio di variabili $\xi=x-\bar{x}$. Poiché $Var_{\hat{p}_x}(x)=P_x$ si determina $\alpha=\sqrt{N+K}$.



\item Al secondo passo si devono mappare le $v_i$ usando la funzione non lineare $g(\cdot)$ ottenendo così $y_i=g(v_i)$ con $i=0,\ldots,2N$. La densitá di probabilitá approssimata di $y$ risulta essere
$$ p_y\simeq\sum_{i=0}^{2N}w_i\delta(y-y_i)=\hat{p}_y  $$


\item Al terzo passo si effettuano le seguenti approssimazioni:
\[
\hat{\hat{p}}_y = \mathcal{N}({\bar{y},P_y})\cong \hat{p}_y(\cdot) , \quad \bar{y}:=\mathbb{E}_{\hat{p}_y}[y]=\sum_{i=0}^{2N}w_iy_i 
\]
\[
P_y:=Var_{\hat{p}_y}(y)=\mathbb{E}_{\hat{p}_y}[(y-\bar{y})(y-\bar{y})^T]=\sum_{i=0}^{N}w_i(y-\bar{y})(y-\bar{y})^T\geq 0
\]
e vorremo che almeno per i momenti del primo e secondo ordine valga $$\hat{\hat{p}}_y(\cdot)\cong p_y(\cdot)\, .$$
Fino ad ora si sono mappate densità di probabilità in densità di probabilità e le abbiamo approssimate con delle gaussiane.
\[
\begin{bmatrix}
x \\ y
\end{bmatrix}
\sim
\mathcal{N}
(
\begin{bmatrix}
\mu_{x} \\ \mu_y
\end{bmatrix}
,
\begin{bmatrix}
P_x & P_{xy} \\
P_{xy}^T & P_y
\end{bmatrix}
)
\]
con
\[
P_{xy}=cov(x,y)\cong{\sum_{i=1}^{N}}w_i(v_i-\bar{x})(y_i-\bar{y})
\]
\[
x|y \sim \mathcal{N}(\mu_x+P_{xy}P_y^{-1}(y-\mu_y),P_x-P_{xy}P_y^{-1}P_{xy}^T)
\]

\end{enumerate}

Si cerca ora di applicare quanto visto al modello dinamico non lineare:
\begin{equation*}
\left\{
\begin{array}{l}
x_{k+1}=f(x_k,u_k,w_k)\\
y_k=h(x_k,v_k)
\end{array}\right.
\end{equation*}
Non essendo necessariamente $w_k$ e $v_k$ gaussiani questi vengono approssimati nel modo seguente:
\[
 w_k  \sim p_w(\cdot)\cong \mathcal{N}(\bar{w},P_w) 
 \]
 \[
 v_k  \sim p_v(\cdot)\cong\mathcal{N}(\bar{v},P_v) 
 \]
 \[
 x_k  \cong\mathcal{N}(\hat{x}_{k|k},P_{k|k}) 
 \]
 \[
 x_k \, \bot \, v_k \, \bot \, w_k
 \]
e dunque si può scrivere  $y=g(\xi)$ con
\[
\xi=
\begin{bmatrix}
x_k \\ w_k \\ v_k
\end{bmatrix}
\sim
\mathcal{N}(
\begin{bmatrix}
\hat{x}_{k|k} \\ \bar{w} \\ \bar{v}
\end{bmatrix},
\begin{bmatrix}
P_{k|k} & 0 & 0 \\
0 & P_w & 0 \\
0 & 0 & P_v
\end{bmatrix}
)\, , \quad
\left\{
\begin{array}{l}
x\in\mathbb{R}^n \\
y\in\mathbb{R}^m\\
w\in \mathbb{R}^p\\
v\in \mathbb{R}^q \\
\xi \in \mathbb{R}^{n+p+q}=\mathbb{R}^N
\end{array}\right.
\]
Si dovranno dunque generare $2N+1$ punti $v_i=0,1,\ldots,2N$ con i pesi

\begin{align*}
w_i &= \frac{1}{2(N+K)} \qquad i\neq0 \\
w_0 &= \frac{K}{N+K}.
\end{align*}

E ora per calcolare $\hat{x}_{k+1|k+1}$ ho bisogno di $\hat{x}_{k+1|k} , \, \hat{y}_{k+1|k},\,P^x_{k+1|k},\,P^y_{k+1|k},\,P^{xy}_{k+1|k}$, infatti:
\begin{equation}
\hat{x}_{k+1|k+1}=\hat{x}_{k+1|k}-P^{xy}_{k+1|k}(P^{y}_{k+1|k})^{-1}(y_{k+1}-\hat{y}_{k+1|k})
\label{aggiornamento}
\end{equation}
e calcolando:
\begin{align*}
\hat{x}_{k+1|k} &=\sum_{i=0}^{2N}w_i f(v_i^x,u_k^x,w_i^w) \\
P_{k+1|k}^x &=\sum_{i=0}^{2N}w_i(f(v_i^x,u_k,w_i^w)-\hat{x}_{k+1|k})(f(v_i^x,u_k,w_i^w)-\hat{x}_{k+1|k})^T \\
\hat{y}_{k+1|k} &=\sum_{i=0}^{2N} w_ih(\hat{x}^i_{k+1|k},v_{i}^{v}) \\
P_{k+1|k}^y &= \sum_{i=0}^{2N}w_i(\hat{y}_{k+1|k}^{i}-\hat{y}_{k+1|k})(\hat{y}_{k+1|k}^{i}-\hat{y}_{k+1|k})^T \\
P^{xy}_{k+1|k} &=\sum_{i=0}^{2N}w_i(\hat{x}^i_{k+1|k}-\hat{x}_{k+1|k})(\hat{y}^i_{k+1|k}-\hat{y}_{k+1|k})^T
\end{align*}
si hanno tutti i termini necessari per implementare la (\ref{aggiornamento}).

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 $\xi$ 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 , \quad \sqrt{P}=Q=[v_1|\ldots|v_N]
\]
\[
P=QQ^T=
\begin{bmatrix}
v_1 &\ldots & v_N
\end{bmatrix}
\begin{bmatrix}
v_1^T \\ \vdots \\ v_N^T
\end{bmatrix}
= \sum_{i=0}^{N}v_iv_i^T
\]

\vspace{0.5cm}
Segue un esempio in cui si mettono a confronto il Filtro di Kalman Unscented con il filtro di Kalman Esteso.\\
\textbf{Esempio:}

\[
y=x^2,\,x\in \mathbb{R},\,x\sim \mathcal{N}(\bar{x},\sigma_x^2)
\]

\begin{figure}[htbp]
    \begin{center}
      \includegraphics[width=0.9\textwidth]{Grafica2}
    \caption[Grafica2]
    {\label{fig:Grafica2} Procedimento del filtro UKF}
    \end{center}
\end{figure}


Rappresentando $x$ come $x:=\bar{x}+\delta{x}$ con $\delta x \sim \mathcal{N}(0,\sigma^2_x)$ si calcolano la media e la varianza esatta di y:
\begin{align*}
\bar{y}_T & =\mathbb{E}_y[y]=\mathbb{E}_x[x^2]=\mathbb{E}_x[(\bar{x}+\delta x)^2] \\
          & = \mathbb{E}[\bar{x}^2+2\bar{x}\delta x +(\delta x)^2] \\
          & = \bar{x}^2 + \sigma_x^2 \\ \\
(\sigma_y^2)_T & = Var(y) = \mathbb{E}[(x^2-(\bar{x}^2+\sigma_x^2)^2)^2] \\
							 & = \mathbb{E}[4\bar{x}^2(\delta x)+(\delta x)^4+\sigma_x^4+4\bar{x}(\delta x)^3-4\bar{x}\sigma_x^2\delta x-2(\delta x)^2\sigma_x^2] \\
							 & = 4\bar{x}^2\sigma_x^2+\mathbb{E}[(\delta x)^4]+\sigma_x^4 - 2\sigma_x^4 \\
							 & = 2 \sigma_x^4 + 4\bar{x}^2\sigma^2_x
\end{align*}

dove nell'ultimo passaggio si è sfruttato il fatto che $\mathbb{E}[(\delta x)^4]=3\sigma^4_x$. L'esempio prosegue nella prossima lezione.


\nocite{Picci:06}
\nocite{Julier:96}
\nocite{Merwe:04}
\nocite{Fiorenzani:02}

\bibliography{PSC_bibliografia}




\end{document}
