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 :
L’objectif est de concevoir un filtre de Kalman pour estimer la véritable sortie du système physique sur la base des mesures bruitées . Ce filtre de Kalman d’état stationnaire utilise les équations suivantes pour cette estimation.
Mise à jour temporelle :
Mise à jour des mesures :
Ici,
est l’estimation de , compte tenu des mesures jusqu’à .
et sont les valeurs et mesures d’état estimées, mises à jours compte tenu de la dernière mesure .
et 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 , et . (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 , ce qui signifie que le bruit de traitement w est du bruit en entrée additif. Définissez également , 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 :
Quand H = 0, il peut être prouvé que (consultez kalman
). Ensemble, ces hypothèses simplifient également les équations de mise à jour pour le filtre de Kalman.
Mise à jour temporelle :
Mise à jour des mesures :
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 de la véritable sortie du système physique et les autres sorties sont les estimations d’état .
Pour cet exemple, écartez les estimations d’état et ne gardez que la première sortie .
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 ). 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 que le gain d’innovation 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 pour que le bruit de traitement w soit du bruit en entrée additif.
Mise à jour temporelle :
Mise à jour des mesures :
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.