Esercitazione 1

Guido Albertin Elena Toffoli Giancarlo Baldan

5 luglio 2007

Inseguimento di un veicolo

1

La varianza di errore converge in quanto si ha che

Q^{1/2} = [ \begin{matrix}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{matrix} ]  (1)

ha rango pieno quindi (A, Q^{1/2}) è raggiungibile e che la coppia (A, C) è osservabile da

rank O = rank [ \begin{matrix}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{matrix} ] = 4  (2)

di conseguenza per quanto sviluppato a lezione si ha che esiste unica e definita positiva una matrice P_{∞} soluzione della ARE e stabilizzante. La matrice di guadagno di retroazione asintotica è

K_{∞} = [ \begin{matrix}0.4973 & 0.0000 \\ 0.7608 & 0.0000 \\ 0.0000 & 0.4973 \\ 0.0000 & 0.7608 \end{matrix} ]  (3)

La varianza campionaria dell’errore di predizione e_{k}^{p} = x_{k} − x̂_{k|k−1} può ottenersi applicando la formula

P_{k}^{p} = \frac{1}{k + 1} ∑_{j=0}^{k}(e_{j}^{p} − m_{k}) (e_{j}^{p} − m_{k})^{T} ,  (4)

in cui m_{k} = \frac{1}{k+1} ∑_{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.

m_{k+1} = \frac{1}{k + 2} ∑_{j=0}^{k+1} e_{j}^{p}

= \frac{1}{k + 2} ∑_{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}.

P_{k+1}^{p} = \frac{1}{k + 2} ∑_{j=0}^{k+1}(e_{j}^{p} − m_{k+1}) (e_{j}^{p} − m_{k+}))^{T}

= \frac{1}{k + 2} ∑_{j=0}^{k} (e_{j}^{p} − \frac{k + 1}{k + 2}m_{k} − \frac{1}{k + 2}e_{k+1}^{p})(e_{j}^{p} − \frac{k + 1}{k + 2}m_{k} − \frac{1}{k + 2}e_{k+1}^{p})^{T} +

+\frac{1}{k + 2} (e_{k+1}^{p} − \frac{k + 1}{k + 2}m_{k} − \frac{1}{k + 2}e_{k+1}^{p})(e_{k+1}^{p} − \frac{k + 1}{k + 2}m_{k} − \frac{1}{k + 2}e_{k+1}^{p})^{T}

= \frac{1}{k + 2} ∑_{j=0}^{k} (e_{j}^{p} − m_{k} + \frac{1}{k + 2}(m_{k} − e_{k+1}^{p}))(e_{j}^{p} − m_{k} + \frac{1}{k + 2}(m_{k} − e_{k+1}^{p}))^{T} +

+\frac{(k + 1)^{2}}{(k + 2)^{3}} (e_{k+1}^{p} − m_{k}) (e_{k+1}^{p} − m_{k})^{T}

= \frac{1}{k + 2} ∑_{j=0}^{k} (e_{j}^{p} − m_{k}) (e_{j}^{p} − m_{k})^{T} + \frac{1}{(k + 2)^{2}} [∑_{j=0}^{k} (e_{j}^{p} − m_{k})] (m_{k} − e_{k+1}^{p})^{T} +

+\frac{1}{(k + 2)^{2}} (m_{k} − e_{k+1}^{p}) [∑_{j=0}^{k} (e_{j}^{p} − m_{k})^{T} ] + \frac{k + 1}{(k + 2)^{2}} (e_{k+1}^{p} − m_{k}) (e_{k+1}^{p} − m_{k})^{T}

= \frac{k + 1}{k + 2}P_{k}^{p} + \frac{k + 1}{(k + 2)^{2}} (e_{k+1}^{p} − m_{k}) (e_{k+1}^{p} − m_{k})^{T} .

Nelle simulazioni effettuate la varianza campionaria dell’errore di predizione converge al valore teorico P_{∞} menzionato in precedenza e soluzione della ARE.

A sua volta anche la varianza campionaria dell’errore di stima e_{k}^{s} = x_{k} − x̂_{k|k} si comporta asintoticamente secondo

P_{k}^{s} = \frac{1}{k + 1} ∑_{j=0}^{k}(e_{j}^{s} − m_{k}) (e_{j}^{s} − m_{k})^{T} → P_{∞} − K_{∞} C^{T} P_{∞}.  (5)

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

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.

è 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_{∞}.

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_{∞} soluzione della ARE e stabilizzante. In questo caso però la matrice P_{∞} è 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.

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_{∞} 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_{∞} = 0 e, conseguentemente, il relativo filtro tempo invariante avrebbe la forma x̂_{k+1} = Ax̂_{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_{∞} = 0) e quindi che e_{k} = A^{k} e_{0}. Per la particolare forma di A si ha che

A^{k} = [ \begin{matrix}1 & 0.1 k & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0.1 k \\ 0 & 0 & 0 & 1 \end{matrix} ]  (6)

da cui si deduce come un iniziale errore sulla stima di una velocità causi un errore divergente sulla relativa posizione.

Stima di parametri costanti

1

In questo caso si ha A = 1, Q = 0, C = 1, R = r, pertanto le equazioni del filtro di Kalman risultano:

p_{k+1} = p_{k} − p_{k}^{2} \frac{1}{p_{k} + r} = \frac{p_{k}}{r + p_{k}} r  (7)
k_{k} = \frac{p_{k}}{p_{k} + r}  (8)
x̂_{k+1} = x̂_{k} + \frac{p_{k+1}}{p_{k+1} + r}(y_{k+1} − x̂_{k}).  (9)

Supponendo che le misure partano dall’istante 0, l’inizializzazione dell’algoritmo prevede di partire con la condizione 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

p_{k} = \frac{p_{0}}{kp_{0} + r}r  (10)

La dimostrazione della (??) può essere fatta per induzione infatti per k = 1 porge p_{1} = \frac{p_{0}}{p_{0}+r}r, che è esattamente quanto si ottiene dalla (??) per k = 0 quindi la base dell’induzione è verificata.

Il passo induttivo si ottiene ancora una volta sfruttando la (??) e l’ipotesi induttiva:

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.

In modo analogo è possibile dimostrare che la forma chiusa per il calcolo della stima al tempo k è:

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) = α(k, p_{0})\overline{x}_{0} + β(k, p_{0})m_{y}(k)  (11)

in cui m_{y}(k) è la media campionaria delle misure fino al tempo k.

La base dell’induzione è verificata perchè la (??), per k = 0, porge x̂_{0} = \frac{r}{p_{0}+r} \overline{x}_{0}+ \frac{p_{0}}{p_{0}+r}y_{0} che è quanto si ottiene anche dalla (??) per k = −1.

Il passo induttivo può invece dimostrarsi utilizzando le (??) e (??) oltre all’ipotesi induttiva:

x̂_{k+1} = \frac{r}{p_{k+1} + r}x̂_{k} + \frac{p_{k+1}}{p_{k+1} + r}y_{k+1} = \frac{(k + 1)p_{0} + r}{(k + 2)p_{0} + r}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).

Esaminando la (??) è ora evidente che lim_{k→∞} p_{k} = 0 ∀p_{0} e, conseguentemente, si ha lim_{k→∞} k_{k} = 0 ∀p_{0}.

2

Dalla (??) si ricava facilmente lim_{p_{0}→∞} p_{k} = \frac{r}{k} (quindi lim_{p_{0}→∞} p_{1} = r).

Dalla (??) si ottiene invece lim_{p_{0}→∞} 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} = ∞).

Per capire come questa conoscenza a priori influenzi il comportamento del filtro occorre esaminare più attentamente la (??) in cui si nota come, ad ogni istante, la stima sia ottenuta come combinazione lineare convessa (α + β = 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) (α diminuisce e β 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} = ∞ la stima coincide, come già visto, con la media campionaria.

Fissato il valore di p_{0}, al crescere di k si ha che α diminuisce mentre β aumenta quindi aumentando il numero di misure la stima tende a spostarsi verso la media campionaria indipendentemente da p_{0}.

3

Nei grafici seguenti si riportano gli andamenti ottenuti in simulazione utilizzando x_{0} = 1.9, \overline{x}_{0} = 1.5, p_{0} = 0.5 ed r = 0.4.

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