Un filtro di Kalman č normalmente sviluppato con un linguaggio testuale del tipo Matlab, C o analoghi. E' possibile sviluppare lo stesso funzionamento anche con un linguaggio di tipo Simulink o LabView.
Un filtro di Kalman č normalmente costruito come una funzione richiamata ciclicamente all'interno di un loop. Nell'esempio che segue, viene mostrato l'esempio di un filtro di Kalman utilizzato per la stima dello stato di un processo del tipo:
x1 = x1 + 0.2 dt x2 + d1
x2 = x2 + d2
dove:
- x1, x2: stati del sistema
- dt : intervallo di campionamento del sistema
- d1, d2: disturbi sullo stato del sistema
La misura a disposizione č x1 e bisogna stimare il valore di x2.
z = x1 + n
dove:
- z: misura dello stato x1
- n: rumore sulla misura
Nel riquadro che segue, sono state utilizzate le seguenti definizioni:
- e1, e2: covarianza dell'incertezza sul valore iniziale dello stato
- q1, q2: covarianza del disturbo sullo stato
- r1: covarianza del rumore sulla misura
Il file di esempio č anche scaricabile dal collegamento che segue:
Per eseguire l'esempio, digitare il comando:
[Xres, Pres, XRres] = Kalman_Loop;
|
function
[Xres, Pres, XRres] = Kalman_Loop() % Definizioni dei valori interni alle matrici del
filtro e1 = 0.16^2; e2 = 0.1^2; q1 =
0.001^2; q2 = 0.001^2; r1 = 0.01^2; dt = 1;
% Inizializzazione dello stato del filtro X = [0; 0]; P = [e1 0;0 e2];
% Definizioni delle matrici del filtro
PHI
= [1 0.2*dt; 0 1];
Q = [q1 0;0 q2];
R =
[r1]; C = [1 0];
% Inizializzazione del vettore dei risultati Xres = []; Pres = [];
% Inizializzazione dello stato reale
Xr = [0.04
0.025];
% Loop per 1000 step
for t=1:dt:1000
%
Lettura delle misure affette da rumore [Z, Xr] = Processo(Xr, dt);
% Esecuzione del filtro [X, P] = KF(Z, X, P, PHI,
C, Q, R);
% Memorizzazione dei risultati XRres(t,:) = [Xr'];
Xres(t,:)
= [X'];
Pres(t,:) = [P(1,1), P(1,2), P(2,1), P(2,2)];
end end
function
[Xn, Pn] = KF(Z, X, P, PHI, C, Q, R)
%
Aggiornamento dello stato Xp = PHI * X;
% Aggiornamento della varianza dell'errore sullo stato
Pp
= PHI * P * PHI' + Q;
%
Calcolo della matrice di guadagno del filtro di Kalman K = Pp * C' * inv(C * Pp * C' + R);
% Aggiornamento della stima dello stato
Xn
= Xp + K * (Z - C * Xp);
%
Aggiornamento della varianza dei disturbi sullo stato e aggiornamento del
ciclo
Pn = (eye(2)
- K * C)* Pp; end function
[Z, Xr] = Processo(Xr, dt)
x1
= Xr(1);
x2 = Xr(2);
x1
= x1 + 0.2 * dt * x2 + 0.001*randn;
x2 = x2 + 0.001*randn;
Xr = [x1; x2];
Z = x1 + 0.01*randn; end |
L'esecuzione del filtro porta ad i seguenti grafici:
Valore da stimare di X2:
Stima di X2:
Alcune semplici istruzioni per mandare in esecuzione il filtro
- copiare il file Kalman_Loop.m
nella directory di lavoro, solitamente c:\matlabx\work
- Digitare il comando che segue nella finestra di comando di Matlab.
[Xres,
Pres, XRres] = Kalman_Loop;
Questa operazione permetterā di generare 3 matrici di cui:
1) Xres: matrice contenente alla riga k-esima il valore dello stato (2 componenti) del filtro al tempo tk
2) Pres: matrice contenente alla riga k-esima il valore delle componenti della matrice di stato al tempo tk (ordinate per colonna nella sequenza [P(1,1), P(1,2), P(2,1), P(2,2)])
3) XRres: matrice contenente alla riga k-esima il valore dello stato del sistema al tempo tk
Il comando inserito eseguirā la prima delle tre funzioni presenti nel file Kalman_Loop.m. Le altre due funzioni (quella del filtro e quella del processo) vengono richiamate all'interno della prima funzione, che riproduce il funzionamento ciclico di una stima di processo.
Per visualizzare i risultati, digitare il comando
plot(Xres(:,1))
per generare un grafico contenente i dati presenti nella prima colonna.
Digitare invece il comando
plot([Xres(:,2), XRres(:,2)])
per disegnare nello stesso grafico i dati presenti nella seconda colonna della matrice Xres
(stato X2 stimato) e nella seconda colonna della matrice XRres (stato X2 del
processo da stimare).
La scelta dei valori della covarianza
e1,
e2, q1, q2, r1 sono ricavati sulla base dei valori scelti per il rumore
sulla misura (r1), per il disturbo sullo stato (q1 e q2) e per l'incertezza
iniziale sul valore dello stato (p1 e p2).
Nell'esempio si ipotizza di avere un rumore di misura con varianza 0.01
Z = x1 + 0.01*randn;
Un'incertezza sullo stato iniziale di 0.16 su x1 e di 0.1 su x2 (anche se
poi lo stato iniziale č stato
fissato a Xr = [0.04 0.025];
Un disturbo sullo stato di varianza pari a 0.001 sia su x1 che su x2:
x1 = x1 + 0.2 * dt * x2 + 0.001*randn;
x2 = x2 + 0.001*randn;
Ackowledges
- Paola Leaci (Universitā di Trento) per il feedback sulla presente pagina.
Questa pagina č stata aggiornata il 06/08/05.