Flitro di Kalman: Implementazione

Precedente Home Su

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:

Kalman_Loop.m

 

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.

Leonardo Daga's Warehouseâ, http://leonardodaga.insyde.it
Send any Comments to: leonardo.daga@gmail.com