ER6.4. Filtro extendido de Kalman conjunto

Introducción (planteamiento)

Dado el siguiente modelo lineal de un sistema masa-resorte, simular y generar "datos experimentales" adicionando un ruido a la variable de posición, implementar un filtro de Kalman conjunto para la estimación del estado y un parámetro, 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}$

Condiciones iniciales y 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) simular el sistema con una entrada RBS y adicionar un ruido blanco gaussiano y un ruido coloreado a la posición; (ii) implementar en MATLAB un filtro de Kalman conjunto para la estimación del estado y la masa a partir de las ecuaciones dinámicas; (iii) analizar los resultados.

Resultados (solución)

La ecuación de estado no lineal con la masa como variable de estado $x_3=m$ (la tercera ecuación de estado indica que la masa es constante) es:

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

La linealización entrega los siguientes resultados:

$\mathbf{A}=\frac{\partial f}{\partial \mathbf{x}}=\left[ \begin{matrix} 0&  1&  0\\ -\frac{k}{x_3}&  -\frac{f}{x_3}&  \frac{1}{x_{3}^{2}}(kx_1+fx_2-u)\\ 0&  0&  0\\ \end{matrix} \right]$

Aproximación de la matriz $\mathbf{\Phi }$: 

$\mathbf{\Phi }\simeq \mathbf{I}+\mathbf{A}T_s$

Discretizando por el método de Euler:

$\begin{cases} x_1(k+1)=x_1(k)+T_sx_2(k)\\ x_2(k+1)=x_2(k)-\frac{T_s}{x_3(k)}[ kx_1(k) + fx_2(k)-u(k)]\\ x_3(k+1)=x_3(k)\\ \end{cases}$

Un período de muestreo adecuado, a partir del tiempo de crecimiento, es igual a 0.2 seg. 

Dado que el modelo es no lineal, es necesario implementar un filtro extendido de Kalman. El siguiente código de MATLAB implementa el filtro de Kalman extendido conjunto (con los parámetros obtenidos por tanteo) para el modelo anterior con un solo parámetro $m$:

ruido_salida = 0; % 0 (blanco), 1 (coloreado)
Av = 1; % Desviación estándar del ruido de la salida
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); 
H = tf([1 0.1],[1 0.5],'Ts',Ts); 
v = lsim(H,e,t);
rng(2)
u = idinput([N, 1, 1], 'RBS', [0, 1/10], [-1,1]); 
x = lsim(S,u,t);
if ruido_salida == 1
    y = x(:,1) + (Av/std(v))*v; 
    texto = 'salida con ruido coloreado';
else
    y = x(:,1) + Av*e; 
    texto = 'salida con ruido blanco';
end
x0 = [0; 0; 0.2]; 
P0 = 0.01*eye(3); 
Q = 0.001*eye(3); 
R = 10;
% Inicialización de las matrices
x_act = zeros(3,N); 
P_act_traza = zeros(N,1); 
K_norma = zeros(N,1); 
N_cond = zeros(N,1);
de_x1 = zeros(N,1); 
de_x2= zeros(N,1); 
de_x3 = zeros(N,1); 
x_pre = x0; 
P_pre = P0; C = [1 0 0];
% Filtro extendido 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) - x_pre(1) ); 
    P_act = ( eye(3) - K*C)*P_pre;
    P_act = (P_act + P_act')/2; % Simetrizacion de la matriz P
    % Actualización del estado
    Fi = eye(3)+Ts*[0 1 0; -k/x_act(3,i) -f/x_act(3,i) (1/x_act(3,i)^2)*(k*x_act(1,i)+f*x_act(2,i)-u(i));0 0 0];
   x_pre=[x_act(1,i)+Ts*x_act(2,i); x_act(2,i)-(Ts/x_act(3,i))*(k*x_act(1,i)+f*x_act(2,i) - u(i)); x_act(3,i)]; 
    P_pre = Fi*P_act*Fi' + Q;  
    P_act_traza(i) = trace(P_act); 
    K_norma(i) = norm(K); 
    N_cond(i) = cond(obsv(Fi,C));
    de_x1(i) = sqrt(P_act(1,1)); 
    de_x2(i) = sqrt(P_act(2,2)); 
    de_x3(i) = sqrt(P_act(3,3));
end
e_est = y - x_act(1,:)'; % Residuos
de_x1 = sqrt(P_act(1,1)); 
de_x2 = sqrt(P_act(2,2)); 
de_x3 = sqrt(P_act(3,3));
figure('Units','normalized','Position',[0 0 0.5 0.5])
sgt = sgtitle(['Q = ' num2str(Q(1,1)) ', R = ' num2str(R(1,1)) ', ' texto]); 
sgt.FontSize = 15;
subplot(2,3,1)
plot(t,y,'c', t,x_act(1,:)','k', t,x_act(1,:)'+de_x1,'r--',t,x_act(1,:)'-de_x1,'r--')
xlabel('t(seg)'), title('x1(t)'), legend({'x1exp','x1est'})
subplot(2,3,2)
plot(t,x_act(2,:)','k', t,x_act(2,:)'+de_x2,'r--',t,x_act(2,:)'-de_x2,'r--'), xlabel('t(seg)'), title('x2(t)')
subplot(2,3,3)
plot(t,x_act(3,:)','k', t,m+t*0,'b', t,x_act(3,:)'+de_x3,'r--',t,x_act(3,:)'-de_x3,'r--')
xlabel('t(seg)'), title('m')
subplot(2,3,4)
plot(t,K_norma), xlabel('t(seg)'), title('||K||')
subplot(2,3,5)
plot(t,P_act_traza), xlabel('t(seg)'), title('tr(P)') 
figure('Units','normalized','Position',[0 0 0.5 0.5])
whiteness_test(e_est) 
figure('Units','normalized','Position',[0 0 0.5 0.5])
plot(t,log10(N_cond)), xlabel('t'), ylabel('log_{10}(Ncond)')
title('Número de condición de la matriz de observabilidad')

Figura 1 con la estimación de las variables de estado y el parámetro $m$ con un filtro de Kalman conjunto (se utilizó un ruido blanco gaussiano), donde se observa una buena estimación, pero con mucha varianza, incluso con una masa inicial de 0.2 kg (diferente del valor exacto de 0.54 kg):

La figura 2 muestra la prueba de blancura (ver explicación en el libro), la cual no es la esperada, debido a que las simulaciones se hicieron con un ruido blanco:

 

La Figura 3 muestra el número de condición de la matriz de observabilidad en escala logarítmica, donde se observa una disminución drástica de la observabilidad en muchos instantes de muestreo (en el peor de los casos es mayor que $10^4$):

 

Discusión y verificación

En el filtro de Kalman conjunto propuesto se estima un solo parámetro, la masa en el denominador de la ecuación de estado, el cual lleva a un modelo no lineal que exige la aplicación de filtro extendido de Kalman (EKF). Los resultados son aceptables, pero el principal problema es el número de condición tan alto de la matriz de observabilidad, de más de $10^4$ (figura 3), lo que conlleva a una varianza muy grande del orden de la masa estimada (figura 1). 

Con dos parámetros el aumento del número de condición es aún mayor. Este problema está muy bien identificado en la literatura científica y tiene varias explicaciones. Por lo anterior, se recomienda utilizar mejor el filtro de Kalman dual (DKF), el cual no se explica en el libro. 

La prueba de blancura no es completamente concluyente (figura 2) debido posiblemente al mayor efecto de la masa en el comportamiento no lineal del modelo, donde la distribución gaussiana no se transforma siempre en otra distribución gaussiana en todos los puntos de operación del sistema, violando los supuestos del filtro de Kalman.