% -----------------------------*- LaTeX -*------------------------------
\documentclass[12pt,A4]{article}
\usepackage[utf8]{inputenc}
\usepackage[italian]{babel}
\usepackage[T1]{fontenc}
\usepackage[dvips]{graphicx}
\usepackage{float}
\usepackage{indentfirst}
\usepackage{amsmath,graphicx}
\usepackage{amssymb}
%\usepackage{../PSC_class}

%--------------IMPOSTAZIONI PAGINA----------------
\oddsidemargin= 0 cm
\evensidemargin= 0 cm
\textwidth =16 cm
\textheight     23cm
\topmargin      0.5cm
\headheight     0cm
\headsep        0cm
\footskip       2cm

\begin{document}

%\scribe{L. Schenato}        % required
%\lecturenumber{4}           % required, must be a number
%\lecturedate{Aprile 26}       % required, omit year
\title{Esercitazione 1}
\author{Guido Albertin\\Elena Toffoli\\Giancarlo Baldan}
\maketitle

\section*{Inseguimento di un veicolo}
\subsection*{1}
La varianza di errore converge in quanto si ha che
\begin{equation}
Q^{1/2} =
\left[
  \begin{array}{cccc}
    0.0316 & 0.0008 & 0 & 0 \\
    0.0008 & 0.1000 & 0 & 0 \\
    0 & 0 & 0.0316 & 0.0008 \\
    0 & 0 & 0.0008 & 0.1000
  \end{array}
\right]
\end{equation}
ha rango pieno quindi $(A,Q^{1/2})$ è raggiungibile e che la coppia $(A,C)$ è osservabile da
\begin{equation}
\mathrm{rank}\,\mathcal{O} = \mathrm{rank}\,
\left[
  \begin{array}{cccccc}
    1.0000   &      0  &       0  &       0 \\
         0   &      0  &  1.0000  &       0 \\
    1.0000   & 0.1000  &       0  &       0 \\
         0   &      0  &  1.0000  &  0.1000 \\
    1.0000   & 0.2000  &       0  &       0 \\
         0   &      0  &  1.0000  &  0.2000 \\
    1.0000   & 0.3000  &       0  &       0 \\
         0   &      0  &  1.0000  &  0.3000
  \end{array}
\right] = 4
\end{equation}
di conseguenza per quanto sviluppato a lezione si ha che esiste unica e definita positiva una matrice $P_\infty$ soluzione della ARE e stabilizzante. La matrice di guadagno di retroazione asintotica è
\begin{equation}
K_\infty =
\left[
  \begin{array}{cc}
    0.4973 &   0.0000 \\
    0.7608 &   0.0000 \\
    0.0000 &   0.4973 \\
    0.0000 &   0.7608
  \end{array}
\right]
\end{equation}
  \subsection*{4}
  La varianza campionaria dell'errore di predizione $e_k^p=x_k - \hat{x}_{k|k-1}$ può ottenersi applicando la formula
  \begin{equation}
  P^{p}_k = \frac{1}{k+1}\sum_{j=0}^k(e_j^p-m_k)\,(e_j^p-m_k)^T,
  \end{equation}

  in cui $m_k=\frac{1}{k+1}\sum_{j=0}^k e_j^p$ rappresenta la media campionaria dell'errore di predizione calcolata all'istante $k$.

  Il calcolo di media e varianza campionaria può effettuarsi anche in modo ricorsivo come dimostrano i passaggi di seguito riportati.
  \begin{eqnarray*}
    m_{k+1} & = & \frac{1}{k+2}\sum_{j=0}^{k+1} e_j^p \\
             & = & \frac{1}{k+2}\sum_{j=0}^{k} e_j^p+\frac{1}{k+2} e_{k+1}^p\\
             & = & \frac{k+1}{k+2}m_k+\frac{1}{k+2} e_{k+1}^p.\\
  \end{eqnarray*}
  \begin{eqnarray*}
    P^{p}_{k+1} & = &  \frac{1}{k+2}\sum_{j=0}^{k+1}(e_j^p-m_{k+1})\,(e_j^p-m_{k+}))^T \\
                & = &  \frac{1}{k+2}\sum_{j=0}^{k}\left (e_j^p- \frac{k+1}{k+2}m_k-\frac{1}{k+2}e_{k+1}^p \right ) \!\! \left( e_j^p-\frac{k+1}{k+2}m_k-\frac{1}{k+2}e_{k+1}^p \right )^T +\\
                &   &  +\frac{1}{k+2}\left (e_{k+1}^p- \frac{k+1}{k+2}m_k-\frac{1}{k+2}e_{k+1}^p \right ) \!\! \left( e_{k+1}^p-\frac{k+1}{k+2}m_k-\frac{1}{k+2}e_{k+1}^p \right )^T\\
                & = &  \frac{1}{k+2}\sum_{j=0}^{k}\left (e_j^p- m_k + \frac{1}{k+2} (m_k-e_{k+1}^p) \right ) \!\! \left( e_j^p- m_k + \frac{1}{k+2} (m_k-e_{k+1}^p) \right )^T +\\
                &   &  +\frac{(k+1)^2}{(k+2)^3}\left (e_{k+1}^p- m_k \right ) \left( e_{k+1}^p- m_k \right )^T\\
                & = &  \frac{1}{k+2}\sum_{j=0}^{k} \left(e_j^p- m_k \right) \left(e_j^p- m_k \right)^T+\frac{1}{(k+2)^2}\left [\sum_{j=0}^{k} \left(e_j^p- m_k \right) \right]\left(  m_k -e_{k+1}^p \right )^T+\\
                &   &  +\frac{1}{(k+2)^2}\left(  m_k -e_{k+1}^p \right )\left [\sum_{j=0}^{k} \left(e_j^p- m_k \right)^T \right]+\frac{k+1}{(k+2)^2}\left (e_{k+1}^p- m_k \right ) \left( e_{k+1}^p- m_k \right )^T\\
                & = &  \frac{k+1}{k+2}P^{p}_{k}+\frac{k+1}{(k+2)^2}\left (e_{k+1}^p- m_k \right ) \left( e_{k+1}^p- m_k \right )^T.
  \end{eqnarray*}

  Nelle simulazioni effettuate la varianza campionaria dell'errore di predizione converge al valore teorico $P_\infty$ menzionato in precedenza e soluzione della ARE.

  A sua volta anche la varianza campionaria dell'errore di stima $e_k^s=x_k - \hat{x}_{k|k}$ si comporta asintoticamente secondo
  \begin{equation}
  P_k^s=\frac{1}{k+1}\sum_{j=0}^k(e_j^s - m_k)\,(e_j^s - m_k)^T \rightarrow P_\infty - K_\infty\,C^T\,P_\infty.
  \end{equation}

  A prova della convergenza riscontrata si riportano di seguito gli andamenti temporali delle tracce delle matrici di varianza campionaria e teorica relative all'errore di predizione e stima %\footnote{Si ricorda che la convergenza della traccia non implica necessariamente la convergenza delle matrici che comunque, nel nostro caso, è verificata}.
  \begin{figure}[!h]
		\begin{center}
			\includegraphics[width = 16cm]{Varianza1.eps}
		\end{center}
		\vspace{-1cm}
 	\end{figure}

	Come si vede lo stimatore asintotico (di cui si è implementata solo la versione che esegue il filtraggio) sulla singola realizzazione
	ha un comportamento peggiore rispetto al filtro di Kalman ricorsivo infatti la sua varianza d'errore campionaria risulta essere, a parte un transitorio iniziale,
	sempre maggiore di quella ottenuta col filtro tempo variante. Altre conferme di questo comportamento si possono ottenere esaminando i numerosi grafici
	prodotti dal codice fornito.
	
	Effettuando inoltre $M$ distinte simulazioni è possibile calcolare la varianza campionaria degli errori in ogni istante utilizzando i campioni nelle diverse simulazioni.
	Le tracce di tali varianze campionarie, che possono ancora essere calcolate mediante le formule ricorsive determinate in precedenza, sono riportate
	nei grafici seguenti.
   \begin{figure}[!h]
		\begin{center}
		\includegraphics[width = 16cm]{Varianza2.eps}
		\end{center}
		\vspace{-1cm}
 	\end{figure}

	è evidente che le medie campionarie convergono, dopo un breve transitorio iniziale, ai rispettivi valori teorici. Si ricorda che il valore teorico
	per la varianza d'errore di filtraggio nel caso di stimatore tempo invariante è proprio $P_\infty$.

	\subsection*{5}
	In questo caso non vi è alcuna modifica sostanziale rispetto al caso in cui $q=1$, $r=1$. Infatti ancora una volta la coppia $(A,Q^{1/2})$ è raggiungibile e la coppia $(A,C)$ è osservabile
	pertanto esiste unica e definita positiva una matrice $P_\infty$ soluzione della ARE e stabilizzante. In questo caso però la matrice
	$P_\infty$ è inferiore rispetto a quella ottenuta nel caso precedente in quanto il rumore è diminuito. La matrice di varianza d'errore di stima
	in questo caso è nulla in corrispondenza agli errori di posizione perchè le misure di posizione sono affette da errore nullo.

	\subsection*{6}
	In questo caso, mentre $(A,C)$ è ancora osservabile, la coppia $(A,Q^{1/2})$ non è più raggiungibile essendo nulla la matrice $Q^{1/2}$. Inoltre la matrice di stato relativa al sottospazio
	non raggiungibile coincide con $A$ e non è quindi asintoticamente stabile perchè ha tutti gli autovalori in $1$. Se ne deduce che
	la coppia $(A,Q^{1/2})$ non è neppure stabilizzabile. Questo implica che la matrice $P_\infty$ verso cui converge $P(t)$ non è stabilizzante
	quindi il filtro tempo invariante che si otterrebbe non sarebbe asintoticamente stabile. Inoltre, essendo nullo il rumore di modello, la matrice $P(t)$
	converge alla matrice nulla cui corrisponderebbe un matrice di guadagno a regime $K_\infty=0$ e, conseguentemente, il relativo filtro tempo invariante
	avrebbe la forma $\hat{x}_{k+1}=A\hat{x}_k$ e realizzerebbe una stima dello stato senza basarsi su alcuna misura; tale stima per condizioni iniziali di velocità non nulle risulta divergente (in quanto A come gia osservato è instabile). Difatti si ha che l'andamento dell'errore di stima è dato da $e_{k+1} = A\,e_{k}$ (segue da $K_\infty=0$) e quindi che $e_{k} = A^k\,e_{0}$. Per la particolare forma di $A$ si ha che
           \begin{equation}
                  A^k = \left[
                          \begin{array}{cccc}
                            1   &    0.1\,k  &       0  &       0 \\
                         0   &      1     &       0  &       0 \\
                             0   &      0     &       1  &  0.1\,k \\
                          0   &      0     &       0  &       1
                          \end{array}
                      \right]
          \end{equation}
        da cui si deduce come un iniziale errore sulla stima di una velocità causi un errore divergente sulla relativa posizione.

\section*{Stima di parametri costanti}
\subsection*{1}
In questo caso si ha $A=1$, $Q=0$, $C=1$, $R=r$, pertanto le equazioni del filtro di Kalman risultano:
\begin{eqnarray}
	p_{k+1} & = &  p_k-p_k^2\frac{1}{p_k+r}=\frac{p_k}{r+p_k}r \label{Kalman1}\\
	k_k     & = &  \frac{p_k}{p_k+r} \\
	\hat{x}_{k+1} & = & \hat{x}_k+\frac{p_{k+1}}{p_{k+1}+r}(y_{k+1}-\hat{x}_k)\label{Kalman3}.
\end{eqnarray}

Supponendo che le misure partano dall'istante $0$, l'inizializzazione dell'algoritmo prevede di partire con la condizione $\hat{x}_{-1}=\overline{x}_0$, ossia
la stima a posteriori all'istante $-1$ coincide con la stima a priori all'istante $0$. La varianza dell'errore di predizione all'istante zero è invece $p_0$

In questo semplice caso tutte le formule ricorsive ottenute possono essere scritte in forma chiusa. Per la varianza dell'errore di predizione si trova
\begin{equation}\label{VarChiusa}
	p_k=\frac{p_0}{kp_0+r}r
\end{equation}
La dimostrazione della (\ref{VarChiusa}) può essere fatta per induzione infatti per $k=1$ porge \mbox{$p_1=\frac{p_0}{p_0+r}r$}, che è esattamente quanto si
ottiene dalla (\ref{Kalman1}) per $k=0$ quindi la base dell'induzione è verificata.

Il passo induttivo si ottiene ancora una volta sfruttando la (\ref{Kalman1}) e l'ipotesi induttiva:
\begin{eqnarray*}
	p_k & = & p_{k-1}\frac{r}{r+p_{k-1}}=(H)=\frac{p_0}{(k-1)p_0+r}\frac{r}{r+\frac{p_0}{(k-1)p_0+r}r}r\\
		 & = & \frac{p_0}{(k-1)p_0+r}\frac{(k-1)p_0+r}{kp_0+r}=\frac{p_0}{kp_0+r}r.
\end{eqnarray*}

In modo analogo è possibile dimostrare che la forma chiusa per il calcolo della stima al tempo $k$ è:
\begin{equation}\label{StimaChiusa}
	\hat{x}_k=\frac{r}{(k+1)p_0+r}\overline{x}_0+\frac{(k+1)p_0}{(k+1)p_0+r}m_y(k)=\alpha(k,p_0)\overline{x}_0+\beta(k,p_0)m_y(k)
\end{equation}
in cui $m_y(k)$ è la media campionaria delle misure fino al tempo $k$.

La base dell'induzione è verificata perchè la (\ref{StimaChiusa}), per $k=0$, porge $\hat{x}_0=\frac{r}{p_0+r}\overline{x}_0+\frac{p_0}{p_0+r}y_0$
che è quanto si ottiene anche dalla (\ref{Kalman3}) per $k=-1$.

Il passo induttivo può invece dimostrarsi utilizzando le (\ref{Kalman3}) e (\ref{VarChiusa}) oltre all'ipotesi induttiva:
\begin{eqnarray*}
	\hat{x}_{k+1} & = & \frac{r}{p_{k+1}+r}\hat{x}_k+\frac{p_{k+1}}{p_{k+1}+r}y_{k+1}=\frac{(k+1)p_0+r}{(k+2)p_0+r}\hat{x}_k+\frac{p_0}{(k+2)p_0+r}y_{k+1}=(H)=\\
					  & = & \frac{r}{(k+2)p_0+r}\overline{x}_0+\frac{(k+2)p_0}{(k+2)p_0+r}m_y(k+1).
\end{eqnarray*}

Esaminando la (\ref{VarChiusa}) è ora evidente che $\lim_{k\rightarrow \infty}p_k=0 \,\, \forall p_0$ e, conseguentemente, si ha $\lim_{k\rightarrow \infty}k_k=0 \,\, \forall p_0$.

\subsection*{2}

Dalla (\ref{VarChiusa}) si ricava facilmente $\lim_{p_0\rightarrow \infty}p_k=\frac{r}{k}$ (quindi $\lim_{p_0\rightarrow \infty}p_1=r$).

Dalla (\ref{StimaChiusa}) si ottiene invece $\lim_{p_0\rightarrow \infty}\hat{x}_k=m_y(k)$.

Questi risultati mostrano quale sia il comportamento del filtro di Kalman quando non si ha alcuna conoscenza a priori della grandezza
da stimare (cioè $p_0=\infty$).

Per capire come questa conoscenza a priori influenzi il comportamento del filtro occorre esaminare più attentamente la (\ref{StimaChiusa})
in cui si nota come, ad ogni istante, la stima sia ottenuta come combinazione lineare convessa ($\alpha+\beta=1$) della conoscenza a
priori del parametro e della media campionaria delle misure fino all'istante k.

Fissato il valore di k, al crescere di $p_0$ la stima si avvicina alla media campionaria $m_y(k)$ ($\alpha$ diminuisce e $\beta$ aumenta)
quindi $p_0$ determina quanto la stima deve basarsi sulla conoscenza a priori del parametro rispetto alla media campionaria. Al limite per $p_0=0$
la stima coincide con $\overline{x}_0$ mentre per $p_0=\infty$ la stima coincide, come già visto, con la media campionaria.

Fissato il valore di $p_0$, al crescere di $k$ si ha che $\alpha$ diminuisce mentre $\beta$ aumenta quindi aumentando il numero di misure
la stima tende a spostarsi verso la media campionaria indipendentemente da $p_0$.

\subsection*{3}
	
	Nei grafici seguenti si riportano gli andamenti ottenuti in simulazione utilizzando \mbox{$x_0=1.9$}, $\overline{x}_0=1.5$, $p_0=0.5$ ed $r=0.4$.

	\begin{figure}[!h]
		\begin{center}
			\includegraphics[width = 16cm]{ImmaginiSecondo.eps}
		\end{center}
 	\end{figure}

	Come si evince facilmente dalla relazione ($\ref{VarChiusa}$) la varianza dell'errore di predizione converge a zero come $\frac{1}{k}$ ,
	questo tipo di andamento è evidente anche nel grafico testé riportato.
\end{document}
