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

Loading your interactive content...

In this video, we'll discuss why you would use Kalman filters. If you're not familiar with the topic, you may be asking yourself, what is a Kalman filter? Is it a new brand of coffee filter that produces the smoothest-tasting coffee? No, it's not.

A Kalman filter is an optimal estimation algorithm. Today we'll discuss two examples that demonstrate common uses of Kalman filters. In the first example, we'll see how a Kalman filter can be used to estimate a system's state when it's cannot be measured directly.

To illustrate this, let's go to Mars before anyone else does. If your spacecraft's engine can burn fuel at a high enough temperature, it can create thrust that will let you fly to Mars. By the way, according to NASA, liquid hydrogen is a light and powerful rocket propellant that burns with extreme intensity at 5,500 degrees Fahrenheit.

But be careful, because at too-high temperature it can put the mechanical components of the engine at risk, and this can lead to the failure of some of the mechanical parts. If that happens, you might be stuck in your small spacecraft where you've got to eat from tubes. To prevent such a situation, you should closely monitor internal temperature of the combustion chamber. This is not an easy task, since a sensor placed inside the chamber would melt.

Instead, it needs to be placed on a cooler surface close to the chamber. The problem you are facing here is that you want to measure internal temperature of the chamber, but you can't. Instead you have to measure external temperature. In this situation, you can use a Kalman filter to find the best estimate of the internal temperature from an indirect measurement. This way you're extracting information about what you can't measure from what you can.

Now that you know the solution to a problem, you can continue your journey to Mars. But are you scared of traveling in space? Let me tell you this: On Mars, you're going to weigh 62% less than what you weigh on Earth. Still not convinced? Okay, then let's go back and take a look at another scenario that takes place on Earth.

In this example, we're going to see how the Kalman filter can be used to estimate the state of the system by combining measurements from different sources that may be subject to noise. You have guests visiting from overseas, and you need to pick them up from the airport. You're using your car's navigation system. Let's look at the sensors you have on board which help you find your position and navigate you to the airport.

The inertial measurement unit uses accelerometers and gyroscopes to measure the car's acceleration and angular velocity. The odometer measures the relative distance traveled by the car. The GPS receiver receives signals from satellites to locate the car on Earth's surface.

If you live in Boston, as I do, you've got to travel through the Big Dig a very, very long tunnel. And in the tunnel, it gets harder to estimate your position through GPS since the receiver's line of sight to satellites is blocked and GPS signal is weak. In this case, you may want to trust the IMU readings, which give you the acceleration.

However, acceleration itself doesn't tell you much about the car's position. For that you need to take to double the integral of the acceleration. Unfortunately, this operation is prone to drift due to small errors accumulating over time. To get better position estimates, you can use IMU measurements along with odometer readings. But note that odometer measurements may be affected by the tire pressure and road conditions.

To summarize, your sensors measuring the relative position of your car give you fast updates, but they are prone to drift. The GPS receiver provides your absolute location, but it gets updated less frequently and it may be noisy. In this scenario, a Kalman filter can be used to fuse these three measurements to find the optimal estimate of the exact position of the car.

Let's look at some facts about Kalman filters. The Kalman filter is named after Rudolf Kalman, who is the primary developer of this theory. It is an optimal estimation algorithm that predicts a parameter of interests such as location, speed, and direction in the presence of noise and measurements.

Common applications of Kalman filters includes guidance, navigation and control systems, computer vision systems, and signal processing. One of the first applications of Kalman filters was in the 1960s. Do you have any guesses as to what it helped with? Engineers used it in the Apollo project, where the Kalman filter was used to estimate trajectories of the manned spacecraft to the moon and back.

Let's summarize what we've seen in this video. Kalman filters are used to optimally estimate the variables of interests when they can't be measured directly, but an indirect measurement is available. They are also used to find the best estimate of states by combining measurements from various sensors in the presence of noise.

In the next videos, we'll cover what Kalman filters are and how they work. Kalman filtering is a method to design optimal state observers. Therefore, in the next video we'll learn state observers, and then we'll continue our discussion with optimal state estimators.

Lettore video in fase di caricamento.

Riproduci il video

PlaySkip BackwardSkip Forward

Disattiva l’audio

Orario attuale 0:00

/

Durata 6:46

Caricato: 0%

0:00

Tipo di streaming LIVE

Seek to live, currently behind liveLIVE

Tempo rimanente -6:46

 

1x

Velocità di riproduzione

• 2x
• 1.5x
• 1.25x
• 1x, selezionato
• 0.75x
• 0.5x
• 0.25x

Capitolo

• Capitolo

Descrizioni

• Descrizioni disattivate, selezionato

Sottotitoli

• Impostazioni sottotitoli, aprire le impostazioni dei sottotitoli
• Senza sottotitoli, selezionato
• English
• 한국어

Traccia audio

• en (Main), selezionato

Quality Levels

Picture-in-PictureSchermo intero

This is a modal window.

Inizio della finestra di dialogo. Il tasto Esc annullerà l’operazione e chiuderà la finestra.

Testo

ColoreBiancoNeroRossoVerdeBluGialloMagentaCianoOpacitàOpacoSemi-Trasparente

Sfondo testo

ColoreNeroBiancoRossoVerdeBluGialloMagentaCianoOpacitàOpacoSemi-TrasparenteTrasparente

Sfondo area sottotitoli

ColoreNeroBiancoRossoVerdeBluGialloMagentaCianoOpacitàTrasparenteSemi-TrasparenteOpaco

Dimensione dei caratteri

50%75%100%125%150%175%200%300%400%

Stile dei bordi del testo

NessunoRaisedDepressedUniformeOmbra

Carattere

Sans-Serif proporzionaleSans-Serif monospaziatoSerif proporzionaleSerif monospaziatoCasualScriptMaiuscoletto

ReinizializzaFatto

Chiudi la finestra di dialogo

Fine della finestra di dialogo.

Chiudi la finestra di dialogo

This is a modal window. Questa finestra di dialogo può essere chiusa premendo sul tasto Esc o attivando il pulsante di chiusura.

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