Filtraggio di Kalman

Questo esempio mostra come eseguire il filtraggio di Kalman. Per prima cosa, progettare un filtro a stato stazionario utilizzando il comando kalman. Quindi, simulare il sistema per mostrare come riduce l'errore dovuto al rumore di misurazione. Questo esempio mostra inoltre come implementare un filtro a tempo variabile, che può risultare utile per sistemi con sorgenti di rumore non stazionarie.

Filtro di Kalman allo stato stazionario

Si consideri il seguente impianto discreto con rumore gaussiano w in input e rumore di misurazione v in output:

$$\begin{array}{c}x\left[n+1\right]=Ax\left[n\right]+Bu\left[n\right]+Gw\left[n\right]\\ y\left[n\right]=Cx\left[n\right]+Du\left[n\right]+Hw\left[n\right]+v\left[n\right]\end{array}$$

L'obiettivo è progettare un filtro di Kalman per stimare l'output effettivo dell'impianto ${\mathit{y}}_{\mathit{t}}\left[\mathit{n}\right]=\mathit{y}\left[\mathit{n}\right]-\mathit{v}\left[\mathit{n}\right]$ basato sulle misurazioni rumorose $\mathit{y}\left[\mathit{n}\right]$. Per tale stima, questo filtro di Kalman a stato stazionario utilizza le seguenti equazioni.

Aggiornamento del tempo:

$\hat{\mathit{x}}\left[\mathit{n}+1|\mathit{n}\right]=\mathit{A}\hat{\mathit{x}}\left[\mathit{n}|\mathit{n}-1\right]+\mathit{Bu}\left[\mathit{n}\right]+\mathit{Gw}\left[\mathit{n}\right]$

Aggiornamento della misurazione:

$$\begin{array}{l}\underset{}{\overset{ˆ}{x}}\left[n|n\right]=\underset{}{\overset{ˆ}{x}}\left[n|n-1\right]+M_x\left(y\left[n\right]-C\underset{}{\overset{ˆ}{x}}\left[n|n-1\right]-Du\left[n\right]\right)\\ \underset{}{\overset{ˆ}{y}}\left[n|n\right]=C\underset{}{\overset{ˆ}{x}}\left[n|n-1\right]+Du\left[n\right]+M_y\left(y\left[n\right]-C\underset{}{\overset{ˆ}{x}}\left[n|n-1\right]-Du\left[n\right]\right)\end{array}$$

In questo caso,

• $\hat{\mathit{x}}\left[\mathit{n}|\mathit{n}-1\right]$ è la stima di $\mathit{x}\left[\mathit{n}\right]$, considerando le misurazioni precedenti date sino a $\mathit{y}\left[\mathit{n}-1\right]$.

• $\hat{\mathit{x}}\left[\mathit{n}|\mathit{n}\right]$ e $\hat{\mathit{y}}\left[\mathit{n}|\mathit{n}\right]$ sono i valori di stato e di misurazione stimati, aggiornati in base all'ultima misurazione $\mathit{y}\left[\mathit{n}\right]$.

• ${\mathit{M}}_{\mathit{x}}$ e ${\mathit{M}}_{\mathit{y}}$ sono i guadagni ottimali dell'innovazione, scelti per minimizzare la covarianza allo stato stazionario dell'errore di stima, considerando le covarianze di rumore date $\mathit{E}\left(\mathit{w}\left[\mathit{n}\right]{\mathit{w}\left[\mathit{n}\right]}^{\mathit{T}}\right)=\mathit{Q}$, $\mathit{E}\left(\mathit{v}\left[\mathit{n}\right]{\mathit{v}\left[\mathit{n}\right]}^{\mathit{T}}\right)=\mathit{R}$ e $\mathit{N}=\mathit{E}\left(\mathit{w}\left[\mathit{n}\right]{\mathit{v}\left[\mathit{n}\right]}^{\mathit{T}}\right)=0$. (Per i dettagli su come vengono scelti questi guadagni, vedere kalman.)

(Queste equazioni di aggiornamento descrivono uno stimatore di tipo current. Per informazioni sulla differenza tra gli stimatori current e gli stimatori delayed, vedere kalman.)

Progettazione del filtro

È possibile utilizzare la funzione kalman per progettare questo filtro di Kalman allo stato stazionario. Questa funzione determina il guadagno ottimale del filtro allo stato stazionario M per un particolare impianto in base alla covarianza del rumore di processo Q e alla covarianza del rumore del sensore R che vengono forniti. Per questo esempio, utilizzare i seguenti valori per le matrici stato-spazio dell'impianto.

A = [1.1269   -0.4940    0.1129 
     1.0000         0         0 
          0    1.0000         0];

B = [-0.3832
      0.5919
      0.5191];

C = [1 0 0];

D = 0;

Per questo esempio, impostare $\mathit{G}=\mathit{B}$, a indicare che il rumore di processo w è un rumore di input additivo. Impostare inoltre $\mathit{H}=0$, a indicare che il rumore di input w non ha alcun effetto diretto sull'output y. Queste ipotesi producono un modello di impianto più semplice:

$$\begin{array}{c}x\left[n+1\right]=Ax\left[n\right]+Bu\left[n\right]+Bw\left[n\right]\\ y\left[n\right]=Cx\left[n\right]+v\left[n\right]\end{array}$$

Quando H = 0, è possibile dimostrare che ${\mathit{M}}_{\mathit{y}}=\mathit{C}{\mathit{M}}_{\mathit{x}}$ (vedere kalman). Insieme, queste ipotesi semplificano inoltre le equazioni di aggiornamento per il filtro di Kalman.

Aggiornamento del tempo:

$\hat{\mathit{x}}\left[\mathit{n}+1|\mathit{n}\right]=\mathit{A}\hat{\mathit{x}}\left[\mathit{n}|\mathit{n}-1\right]+\mathit{Bu}\left[\mathit{n}\right]+\mathit{Bw}\left[\mathit{n}\right]$

Aggiornamento della misurazione:

$$\begin{array}{l}\underset{}{\overset{ˆ}{x}}\left[n|n\right]=\underset{}{\overset{ˆ}{x}}\left[n|n-1\right]+M_x\left(y\left[n\right]-C\underset{}{\overset{ˆ}{x}}\left[n|n-1\right]\right)\\ \underset{}{\overset{ˆ}{y}}\left[n|n\right]=C\underset{}{\overset{ˆ}{x}}\left[n|n\right]\end{array}$$

Per progettare questo filtro, creare prima il modello dell'impianto con un input per w. Impostare il tempo di campionamento su -1 per contrassegnare l'impianto come discreto (senza un tempo di campionamento specifico).

Ts = -1;
sys = ss(A,[B B],C,D,Ts,'InputName',{'u' 'w'},'OutputName','y');  % Plant dynamics and additive input noise w

La covarianza del rumore di processo Q e la covarianza del rumore del sensore R sono valori maggiori di zero che si ottengono solitamente da studi o misurazioni del sistema. Per questo esempio, specificare i seguenti valori.

Q = 2.3; 
R = 1; 

Utilizzare il comando kalman per progettare il filtro.

[kalmf,L,~,Mx,Z] = kalman(sys,Q,R);

Questo comando progetta il filtro di Kalman kalmf, un modello stato-spazio che implementa le equazioni di aggiornamento del tempo e di aggiornamento della misurazione. Gli input del filtro sono l'input dell'impianto u e l'output rumoroso dell'impianto y. Il primo output di kalmf è la stima $\hat{\mathit{y}}$ dell'output effettivo dell'impianto, mentre gli output rimanenti sono le stime di stato $\hat{\mathit{x}}$.

Per questo esempio, scartare le stime di stato e mantenere solo il primo output $\hat{\mathit{y}}$.

kalmf = kalmf(1,:);

Utilizzo del filtro

Per vedere come funziona questo filtro, generare alcuni dati e confrontare la risposta filtrata con la risposta effettiva dell'impianto. Il sistema completo è mostrato nel diagramma seguente.

Per simulare questo sistema, utilizzare un sumblk per creare un input per il rumore di misurazione v. Quindi, utilizzare connect per unire sys e il filtro di Kalman in modo tale che u sia un input condiviso e l'output rumoroso dell'impianto y alimenti l'altro input del filtro. Il risultato è un modello di simulazione con input w, v e u e output yt (risposta effettiva) e ye (risposta filtrata o stimata $\hat{\mathit{y}}$). I segnali yt e ye sono rispettivamente gli output dell'impianto e del filtro.

sys.InputName = {'u','w'};
sys.OutputName = {'yt'};
vIn = sumblk('y=yt+v');

kalmf.InputName = {'u','y'};
kalmf.OutputName = 'ye';

SimModel = connect(sys,vIn,kalmf,{'u','w','v'},{'yt','ye'});

Per simulare il comportamento del filtro, generare un vettore di input sinusoidale noto.

t = (0:100)';
u = sin(t/5);

Generare vettori di rumore di processo e di rumore del sensore utilizzando gli stessi valori di covarianza del rumore Q e R, utilizzati per progettare il filtro.

rng(10,'twister');
w = sqrt(Q)*randn(length(t),1);
v = sqrt(R)*randn(length(t),1);

Infine, simulare la risposta utilizzando lsim.

out = lsim(SimModel,[u,w,v]);

lsim genera la risposta agli output yt e ye agli input applicati a w, v e u. Estrarre i canali yt e ye e calcolare la risposta misurata.

yt = out(:,1);   % true response
ye = out(:,2);  % filtered response
y = yt + v;     % measured response

Confrontare la risposta effettiva con quella filtrata.

clf
subplot(211), plot(t,yt,'b',t,ye,'r--'), 
xlabel('Number of Samples'), ylabel('Output')
title('Kalman Filter Response')
legend('True','Filtered')
subplot(212), plot(t,yt-y,'g',t,yt-ye,'r--'),
xlabel('Number of Samples'), ylabel('Error')
legend('True - measured','True - filtered')

Come mostra il secondo grafico, il filtro di Kalman riduce l'errore yt - y dovuto al rumore di misurazione. Per confermare questa riduzione, calcolare la covarianza dell'errore prima del filtraggio (covarianza dell'errore di misurazione) e dopo il filtraggio (covarianza dell'errore di stima).

MeasErr = yt - y;
MeasErrCov = sum(MeasErr.*MeasErr)/length(MeasErr)

MeasErrCov = 
0.9871

EstErr = yt - ye;
EstErrCov = sum(EstErr.*EstErr)/length(EstErr)

EstErrCov = 
0.3479

Progettazione di un filtro di Kalman a tempo variabile

Il progetto precedente presupponeva che le covarianze del rumore non cambiassero nel tempo. Un filtro di Kalman a tempo variabile può offrire buone prestazioni anche quando la covarianza del rumore non è stazionaria.

Il filtro di Kalman a tempo variabile dispone delle seguenti equazioni di aggiornamento. Nel filtro a tempo variabile, sia la covarianza dell'errore $\mathit{P}\left[\mathit{n}\right]$ che il guadagno dell'innovazione ${\mathit{M}}_{\mathit{x}}\left[\mathit{n}\right]$ possono variare nel tempo. È possibile modificare le equazioni di aggiornamento del tempo e della misurazione per tenere conto della variazione del tempo come segue, considerando nuovamente $\mathit{G}=\mathit{B}$ in modo che il rumore di processo w sia un rumore di input additivo.

Aggiornamento del tempo:

$$\begin{array}{l}\underset{}{\overset{ˆ}{x}}\left[n+1|n\right]=A\underset{}{\overset{ˆ}{x}}\left[n|n\right]+Bu\left[n\right]+Bw\left[n\right]\\ P\left[n+1|n\right]=AP\left[n|n\right]A^T+BQ{B}^T\end{array}$$

Aggiornamento della misurazione:

$$\begin{array}{l}\underset{}{\overset{ˆ}{x}}\left[n|n\right]=\underset{}{\overset{ˆ}{x}}\left[n|n-1\right]+M_x\left[n\right]\left(y\left[n\right]-C\underset{}{\overset{ˆ}{x}}\left[n|n-1\right]\right)\\ M_x\left[n\right]=P\left[n|n-1\right]C^T{\left(CP\left[n|n-1\right]C^T+R\left[n\right]\right)}^{-1}\\ P\left[n|n\right]=\left(I-M_x\left[n\right]C\right)P\left[n|n-1\right]\\ \underset{}{\overset{ˆ}{y}}\left[n|n\right]=C\underset{}{\overset{ˆ}{x}}\left[n|n\right]\end{array}$$

Per maggiori dettagli su queste espressioni, vedere kalman.

In Simulink®, è possibile implementare un filtro di Kalman a tempo variabile utilizzando il blocco Kalman Filter (vedere State Estimation Using Time-Varying Kalman Filter).

Per creare il filtro di Kalman a tempo variabile in MATLAB®, per prima cosa generare la risposta rumorosa dell'impianto. Simulare la risposta dell'impianto al segnale di input u e al rumore di processo w definiti in precedenza. Quindi, aggiungere il rumore di misurazione v alla risposta effettiva simulata yt per ottenere la risposta rumorosa y. In questo esempio, le covarianze dei vettori di rumore w e v non cambiano con il tempo. Tuttavia, è possibile utilizzare la stessa procedura per il rumore non stazionario.

yt = lsim(sys,[u w]);   
y = yt + v;         

Successivamente, implementare le equazioni di aggiornamento del filtro ricorsivo in un loop for.

P = B*Q*B';         % Initial error covariance
x = zeros(3,1);     % Initial condition on the state

ye = zeros(length(t),1);
ycov = zeros(length(t),1); 
errcov = zeros(length(t),1); 

for i=1:length(t)
  % Measurement update
  Mxn = P*C'/(C*P*C'+R);
  x = x + Mxn*(y(i)-C*x);   % x[n|n]
  P = (eye(3)-Mxn*C)*P;     % P[n|n]

  ye(i) = C*x;
  errcov(i) = C*P*C';

  % Time update
  x = A*x + B*u(i);        % x[n+1|n]
  P = A*P*A' + B*Q*B';     % P[n+1|n] 
end

Confrontare la risposta effettiva con quella filtrata.

subplot(211), plot(t,yt,'b',t,ye,'r--')
xlabel('Number of Samples'), ylabel('Output')
title('Response with Time-Varying Kalman Filter')
legend('True','Filtered')
subplot(212), plot(t,yt-y,'g',t,yt-ye,'r--'),
xlabel('Number of Samples'), ylabel('Error')
legend('True - measured','True - filtered')

Il filtro a tempo variabile stima inoltre la covarianza di output durante la stima. Poiché questo esempio utilizza un rumore di input stazionario, la covarianza di output tende a un valore allo stato stazionario. Tracciare la covarianza di output per confermare che il filtro ha raggiunto uno stato stazionario.

figure
plot(t,errcov) 
xlabel('Number of Samples'), ylabel('Error Covariance'),

Dal grafico di covarianza, è possibile constatare che la covarianza di output raggiunge uno stato stazionario in circa cinque campioni. Da quel momento in poi, il filtro a tempo variabile ha le stesse prestazioni della versione a stato stazionario.

Come nel caso dello stato stazionario, il filtro riduce l'errore dovuto al rumore di misurazione. Per confermare questa riduzione, calcolare la covarianza dell'errore prima del filtraggio (covarianza dell'errore di misurazione) e dopo il filtraggio (covarianza dell'errore di stima).

MeasErr = yt - y;
MeasErrCov = sum(MeasErr.*MeasErr)/length(MeasErr)

MeasErrCov = 
0.9871

EstErr = yt - ye;
EstErrCov = sum(EstErr.*EstErr)/length(EstErr)

EstErrCov = 
0.3479

Infine, quando il filtro a tempo variabile raggiunge lo stato stazionario, i valori nella matrice di guadagno Mxn corrispondono a quelli calcolati da kalman per il filtro allo stato stazionario.

Mx,Mxn

Mx = 3×1

    0.5345
    0.0101
   -0.4776

Mxn = 3×1

    0.5345
    0.0101
   -0.4776

Ulteriori informazioni sui filtri di Kalman

Per ulteriori informazioni sui filtri di Kalman, riprodurre il video o guardare il resto della serie su Understanding Kalman Filters — MATLAB Tech Talks (Comprendere i filtri di Kalman - Discussioni tecniche di MATLAB).

Trascrizione video

Vedi anche

kalman

Argomenti

• State Estimation Using Time-Varying Kalman Filter
• LQG Regulation: Rolling Mill Case Study

Siti web esterni

• Comprendere i filtri di Kalman — Serie di video di MATLAB