ER6.3. Filtro de Kalman lineal para la estimación de la velocidad de un sistema masa-resorte

Introducción (planteamiento)

Dado el siguiente modelo dinámico de un sistema masa-resorte que se mueve a lo largo de un solo eje de coordenadas, simular y generar "datos experimentales" adicionando un ruido a la posición, y estimar la velocidad con un filtro de Kalman a partir de las ecuaciones cinemáticas y la medición de la posición y la aceleración, y analizar los resultados:

$\begin{cases} \dot{x}_1=x_2\\ \dot{x}_2=-\frac{k}{m}x_1-\frac{f}{m}x_2+\frac{u}{m}\\ \end{cases}$

Salida y entrada:

$y=x_1, u=F_{ext}$

Parámetros:

$x_1(0)=0.20,x_2(0)=0, k=0.15~\mathrm{N/m}, f=0.084~\mathrm{N/{m}^2}, m=0.54~\mathrm{kg}$

Método (plan de solución)

Pasos: (i) implementar en MATLAB un filtro de Kalman a partir de las ecuaciones cinemáticas (solo relacionan la posición y la velocidad) si se mide, además, la aceleración del cuerpo; (ii) simular el sistema con una entrada formada por escalones y un seno, y adicionar un ruido blanco gaussiano y un ruido coloreado a la posición y a la aceleración; (iii) estimar el estado completo utilizando diferentes valores de $[\mathbf{P}(0),\mathbf{Q},\mathbf{R}t]$; (iv) implementar el sistema con el filtro en Simulink; (v) analizar los resultados.

Resultados (solución)

El modelo cinemático de tiempo continuo que relaciona la velocidad y posición de un cuerpo que se mueve a lo largo de un solo eje de coordenadas, sin necesidad de conocer el modelo dinámico (en este caso es necesario medir la aceleración), es:

$\left\{ \begin{aligned} &\dot{x}_1=x_2\\ &\dot{x}_2=u\\ &y=x_1\\ \end{aligned} \right.$

Entrada igual a la aceleración (proporcional a la fuerza aplicada):

$u=a$

Modelo en forma matricial:

$\dot{\mathbf{x}}=\left[ \begin{matrix} 0&  1\\ 0&  0\\\end{matrix} \right] \mathbf{x}+\left[ \begin{array}{c} 0\\ 1\\\end{array} \right] u$

$y=\left[ \begin{matrix} 1&  0\\\end{matrix} \right] \mathbf{x}$

El sistema es observable si se mide la posición y no la velocidad:

$\mathbf{C}=\left[ \begin{matrix} 1&  0\\ \end{matrix} \right], \mathrm{rank}\left( \mathbf{M}_o \right) =\mathrm{rank}\left[ \begin{array}{c} \mathbf{C}\\ \mathbf{CA}\\ \end{array} \right] =\mathrm{rank}\left[ \begin{matrix} 1&  0\\ 0&  1\\ \end{matrix} \right] =2$

$\mathbf{C}=\left[ \begin{matrix} 0&  1\\ \end{matrix} \right] , \mathrm{rank}\left( \mathbf{M}_o \right) =\mathrm{rank}\left[ \begin{array}{c} \mathbf{C}\\ \mathbf{CA}\\ \end{array} \right] =\mathrm{rank}\left[ \begin{matrix} 0&  1\\ 0&  0\\ \end{matrix} \right] =1$

Discretización de la ecuación de estado por el método de Euler:

$\left\{ \begin{aligned} &x_1(k+1)=x_1(k)+T_sx_2(k)\\ &x_2(k+1)=x_2(k)+T_su(k)\\ \end{aligned} \right.\\ y=x_1(k)$

Donde, en forma matricial se tiene:

$\mathbf{\Phi }=\left[ \begin{matrix} 1&  T_s\\ 0&  1\\\end{matrix} \right]\\ \mathbf{\Gamma }=\left[ \begin{array}{c} 0\\ T_s\\\end{array} \right]\\  \mathbf{C}=\left[ \begin{matrix} 1&  0\\\end{matrix} \right]$

Con un tiempo de crecimiento de 2.23 seg del sistema, un período de muestreo de 0.2 seg es adecuado.

El filtro de Kalman con $\mathbf{G}=\mathbf{I}$ para los dos casos anteriores se implementa con el siguiente código de MATLAB (entender el código ayuda a asimilar las ideas del filtro de Kalman):

ruido_salida = 1; % 0 (blanco), 1 (coloreado)

ruido_aceleracion = 1; % 0 (blanco), 1 (coloreado)

Av = 1; % Desviación estándar del ruido de la salida

Aa = 0.2; % Desviación estándar del ruido de la aceleración

m = 0.54; k = 0.15; f = 0.084; % Parámetros del sistema masa-resorte

S = ss([0 1; -k/m -f/m],[0; 1/m],[1 0;0 1],[0;0]); 

Ts = 0.2; N = 1000;

t = (0:Ts:(N-1)*Ts)';

rng(1)

e = randn(N,1); 

Hy = tf([1 0.1],[1 0.5],'Ts',Ts); 

v = lsim(Hy,e,t);

u = [ones(1,N/4)  -0.6*ones(1,N/4)  0.8*sin(0.2*(0:Ts:(N/2-1)*Ts))]'; 

x = lsim(S,u,t);

if ruido_salida == 1

    y = x(:,1) + (Av/std(v))*v; 

    texto1 = 'salida con ruido coloreado';

else

    y = x(:,1) + Av*e; 

    texto1 = 'salida con ruido blanco';

end

rng(2)

ea = randn(N,1); 

Ha = tf([1 -0.1],[1 -0.7],'Ts',Ts); 

va = lsim(Ha,ea,t);

a1 = -(k/m)*x(:,1) - (f/m)*x(:,2) + (1/m)*u; % Aceleración

if ruido_aceleracion==1 

    a = a1 + (Aa/std(va))*va; 

    texto2 = 'aceleración con ruido coloreado';

else

    a = a1 + Aa*ea; 

    texto2 = 'aceleración con ruido blanco';

end

% Condiciones y matrices iniciales 

x0 = [0; 0]; 

P0 = 0.01*eye(2); 

Q = 0.01*eye(2); 

R = Av^2; 

% Inicialización de las matrices:

x_act = zeros(2,N); 

P_act_traza = zeros(N,1); 

K_norma = zeros(N,1); 

x_pre = x0; 

P_pre = P0;

Fi = [1 Ts;0 1]; 

Gamma = [0;Ts]; 

C = [1 0]; % Modelo discreto

% Filtro de Kalman

for i = 1:N

    % Corrección del estado

    K = P_pre*C'/(C*P_pre*C' + R); x_act(:,i) = x_pre + K*( y(i) - C*x_pre ); P_act = ( eye(2) - K*C)*P_pre;

    P_act = (P_act + P_act')/2; % Simetrización de la matriz P

    % Actualización del estado

    x_pre = Fi*x_act(:,i) + Gamma*a(i); 

    P_pre = Fi*P_act*Fi' + Q; 

    P_act_traza(i) = trace(P_act);

    K_norma(i) = norm(K); % Norma euclidiana

end

e_est = y - x_act(1,:)'; % Residuos

de_x1 = sqrt(P_act(1,1)); 

de_x2 = sqrt(P_act(2,2)); % Desviaciones estándar a partir de P

figure(1)

sgt = sgtitle(['Q = ' num2str(Q(1,1)) ', R = ' num2str(R(1,1)) ', ' texto1 ', ' texto2]); 

sgt.FontSize = 10;

subplot(2,2,1) % Estado estimado x1 con la desviación estándar a partir de P_act

plot(t,y,'c',t,x(:,1),'b',t,x_act(1,:)'+de_x1,'r--',t,x_act(1,:)'-de_x1,'r--',t,x_act(1,:)','k')

xlabel('t(seg)'), title('x1(t)'), legend({'y' 'x1teo' 'x1+σ' 'x1-σ' 'x1est'},'Location','southwest')

subplot(2,2,2)

plot(t,x(:,2),'b',t,x_act(2,:)'+de_x2,'r--',t,x_act(2,:)'-de_x2,'r--',t,x_act(2,:)','k')

xlabel('t(seg)'), title('x2(t)'), legend({'x2teo' 'x2+σ' 'x2-σ' 'x2est'},'Location','southeast')

subplot(2,2,3)

plot(t,P_act_traza), xlabel('t(seg)'), title('tr(P)')

subplot(2,2,4)

plot(t,K_norma), xlabel('t(seg)'), title('||K||')

figure(2)

whiteness_test(e_est)

Las siguientes figuras muestran los resultados con diferentes valores sobre la diagonal de las matrices R y Q, y diferente tipo de ruido (coloreado o blanco) adicionado a las señales medidas de posición y aceleración (ver el título de cada figura). En todos los casos x(0) = 0 y como se sabe con exactitud que esa posición es correcta, salvo por errores de medición, se tiene siempre que P(0) = 0.01*I. En el gráfico de estimación de $x_2$ se compara con el valor teórico por ser una simulación, dado que en un problema real eso no es siempre posible.

Figura 1 con el valor de R experimental y un valor de Q más pequeño, donde se observa una aceptable estimación a pesar de tener ruidos coloreados:

 

Ampliación del gráfico anterior:

 

Análisis residual:

 

Figura 2 con ruidos blancos, donde hay un leve mejor ajuste al estado teórico y los residuos pasan la prueba de blancura (aumentando la magnitud del ruido de la posición la diferencia con el ruido blanco y coloreado es más notoria):

 

Análisis residual:

 

Figura 3 disminuyendo sustancialmente la matriz Q, donde se observa una menor varianza en la estimación, pero mayor error en la estimación, debido a una norma de K pequeña que da más peso a la predicción que a la corrección:

 

Figura 4 aumentando Q, donde se observa una mayor varianza en la estimación:

 

Figura 5 disminuyendo R, donde se observa un mejor ajuste al estado teórico, pero con más varianza (inexactitud), al punto que no reduce el ruido de la señal:

 

Ampliación del gráfico anterior:

 

Figura 6 con un Q demasiado grande, donde se observa mucha incertidumbre en la estimación y una norma de K cercana a 1 que indica un mayor peso a la corrección que a la predicción:

 

Figura 7 con el diagrama en Simulink utilizando el bloque Kalman Filter para Simulink del Control System Toolbox de MATLAB y un código propio:

 

Parámetros del bloque "Kalman Filter":

   

Subsistema "Filtro de Kalman lineal":

 

Figura 8 con los resultados de Simulink:

Discusión y verificación

Este ejemplo ayuda a entender la razón por la cual un Sistema de Navegación Inercial (INS) o un GPS funcionan para cualquier objeto en movimiento sin importar su masa y otras características dinámicas: todo se basa en la relación cinemática entre posición, velocidad y aceleración; en el INS y GPS las ecuaciones son más complejas y el vector de estado incluye otras variables de estado y entradas, como las posiciones lineales y velocidades en los tres ejes y la medición de aceleraciones con acelerómetros y velocidades angulares con giróscopos.

El filtro de Kalman funciona muy bien a pesar de que el ruido de la posición y la aceleración no son blancos (ver todas las figuras, excepto la figura 2). La figura 2 muestra un leve mejor ajuste si los ruidos son blancos, con incluso una mejor prueba de blancura. 

Las pruebas muestran que es importante seleccionar bien las matrices Q y R para lograr estimación insesgada y con poca incertidumbre. Parece que una buena práctica en este ejemplo es dejar R es su valor experimental y dar valores menores a Q (figuras 1, 2 y 4), pero sin ser tan pequeños que generen estimaciones sesgadas (figura 3) o tan grandes con relación a R que generen estimaciones con mucha varianza (figuras 4, 5 y 6). Dado que se conocía muy bien el estado inicial, se puede iniciar con una matriz P con valores pequeños. 

En todos los casos se observa la convergencia de la traza de P y la norma de K a un valor, lo cual demuestra que el ruido corresponde a un proceso estocástico estacionario. De esta manera, tener una información de P y K ayuda a predecir de manera rápida el estado sin necesidad de partir siempre de un desconocimiento total del comportamiento pasado del sistema.

La implementación en Simulink, tanto con el bloque del Control System Toolbox como uno propio (figura 7), da resultados similares (figura 8) a los del código y puede utilizarse para una mayor interactividad en otros contextos.