PSC: Progettazione di sistemi di controllo  I Sem. 2010

Lezione 5 — Ottobre 22

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

5.1 Equazioni filtro di Kalman

Nei prossimi esempi si vedranno delle ulteriori applicazioni delle equazioni del filtro di Kalman in forma di informazione che per comodità vengono qui di seguito riassunte:

x̂_{k+1|k+1} = Ax̂_{k|k} + K_{k+1} (y_{k+1} − CAx̂_{k|k})  (5.1)
P_{k+1|k} = AP_{k|k}A^{T} + Q  (5.2)
P_{k+1|k+1} = P_{k+1|k} − P_{k+1|k}C^{T} (CP_{k+1|k}C^{T} + R)^{−1} CP_{k+1|k}  (5.3)
K_{k+1} = P_{k+1|k}C^{T} (CP_{k+1|k}C^{T} + R)^{−1}  (5.4)

con condizioni iniziali:

P_{0|−1} = P_{0} x̂_{0} = x̄_{0}  (5.5)

Si continua l’esempio 4 della lezione precedente e si prende in considerazione il vettore delle misure:

\begin{aligned}y_{i} = [x_{i} 1] \underbrace{[\begin{matrix}a \\ b \end{matrix}]}_{θ} +v_{i} , & i = 1, …, N \\ & v_{i} ∼ N (0, σ^{2}) \end{aligned}

Ponendoci nel caso di stima di un vettore θ costante (A = I, Q = 0, P_{0} = ∞) e ponenendo y = [y_{1} ⋯ y_{N}]^{T}, con R = σ^{2}I la stima diventa:

θ̂ = x̂_{0|0} = (C^{T} C)^{−1}C^{T} y

Nel caso di rumori con varianze diverse e possibilmente correlati ovvero, R ≠ σ^{2}I

si ottiene:

θ̂ = x̂_{0|0} = (C^{T} R^{−1}C)^{−1}C^{T} R^{−1}y

chiamato ‘stimatore ai minimi quadrati pesati’.

PSC  Lezione 5 — Ottobre 22  I Sem. 2010

5-1 Esempio 5) Si considerino le matrici

C = [ C_{1} ; ⋮ ; C_{N} ] ∈ ℝ^{N×2}, R = [\begin{matrix}R_{1} & & & & 0 \\ & . & . & . \\ 0 & & & & R_{N} \end{matrix}  ]

Si vuole vedere come si scrive lo stimatore ai minimi quadrati pesati in questa situazione. Supponendo che la matrice C sia composta dai vettori riga C_{i} si ottiene:

C^{T} R^{−1}C = [C_{1}^{T} ⋯ C_{N}^{T}] [\begin{matrix}R_{1} & & & & 0 \\ & . & . & . \\ 0 & & & & R_{N} \end{matrix}  ] [ C_{1} ; ⋮ ; C_{N} ] = ∑_{i=1}^{N} C_{i}^{T} R_{i}^{−1}C_{i}

e dunque ciò che si ottiene è che l’incertezza complessiva è data dalla somma delle incertezze di ogni misura. Vale poi:

C^{T} R^{−1}y = ∑_{i=1}^{N} C_{i}R_{i}^{−1}y_{i}

e dunque lo stimatore finale è:

θ̂ = (∑_{i=1}^{N} C_{i}^{T} R_{i}^{−1}C_{i})^{−1}(∑_{i=1}^{N} C_{i}R_{i}^{−1}y_{i})

Moltiplicando e dividendo per 1/N si ha:

θ̂ = (1/N ∑_{i=1}^{N} C_{i}^{T} R_{i}^{−1}C_{i})^{−1}(1/N ∑_{i=1}^{N} C_{i}R_{i}^{−1}y_{i})

Calcolando le due medie precedenti si ottiene la stima cercata che verrá chiamata ‘stimatore ai minimi quadrati distribuita’ e vale:

θ̂ ∼ N ( θ, (∑_{i=1}^{N} C_{i}^{T} R_{i}^{−1}C_{i})^{−1} )

per cui é ancora uno stimatore corretto.

5-2 Esempio 6)[Stima ai minimi quadrati iterattiva]

Si prende ora in considerazione il modello precedente (con lo stesso significato delle variabili)

y_{k} = c_{k}θ + v_{k}, v_{k} ∼ N (0, R_{k}), K = 1, …

ma in questo caso il pedice k sta a significare che le misure non arrivano più tutte assieme ma in maniera sequenziale. Anche in questo caso il parametro non noto resta costante nel tempo, e dunque si ha: A = I, Q = 0, P_{0} = ∞ per cui anche P_{1|0} = P_{0|0} = ∞. Con questa inizializzazione l’algoritmo del filtro di Kalman in forma di informazione diventa:

\begin{matrix}x̂_{k|k} = (∑_{i=1}^{k} C_{i}^{T} R_{i}^{−1}C_{i})^{−1}(∑_{i=1}^{N} C_{i}^{T} R_{i}^{−1}y_{i}) \\ P_{k|k} = (∑_{i=1}^{k} C_{i}^{T} R_{i}^{−1}C_{i})^{−1} = (P_{k−1|k−1}^{−1} + C_{k}^{T} R_{k}^{−1}C_{k})^{−1} \end{matrix}

si osservi come la varianza dell’errore di stima diminuisca al crescere di k, e in particolare diminuisce in maniera pesata lungo alcune direzioni. Questa può essere riscritta come:

P_{k|k}^{−1} = P_{k−1|k−1}^{−1} + C_{k}^{T} R_{k}^{−1}C_{k}

e dunque lo stimatore diventa:

x̂_{k|k} = (P_{k−1|k−1}^{−1} + C_{k}^{T} R_{k}^{−1}C_{k})^{−1}(P_{k−1|k−1}^{−1} x̂_{k−1|k−1} + C_{k}^{T} R_{k}^{−1}y_{k})

ad ogni passo è necessario quindi invertire la matrice P, questo è il prezzo che viene pagato nel fare il conto iterativamente. Nel caso in cui l’inversa non esistesse si possono adottare due soluzioni. La prima consiste nel prendere P_{0} = MI con M ≫ 1 in modo tale che l’inversa esista già dal primo passo. Nel secondo caso si aspetta qualche passo fino a che il termine in sommatoria diventi definito positivo e dal quel punto si inizia ad usare l’algoritmo di stima. La differenza fra le due soluzioni è che la secondo soluzione non da delle stime nei primi passi. Quando peró da dei risultati quelli teoricamente sono piú vicini al valore vero rispetto alle stime ottenute usando la prima soluzione.

Esempio 7)[Stima ai minimi quadrati dinamica o adattiva]

Puó succedere che il parametro θ cambia nel tempo. Si puó far ripartire l’algoritmo con il nuovo valore di θ oppure usare un’algoritmo che tiene conto dei possibili cambiamenti di θ. Un modello molto semplice che tiene conto di questi possibili cambiamenti è il seguente:

x_{k+1} = x_{k} + w_{k}, w_{k} ∼ N (0, qI).

Il nuovo θ è quindi uguale a quello precedente a cui viene sommato il rumore. Se il parametro q è piccolo si ha che θ cambia di poco. Il filtro userá quindi molte misure in modo da filtrare il rumore. Si hanno peró dei transitori di stima lenti. Se q è invece grande il filtro ha un transitorio di stima veloce e le stime saranno peró soggette a piú rumore.

5-3