Filtrage Kalman

Cet exemple indique comment effectuer un filtrage Kalman. En premier lieu, vous concevez un filtre d’état stationnaire au moyen de la commande kalman. Puis vous simulez le système pour montrer comment il réduit l’erreur provenant du bruit de mesure. Cet exemple vous montre également comment implémenter un filtre variant dans le temps, qui peut être utile pour les systèmes ayant des sources de bruit non stationnaires.

Filtre de Kalman d’état stationnaire

Considérons le système physique discret suivant avec bruit gaussien w en entrée et bruit de mesure v en sortie :

$$\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’objectif est de concevoir un filtre de Kalman pour estimer la véritable sortie du système physique ${\mathit{y}}_{\mathit{t}}\left[\mathit{n}\right]=\mathit{y}\left[\mathit{n}\right]-\mathit{v}\left[\mathit{n}\right]$ sur la base des mesures bruitées $\mathit{y}\left[\mathit{n}\right]$. Ce filtre de Kalman d’état stationnaire utilise les équations suivantes pour cette estimation.

Mise à jour temporelle :

$\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]$

Mise à jour des mesures :

$$\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}$$

Ici,

• $\hat{\mathit{x}}\left[\mathit{n}|\mathit{n}-1\right]$ est l’estimation de $\mathit{x}\left[\mathit{n}\right]$, compte tenu des mesures jusqu’à $\mathit{y}\left[\mathit{n}-1\right]$.

• $\hat{\mathit{x}}\left[\mathit{n}|\mathit{n}\right]$ et $\hat{\mathit{y}}\left[\mathit{n}|\mathit{n}\right]$ sont les valeurs et mesures d’état estimées, mises à jours compte tenu de la dernière mesure $\mathit{y}\left[\mathit{n}\right]$.

• ${\mathit{M}}_{\mathit{x}}$ et ${\mathit{M}}_{\mathit{y}}$ sont les gains d’innovation optimaux, choisis pour minimiser la covariance d’état stationnaire de l’erreur d’estimation, compte tenu des covariances du bruit $\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}$ et $\mathit{N}=\mathit{E}\left(\mathit{w}\left[\mathit{n}\right]{\mathit{v}\left[\mathit{n}\right]}^{\mathit{T}}\right)=0$. (Pour plus de détails sur la façon dont sont choisis ces gains, consultez kalman.)

(Ces équations de mise à jour décrivent un estimateur de type current. Pour plus d’informations sur la différence entre les estimateurs current et delayed, consultez kalman.)

Design du filtre

Vous pouvez utiliser la fonction kalman pour concevoir ce filtre de Kalman d’état stationnaire. Cette fonction détermine le gain de filtre d’état stationnaire optimal M pour un système physique particulier sur la base de la covariance du bruit de traitement Q et de la covariance du bruit de capteur R que vous fournissez. Pour cet exemple, utilisez les valeurs suivantes pour les matrices de représentation d’état du système physique.

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;

Pour cet exemple, définissez $\mathit{G}=\mathit{B}$, ce qui signifie que le bruit de traitement w est du bruit en entrée additif. Définissez également $\mathit{H}=0$, ce qui signifie que le bruit en entrée w n’a pas d’effet direct sur la sortie y. Ces hypothèses créent un modèle de système physique plus simple :

$$\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}$$

Quand H = 0, il peut être prouvé que ${\mathit{M}}_{\mathit{y}}=\mathit{C}{\mathit{M}}_{\mathit{x}}$ (consultez kalman). Ensemble, ces hypothèses simplifient également les équations de mise à jour pour le filtre de Kalman.

Mise à jour temporelle :

$\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]$

Mise à jour des mesures :

$$\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}$$

Pour concevoir ce filtre, créez d’abord le modèle de système physique avec une entrée pour w. Définissez le pas d’échantillonnage sur -1 pour marquer le système physique comme discret (sans pas d’échantillonnage spécifique).

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

La covariance du bruit de traitement Q et la covariance du bruit du capteur R sont des valeurs supérieures à zéro que vous obtenez généralement à partir d’études ou de mesures de votre système. Pour cet exemple, spécifiez les valeurs suivantes.

Q = 2.3; 
R = 1; 

Utilisez la commande kalman pour concevoir le filtre.

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

Cette commande conçoit le filtre de Kalman, kalmf, un modèle de représentation d’état qui implémente les équations de mise à jour temporelle et de mise à jour des mesures. Les entrées du filtre sont l’entrée du système physique u et la sortie du système physique bruitée y. La première sortie de kalmf est l’estimation $\hat{\mathit{y}}$ de la véritable sortie du système physique et les autres sorties sont les estimations d’état $\hat{\mathit{x}}$.

Pour cet exemple, écartez les estimations d’état et ne gardez que la première sortie $\hat{\mathit{y}}$.

kalmf = kalmf(1,:);

Utiliser le filtre

Pour voir comment fonctionne ce filtre, générez des données et comparez la réponse filtrée avec la véritable réponse du système physique. Le système complet est affiché dans le diagramme suivant.

Pour simuler ce système, utilisez un sumblk pour créer une entrée pour le bruit de mesure v. Utilisez ensuite connect pour joindre sys et le filtre de Kalman entre eux, de sorte que u est une entrée partagée et la sortie de système physique bruitée y alimente l’autre entrée du filtre. Le résultat est un modèle de simulation avec entrées w, v et u et avec sorties yt (réponse véritable) et ye (la réponse filtrée ou estimée $\hat{\mathit{y}}$). Les signaux yt et ye sont respectivement les sorties du système physique et du filtre.

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'});

Pour simuler le comportement du filtre, générez un vecteur d’entrée sinusoïdal connu.

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

Générez des vecteurs de bruit de traitement et de bruit de capteur avec les mêmes valeurs de covariance de bruit Q et R que vous avez utilisées pour concevoir le filtre.

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

Enfin, simulez la réponse au moyen de lsim.

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

lsim génère la réponse aux sorties yt et ye aux entrées appliquées à w, v et u. Extrayez les canaux yt et ye et calculez la réponse mesurée.

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

Comparez la véritable réponse à la réponse filtrée.

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')

Comme le montre le deuxième tracé, le filtre de Kalman réduit l’erreur yt - y due au bruit de mesure. Pour confirmer cette réduction, calculez la covariance de l’erreur avant le filtrage (covariance d’erreur de mesure) et après le filtrage (covariance d’erreur d’estimation).

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

MeasErrCov = 
0.9871

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

EstErrCov = 
0.3479

Design de filtres de Kalman variant dans le temps

Le design précédent supposait que les covariances de bruit ne changent pas au fil du temps. Un filtre de Kalman variant dans le temps peut bien fonctionner même quand la covariance de bruit n’est pas stationnaire.

Le filtre de Kalman variant dans le temps possède les équations de mise à jour suivantes. Dans le filtre variant dans le temps, aussi bien la covariance d’erreur $\mathit{P}\left[\mathit{n}\right]$ que le gain d’innovation ${\mathit{M}}_{\mathit{x}}\left[\mathit{n}\right]$ peuvent varier avec le temps. Vous pouvez modifier les équations de mise à jour temporelle et de mesures pour tenir compte des variations temporelles comme suit, encore une fois avec $\mathit{G}=\mathit{B}$ pour que le bruit de traitement w soit du bruit en entrée additif.

Mise à jour temporelle :

$$\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}$$

Mise à jour des mesures :

$$\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}$$

Consultez kalman pour plus de détails sur ces expressions.

Dans Simulink®, vous pouvez implémenter un filtre de Kalman variant dans le temps au moyen du bloc Kalman Filter (consultez State Estimation Using Time-Varying Kalman Filter).

Pour créer un filtre de Kalman variant dans le temps dans MATLAB®, générez d’abord la réponse du système physique bruitée. Simulez la réponse du système physique au signal d’entrée u et traitez le bruit w défini précédemment. Puis, ajoutez le bruit de mesure v à la réponse véritable simulée yt pour obtenir la réponse bruitée y. Dans cet exemple, les covariances des vecteurs de bruit w et v ne changent pas avec le temps. Cependant, vous pouvez utiliser la même procédure pour le bruit non stationnaire.

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

Ensuite, implémentez les équations de mise à jour de filtre récursives dans une boucle 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

Comparez la véritable réponse à la réponse filtrée.

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')

Le filtre variant dans le temps estime également la covariance de sortie pendant l’estimation. Puisque cet exemple utilise du bruit en entrée stationnaire, la covariance de sortie tend vers une valeur d’état stationnaire. Tracez la covariance de sortie pour confirmer que le filtre a atteint un état stationnaire.

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

Grâce au tracé de covariance, vous pouvez voir que la covariance de sortie atteint un état stationnaire en environ cinq échantillons. À partir de ce moment-là, le filtre variant dans le temps possède la même performance que la version en état stationnaire.

Comme dans le cas de l’état stationnaire, le filtre réduit l’erreur due au bruit de mesure. Pour confirmer cette réduction, calculez la covariance de l’erreur avant le filtrage (covariance d’erreur de mesure) et après le filtrage (covariance d’erreur d’estimation).

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

MeasErrCov = 
0.9871

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

EstErrCov = 
0.3479

Enfin, quand le filtre variant dans le temps atteint l’état stationnaire, les valeurs de la matrice de gain Mxn correspondent à celles calculées par kalman pour le filtre d’état stationnaire.

Mx,Mxn

Mx = 3×1

    0.5345
    0.0101
   -0.4776

Mxn = 3×1

    0.5345
    0.0101
   -0.4776

En savoir plus sur les filtres de Kalman

Pour plus d’informations sur les filtres de Kalman, regardez la vidéo ou regardez le reste de la série sur Understanding Kalman Filters — MATLAB Tech Talks.

Transcription de la vidéo

Voir aussi

kalman

Rubriques

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

Sites web externes

• Comprendre les filtres de Kalman — MATLAB Série de vidéos