Main Content

La traducción de esta página aún no se ha actualizado a la versión más reciente. Haga clic aquí para ver la última versión en inglés.

Estimación de estados con el filtro de Kalman de tiempo variante

Este ejemplo utiliza:

Abrir script

Este ejemplo muestra cómo estimar los estados de sistemas lineales con filtros de Kalman de tiempo variante en Simulink®. Puede utilizar el bloque Kalman Filter de la biblioteca de Control System Toolbox™ para estimar la posición y la velocidad de un vehículo terrestre en función de las medidas de posición con ruido, como las medidas de sensores GPS. El modelo de planta del filtro de Kalman tiene características de ruido de tiempo variante.

Introducción

Se desea estimar la posición y la velocidad de un vehículo terrestre en las direcciones norte y este. El vehículo puede desplazarse con libertad por el espacio bidimensional sin restricciones. Se diseña un sistema de navegación y seguimiento polivalente que se puede utilizar en cualquier objeto y no solo en un vehículo.

$x_e(t)$ y $x_n(t)$ son las posiciones este y norte del vehículo con respecto al origen, $\theta(t)$ es la orientación del vehículo con respecto al este y $u_\psi(t)$ es el ángulo de giro del vehículo. $t$ es la variable de tiempo continuo.

El modelo de Simulink consta de dos partes principales: el modelo del vehículo y el filtro de Kalman. Estas se explican más detalladamente en las siguientes secciones.

open_system('ctrlKalmanNavigationExample');

Modelo del vehículo

El vehículo a seguir se representa con un modelo de masa de puntos sencillo:

$$ \frac{d}{dt} \left[
\begin{array} {c}
 x_e(t) \\
 x_n(t) \\
 s(t) \\
 \theta(t)
\end{array} \right] = \left[
\begin{array} {c}
 s(t)\cos(\theta(t)) \\
 s(t)\sin(\theta(t)) \\
 (P \; \frac{u_T(t)}{s(t)} - A \; C_d \; s(t)^2) / m \\
 s(t) \tan(u_\psi(t)) / L
\end{array} \right]
$$

donde los estados del vehículo son:

$$ \begin{array} {ll}
x_e(t) \; & \textnormal{East position} \; [m] \\
x_n(t) \; & \textnormal{North position} \; [m] \\
s(t) \; & \textnormal{Speed} \; [m/s] \\
\theta(t) \; & \textnormal{Orientation from east} \; [deg] \\
\end{array} $$

los parámetros del vehículo son:

$$ \begin{array} {ll}
P=100000 \; & \textnormal{Peak engine power} \; [W] \\
A=1 \; & \textnormal{Frontal area} \; [m^2] \\
C_d=0.3 \; & \textnormal{Drag coefficient} \; [Unitless] \\
m=1250 \; & \textnormal{Vehicle mass} \; [kg] \\
L=2.5 \; & \textnormal{Wheelbase length} \; [m] \\
\end{array} $$

y las entradas de control son:

$$ \begin{array} {ll}
u_T(t) \; & \textnormal{Throttle position in the range of -1 and 1} \; [Unitless] \\
u_\psi(t) \; & \textnormal{Steering angle} \; [deg] \\
\end{array} $$

La dinámica longitudinal del modelo ignora la resistencia a la rodadura de los neumáticos. La dinámica lateral del modelo supone que el ángulo de giro deseado se puede lograr al instante e ignorar el momento de inercia de la guiñada.

El modelo de vehículo se implementa en el subsistema ctrlKalmanNavigationExample/Vehicle Model. El modelo de Simulink contiene dos controladores PI para el seguimiento de la orientación y la velocidad deseadas en el vehículo del subsistema ctrlKalmanNavigationExample/Speed And Orientation Tracking. Esto permite especificar varias condiciones de funcionamiento para el vehículo y probar el rendimiento del filtro de Kalman.

Diseño del filtro de Kalman

El filtro de Kalman es un algoritmo para estimar las variables de interés desconocidas en función de un modelo lineal. Este modelo lineal describe la evolución de las variables estimadas a lo largo del tiempo en respuesta a las condiciones iniciales del modelo, así como las entradas conocidas y desconocidas del modelo. En este ejemplo, se estiman los siguientes parámetros/variables:

$$ \hat{x}[n] = \left[
 \begin{array}{c}
 \hat{x}_e[n] \\
 \hat{x}_n[n] \\
 \hat{\dot{x}}_e[n] \\
 \hat{\dot{x}}_n[n]
 \end{array}
 \right]
$$

donde

$$ \begin{array} {ll}
\hat{x}_e[n] \; & \textnormal{East position estimate} \; [m] \\
\hat{x}_n[n] \; & \textnormal{North position estimate} \; [m] \\
\hat{\dot{x}}_e[n] \; & \textnormal{East velocity estimate} \; [m/s] \\
\hat{\dot{x}}_n[n] \; & \textnormal{North velocity estimate} \; [m/s] \\
\end{array} $$

Los términos $\dot{x}$ expresan las velocidades y no el operador derivativo. $n$ es el índice de tiempo discreto. El modelo utilizado en el filtro de Kalman presenta el siguiente formato:

$$ \begin{array} {rl}
\hat{x}[n+1] &= A\hat{x}[n] + Gw[n] \\
y[n] &= C\hat{x}[n] + v[n]
\end{array} $$

donde $\hat{x}$ es el vector estado, $y$ es la medición, $w$ es el ruido de proceso y $v$ es el ruido de medición. El filtro de Kalman supone que $w$ y $v$ son variables aleatorias independientes con una media de cero y las varianzas conocidas $E[ww^T]=Q$, $E[vv^T]=R$ y $E[wv^T]=N$. Aquí, las matrices A, G y C son:

$$ A = \left[
 \begin{array}{c c c c}
 1 & 0 & T_s & 0 \\
 0 & 1 & 0 & T_s \\
 0 & 0 & 1 & 0 \\
 0 & 0 & 0 & 1
 \end{array}
 \right]
$$

$$ G = \left[
 \begin{array}{c c}
 T_s/2 & 0 \\
 0 & T_s/2 \\
 1 & 0 \\
 0 & 1
 \end{array}
 \right]
$$

$$ C = \left[
 \begin{array}{c c c c}
 1 & 0 & 0 & 0 \\
 0 & 1 & 0 & 0
 \end{array}
 \right]
$$

donde $T_s=1\;[s]$

La tercera fila de A y G modelan la velocidad hacia el este como un recorrido aleatorio: $\hat{\dot{x}}_e[n+1]=\hat{\dot{x}}_e[n]+w_1[n]$. En realidad, la posición es una variable de tiempo continuo y es la integral de la velocidad a lo largo del tiempo $\frac{d}{dt}\hat{x}_e=\hat{\dot{x}}_e$. La primera fila de A y G representan una aproximación discreta a esta relación cinemática: $(\hat{x}_e[n+1]-\hat{x}_e[n])/Ts=(\hat{\dot{x}}_e[n+1]+\hat{\dot{x}}_e[n])/2$. La segunda y cuarta filas de A y G representan la misma relación entre la velocidad hacia el norte y la posición.

La matriz C muestra que solo se dispone de medidas de posición. Un sensor de posición, como GPS, ofrece estas medidas con una tasa de muestreo de 1 Hz. Varianza del ruido de medición $v$, matriz R, se especifica como $R=50$. Puesto que R se especifica como un escalar, el bloque de filtros de Kalman supone que la matriz R es diagonal, que sus diagonales son 50 y que sus dimensiones son compatibles con y. Si el ruido de medición es gaussiano, R = 50 corresponde al 68% de las medidas de posición que se encuentran dentro de $\pm\sqrt{50}\;m$ o la posición real en las direcciones este y norte. Sin embargo, este supuesto no es necesario para el filtro de Kalman.

Los elementos de $w$ capturan cuánto puede cambiar la velocidad del vehículo a lo largo un tiempo de muestreo Ts. Se ha optado por que la varianza del ruido de proceso w, la matriz Q, varíe en el tiempo. Captura la intuición de que los valores típicos de $w[n]$ son más pequeños cuando la velocidad es alta. Por ejemplo, ir de 0 a 10 m/s es más fácil que ir de 10 a 20 m/s. En concreto, se utilizan las velocidades estimadas hacia el norte y el este y una función de saturación para generar Q[n]:

$$f_{sat}(z)=min(max(z,25),625)$$

$$ Q[n] = \left[
 \begin{array}{ c c }
 \displaystyle 1+\frac{250}{f_{sat}(\hat{\dot{x}}_e^2)} & 0 \\
 0 & \displaystyle 1+\frac{250}{f_{sat}(\hat{\dot{x}}_n^2)}
 \end{array}
 \right]
$$

Las diagonales de Q modelan la varianza de w inversamente proporcional al cuadrado de las velocidades estimadas. La función de saturación evita que Q sea demasiado grande o demasiado pequeña. El coeficiente 250 se obtiene a partir de un ajuste con mínimos cuadrados a los datos de tiempo de aceleración de 0-5, 5-10, 10-15, 15-20 y 20-25 m/s en un vehículo genérico. Tenga en cuenta que la selección de la diagonal Q representa el supuesto ingenuo de que los cambios de velocidad en la dirección norte y este no están correlacionados.

Entradas y configuración del bloque Kalman Filter

El bloque Kalman Filter forma parte de la biblioteca de Control System Toolbox de Simulink. También se encuentra en la biblioteca de System Identification Toolbox/Estimators. Configure los parámetros del bloque para obtener una estimación de los estados de tiempo discreto. Especifique los parámetros de Configuración de filtro siguientes:

  • Time domain (Dominio del tiempo): Discrete-time (Tiempo discreto). Seleccione esta opción para estimar estados de tiempo discreto.

  • Seleccione la casilla de verificación Use current measurement y[n] to improve xhat[n] (Usar la medida actual y[n] para mejorar xhat[n]). Esta opción implementa la variante “estimador actual” del filtro de Kalman de tiempo discreto. Esta opción mejora la precisión de la estimación y es más útil para los tiempos de muestreo lentos. Sin embargo, aumenta la carga computacional. Además, esta variante del filtro de Kalman tiene alimentación directa, que provoca un lazo algebraico si se usa el filtro de Kalman en un lazo de feedback que no contenga ningún retardo (el propio lazo de feedback también tiene una vía de paso directa). El lazo algebraico también puede afectar a la velocidad de la simulación.

Haga clic en la pestaña Options (Opciones) para definir las opciones del puerto de entrada y el puerto de salida del bloque:

  • Anule la selección de la casilla de verificación Add input port u (Añadir puerto de entrada u). No hay ninguna entrada conocida en el modelo de planta.

  • Seleccione la casilla de verificación Output state estimation error covariance Z (Covarianza de error de estimación del estado de salida Z). La matriz Z ofrece información acerca de la confianza del filtro en las estimaciones del estado.

Haga clic en Model Parameters (Parámetros del modelo) para especificar las características del modelo de planta y de ruido:

  • Model source (Fuente del modelo): Individual A, B, C, D matrices.

  • A: A. La matriz A ya se ha definido en este ejemplo.

  • C: C. La matriz C ya se ha definido en este ejemplo.

  • Initial Estimate Source (Fuente de la estimación inicial): Dialog (Cuadro de diálogo)

  • Initial states x[0] (Estados iniciales x[0]): 0. Representa una estimación inicial de 0 de las estimaciones de posición y velocidad en t = 0 s.

  • State estimation error covariance P[0] (Covarianza de error de estimación del estado P[0]): 10. Supone que el error entre la estimación inicial x[0] y su valor real es una variable aleatoria con una desviación estándar $\sqrt{10}$.

  • Seleccione la casilla de verificación Use G and H matrices (default G=I and H=0) Usar matrices G y H (valores predeterminados: G = I y H = 0) para especificar una matriz G no predeterminada.

  • G: G. La matriz G ya se ha definido en este ejemplo.

  • H: 0. El ruido de proceso no afecta a las medidas y al introducir el bloque de filtros de Kalman.

  • Anule la selección de la casilla de verificación Time-invariant Q (tiempo invariante Q). La matriz Q varía en el tiempo y se proporciona a través del puerto de entrada del bloque Q. El bloque utiliza un filtro de Kalman variante en el tiempo debido a esta configuración. Puede seleccionar esta opción para utilizar un filtro de Kalman de tiempo invariante. Un filtro de Kalman de tiempo invariante tiene un rendimiento ligeramente peor en este problema, pero es más fácil de diseñar y tiene una carga computacional menor.

  • R: R. Es la covarianza del ruido de medición $v[n]$. La matriz R ya se ha definido en este ejemplo.

  • N: 0. Supone que no existe ninguna correlación entre el ruido del proceso y el ruido de medición.

  • Sample time (-1 for inherited) [Tiempo de muestreo (-1 para heredados)]: Ts, que ya se ha definido en este ejemplo.

Resultados

Pruebe el rendimiento del filtro de Kalman simulando una situación en la que el vehículo realice las siguientes maniobras:

  • En t = 0, el vehículo se encuentra en $x_e(0)=0$, $x_n(0)=0$ y está estacionado.

  • En dirección este, acelera a 25 m/s. Desacelera a 5 m/s en t = 50 s.

  • En t = 100 s, gira hacia el norte y acelera a 20 m/s.

  • En t = 200 s, gira otra vez hacia el oeste. Acelera a 25 m/s.

  • En t = 260 s, desacelera a 15 m/s y gira 180 grados a una velocidad constante.

Simule el modelo de Simulink. Represente gráficamente las estimaciones reales, medidas y del filtro de Kalman de la posición del vehículo.

sim('ctrlKalmanNavigationExample');

figure;
% Plot results and connect data points with a solid line.
plot(x(:,1),x(:,2),'bx',...
    y(:,1),y(:,2),'gd',...
    xhat(:,1),xhat(:,2),'ro',...
    'LineStyle','-');
title('Position');
xlabel('East [m]');
ylabel('North [m]');
legend('Actual','Measured','Kalman filter estimate','Location','Best');
axis tight;

El error entre la posición medida y real así como el error entre la estimación del filtro de Kalman y la posición real es:

% East position measurement error [m]
n_xe = y(:,1)-x(:,1);
% North position measurement error [m]
n_xn = y(:,2)-x(:,2);
% Kalman filter east position error [m]
e_xe = xhat(:,1)-x(:,1);
% Kalman filter north position error [m]
e_xn = xhat(:,2)-x(:,2);

figure;
% East Position Errors
subplot(2,1,1);
plot(t,n_xe,'g',t,e_xe,'r');
ylabel('Position Error - East [m]');
xlabel('Time [s]');
legend(sprintf('Meas: %.3f',norm(n_xe,1)/numel(n_xe)),...
    sprintf('Kalman f.: %.3f',norm(e_xe,1)/numel(e_xe)));
axis tight;
% North Position Errors
subplot(2,1,2);
plot(t,y(:,2)-x(:,2),'g',t,xhat(:,2)-x(:,2),'r');
ylabel('Position Error - North [m]');
xlabel('Time [s]');
legend(sprintf('Meas: %.3f',norm(n_xn,1)/numel(n_xn)),...
    sprintf('Kalman f: %.3f',norm(e_xn,1)/numel(e_xn)));
axis tight;

Las leyendas de las gráficas muestran la medición de la posición y el error de estimación ($||x_e-\hat{x}_e||_1$ y $||x_n-\hat{x}_n||_1$) normalizados por el número de puntos de datos. Las estimaciones del filtro de Kalman presentan un 25% menos de error que las medidas sin procesar.

La velocidad real en dirección este y su estimación del filtro de Kalman se muestra a continuación en la gráfica superior. La gráfica inferior muestra el error de estimación.

e_ve = xhat(:,3)-x(:,3); % [m/s] Kalman filter east velocity error
e_vn = xhat(:,4)-x(:,4); % [m/s] Kalman filter north velocity error
figure;
% Velocity in east direction and its estimate
subplot(2,1,1);
plot(t,x(:,3),'b',t,xhat(:,3),'r');
ylabel('Velocity - East [m/s]');
xlabel('Time [s]');
legend('Actual','Kalman filter','Location','Best');
axis tight;
subplot(2,1,2);
% Estimation error
plot(t,e_ve,'r');
ylabel('Velocity Error - East [m/s]');
xlabel('Time [s]');
legend(sprintf('Kalman filter: %.3f',norm(e_ve,1)/numel(e_ve)));
axis tight;

La leyenda de la gráfica de error muestra el error de estimación de la velocidad en dirección este $||\dot{x}_e-\hat{\dot{x}}_e||_1$ normalizado por el número de puntos de datos.

Las estimaciones de velocidad del filtro de Kalman rastrean las tendencias de la velocidad real correctamente. Los niveles de ruido disminuyen cuando el vehículo se desplaza a velocidades elevadas. Este hecho concuerda con el diseño de la matriz Q. Los dos picos grandes se encuentran en t = 50 s y t = 200 s. Coinciden con el momento en que el vehículo sufre una deceleración repentina y realiza un giro brusco, respectivamente. Los cambios de velocidad en dichos instantes son mucho mayores que las predicciones del filtro de Kalman, que se basa en su entrada en la matriz Q. Tras unos saltos de tiempo, las estimaciones del filtro alcanzan la velocidad real.

Resumen

Ha estimado la posición y la velocidad de un vehículo con el bloque Kalman Filter en Simulink. La dinámica del ruido de proceso del modelo variaba en el tiempo. Ha validado el rendimiento del filtro simulando varias maniobras del vehículo y ha generado aleatoriamente ruido de medición. El filtro de Kalman ha mejorado las medidas de posición y ha proporcionado estimaciones de velocidad del vehículo.

bdclose('ctrlKalmanNavigationExample');

Consulte también

Temas relacionados