Main Content

Filtro de Kalman

Este ejemplo muestra cómo realizar un filtrado de Kalman. En primer lugar, diseñe un filtro de estado estacionario con el comando kalman. A continuación, simule el sistema para mostrar cómo reduce los errores de ruido en las mediciones. Este ejemplo también muestra cómo implementar un filtro variante en el tiempo, que puede resultar útil para sistemas con fuentes de ruido no estacionario.

Filtro de Kalman de estado estacionario

Considere la siguiente planta discreta con ruido gaussiano w en la entrada y ruido en las mediciones v en la salida:

x[n+1]=Ax[n]+Bu[n]+Gw[n]y[n]=Cx[n]+Du[n]+Hw[n]+v[n]

El objetivo es diseñar un filtro de Kalman para estimar la verdadera salida de la planta yt[n]=y[n]-v[n] en función de las mediciones con ruido y[n]. Este filtro de Kalman de estado estacionario utiliza las siguientes ecuaciones para esta estimación.

Actualización del tiempo:

xˆ[n+1|n]=Axˆ[n|n-1]+Bu[n]+Gw[n]

Actualización de la medición:

xˆ[n|n]=xˆ[n|n-1]+Mx(y[n]-Cxˆ[n|n-1]-Du[n])yˆ[n|n]=Cxˆ[n|n-1]+Du[n]+My(y[n]-Cxˆ[n|n-1]-Du[n])

En este caso,

  • xˆ[n|n-1] es la estimación de x[n], dadas las mediciones anteriores de hasta y[n-1].

  • xˆ[n|n]y yˆ[n|n] son los valores de estado y las mediciones estimadas, actualizadas de acuerdo con la última medición y[n].

  • Mx y My son las ganancias de innovación óptimas, elegidas para minimizar la covarianza de estado estacionario del error de estimación, dadas las covarianzas de ruido E(w[n]w[n]T)=Q , E(v[n]v[n]T)=R y N=E(w[n]v[n]T)=0. (Para obtener más información sobre cómo se eligen estas ganancias, consulte kalman).

(Estas ecuaciones de actualización describen un estimador de tipo current. Para obtener información sobre la diferencia entre los estimadores current y delayed, consulte kalman).

Diseñar el filtro

Puede utilizar la función de kalman para diseñar este filtro de Kalman de estado estacionario. Esta función determina la ganancia óptima del filtro de estado estacionario M para una planta concreta en función de la covarianza de ruido del proceso Q y la covarianza de ruido del sensor R que proporcione. Para este ejemplo, utilice los siguientes valores para las matrices de espacio de estados de la planta.

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;

Para este ejemplo, establezca G=B, lo que significa que el ruido del proceso w es ruido de entrada añadido. Asimismo, establezca H=0, lo que significa que el ruido de entrada w no tiene un efecto directo sobre la salida y. Estos supuestos ofrecen un modelo de planta más sencillo:

x[n+1]=Ax[n]+Bu[n]+Bw[n]y[n]=Cx[n]+v[n]

Cuando H = 0, se puede mostrar que My=CMx (consulte kalman). Estos supuestos juntos también simplifican las ecuaciones de actualización para el filtro de Kalman.

Actualización del tiempo:

xˆ[n+1|n]=Axˆ[n|n-1]+Bu[n]+Bw[n]

Actualización de la medición:

xˆ[n|n]=xˆ[n|n-1]+Mx(y[n]-Cxˆ[n|n-1])yˆ[n|n]=Cxˆ[n|n]

Para diseñar este filtro, cree primero el modelo de planta con una entrada para w. Establezca el tiempo de muestreo en -1 para marcar la planta como discreta (sin un tiempo de muestreo específico).

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

La covarianza de ruido del proceso Q y la covarianza de ruido del sensor R son valores mayores que cero que normalmente se obtienen a partir de estudios o de mediciones del sistema. Para este ejemplo, especifique los siguientes valores.

Q = 2.3; 
R = 1; 

Utilice el comando kalman para diseñar el filtro.

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

Este comando diseña el filtro de Kalman kalmf, un modelo de espacio de estados que implementa las ecuaciones de actualización del tiempo y de actualización de las mediciones. Las entradas del filtro son la entrada de la planta u y la salida de la planta con ruido y. La primera salida de kalmf es la estimación yˆ de la salida de la planta verdadera y las salidas restantes son las estimaciones de estado xˆ.

kalmdemo2.png

Para este ejemplo, descarte las estimaciones de estado y conserve únicamente la primera salida yˆ.

kalmf = kalmf(1,:);

Utilizar el filtro

Para ver cómo funciona este filtro, genere algunos datos y compare la respuesta filtrada con la respuesta de la planta verdadera. En el siguiente diagrama se muestra el sistema completo.

kalmdemo1.png

Para simular este sistema, utilice sumblk para crear una entrada para el ruido de las mediciones v. A continuación, utilice connect para unir sys con el filtro de Kalman, de modo que u sea una entrada compartida y la salida de la planta con ruido y se introduce en la otra entrada del filtro. El resultado es un modelo de simulación con las entradas w, v y u, y las salidas yt (respuesta verdadera) e ye (la respuesta filtrada o estimada yˆ). Las señales yt e ye son las salidas de la planta y del filtro respectivamente.

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

Para simular el comportamiento del filtro, genere un vector de entrada sinusoidal conocido.

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

Genere vectores de ruido de proceso y ruido de sensor utilizando los mismos valores de covarianza de ruido Q y R que utilizó para diseñar el filtro.

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

Por último, simule la respuesta mediante lsim.

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

lsim genera la respuesta en las salidas yt e ye para las entradas aplicadas en w, v y u. Extraiga los canales yt e ye y calcule la respuesta medida.

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

Compare la respuesta verdadera con la respuesta filtrada.

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

Figure contains 2 axes objects. Axes object 1 with title Kalman Filter Response contains 2 objects of type line. These objects represent True, Filtered. Axes object 2 contains 2 objects of type line. These objects represent True - measured, True - filtered.

Tal como muestra la segunda gráfica, el filtro Kalman reduce el error yt - y debido al ruido en las mediciones. Para confirmar esta reducción, calcule la covarianza del error antes de filtrar (covarianza del error de mediciones) y tras filtrar (covarianza del error de estimación).

MeasErr = yt-yt;
MeasErrCov = sum(MeasErr.*MeasErr)/length(MeasErr)
MeasErrCov = 0
EstErr = yt-ye;
EstErrCov = sum(EstErr.*EstErr)/length(EstErr)
EstErrCov = 0.3479

Diseño del filtro de Kalman variante en el tiempo

En el diseño anterior, se suponía que las covarianzas de ruido no cambiaban a lo largo del tiempo. Un filtro de Kalman variante en el tiempo puede obtener buenos resultados, incluso cuando la covarianza de ruido no es estacionaria.

El filtro de Kalman variante en el tiempo tiene las siguientes ecuaciones de actualización. En el filtro variante en el tiempo, tanto la covarianza de error P[n] como la ganancia de innovación Mx[n] pueden variar en el tiempo. Puede modificar las ecuaciones de actualización del tiempo y de las mediciones para tener en cuenta la variación de tiempo de la siguiente manera. (Consulte kalman para obtener más información sobre estas expresiones).

Actualización del tiempo:

xˆ[n+1|n]=Axˆ[n|n]+Bu[n]+Bw[n]P[n+1|n]=AP[n|n]AT+BQBT

Actualización de la medición:

xˆ[n|n]=xˆ[n|n-1]+Mx[n](y[n]-Cxˆ[n|n-1])Mx[n]=P[n|n-1]CT(CP[n|n-1]CT+R[n])-1P[n|n]=(I-Mx[n]C)P[n|n-1]yˆ[n|n]=Cxˆ[n|n]

Puede implementar un filtro de Kalman variante en el tiempo en Simulink® mediante el bloque Kalman Filter. Para ver un ejemplo sobre el uso de ese bloque, consulte Estimación de estados con el filtro de Kalman variante en el tiempo. Para este ejemplo, implemente el filtro variante en el tiempo en MATLAB®.

Para crear el filtro de Kalman variante en el tiempo, genere, en primer lugar, la respuesta de la planta con ruido. Simule la respuesta de la planta para la señal de entrada u y el ruido de proceso w definidos anteriormente. A continuación, añada el ruido en las mediciones v a la respuesta verdadera simulada yt para obtener la respuesta con ruido y. En este ejemplo, las covarianzas de los vectores de ruido w y v no cambian con el tiempo. No obstante, puede utilizar el mismo procedimiento para el ruido no estacionario.

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

A continuación, implemente las ecuaciones de actualización recurrentes en un bucle 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

Compare la respuesta verdadera con la respuesta filtrada.

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

Figure contains 2 axes objects. Axes object 1 with title Response with Time-Varying Kalman Filter contains 2 objects of type line. These objects represent True, Filtered. Axes object 2 contains 2 objects of type line. These objects represent True - measured, True - filtered.

El filtro variante en el tiempo también estima la covarianza de salida durante la estimación. Dado que este ejemplo utiliza ruido de entrada de estado estacionario, la covarianza de salida tiende a un valor de estado estacionario. Represente la covarianza de salida para confirmar que el filtro ha alcanzado de estado estacionario.

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

Figure contains an axes object. The axes object contains an object of type line.

A partir de la gráfica de covarianza, puede ver que la covarianza de salida alcanza de estado estacionario en unas cinco muestras. A partir de entonces, el filtro variante en el tiempo tiene el mismo rendimiento que la versión de estado estacionario.

Como en el caso del estado estacionario, el filtro reduce el error debido al ruido en las mediciones. Para confirmar esta reducción, calcule la covarianza del error antes de filtrar (covarianza del error de mediciones) y tras filtrar (covarianza del error de estimación).

MeasErr = yt - y;
MeasErrCov = sum(MeasErr.*MeasErr)/length(MeasErr)
MeasErrCov = 0.9871
EstErr = yt - ye;
EstErrCov = sum(EstErr.*EstErr)/length(EstErr)
EstErrCov = 0.3479

Por último, cuando el filtro variante en el tiempo alcanza de estado estacionario, los valores en la matriz de ganancia Mxn se corresponden con los que se han calculado para el filtro de estado estacionario kalman.

Mx,Mxn
Mx = 3×1

    0.5345
    0.0101
   -0.4776

Mxn = 3×1

    0.5345
    0.0101
   -0.4776

Consulte también

Temas relacionados

Sitios web externos