El filtro de Kalman es un algoritmo clásico de predicción y seguimiento que permite realizar estimaciones óptimas del estado de un sistema en presencia de ruido e interferencias. Su aplicación se extiende ampliamente en campos como navegación, guiado y control.
1. Un escenario práctico
Imaginemos que hemos desarrollado un dron (llamémoslo Alex) para realizar entregas urbanas. Este dron está equipado con varios sensores que nos proporcionan información sobre su estado.
Podemos conocer su velocidad v (componentes en los ejes x, y, z del espacio tridimensional), y también cuenta con sistemas GPS, barómetros y otros dispositivos que nos indican su posición p (coordenadas geográficas, altitud, etc.). En otras palabras, podemos observar en tiempo real el estado de Alex.
Podemos representar el estado de Alex en un momento determinado como un vector:
Incertidumbre y correlación
Aunque tenemos cierta certeza sobre el estado actual de Alex, el sistema siempre contendrá errores, tanto de cálculo como de detección por los sensores. Por lo tanto, solo podemos considerar que el estado actual es la mejor estimación del estado real.
Podemos asumir que el estado actual de Alex sigue una distribución gaussiana, como se muestra en la siguiente figura:
El centro de la distribución gaussiana corresponde al punto μ en la imagen.
Para la varianza (representada por el área de la elipse en la figura), como tenemos dos variables, podemos usar una matriz de covarianza P para representarla:
Por lo tanto, el estado real de Alex podría encontrarse dentro del área de la elipse de la figura, con la mayor probabilidad en el centro.
2. Predicción del siguiente estado del sistema y su error
A continuación, utilizando el estado actual de Alex y conocimientos de física, podemos predecir su siguiente estado. Mediante principios físicos simples, a partir de la posición y velocidad en el momento k-1, podemos estimar el estado en el siguiente instante:
En forma matricial, esto se expresa como:
Aquí, Fk es la matriz de transición de estado. El error del sistema de Alex se representa mediante la matriz de covarianza. Según las propiedades de la matriz de covarianza:
Entonces, el error predicho en el siguiente estado de Alex sería:
- Consideración del control del sistema
Para permitir que Alex llegue a cualquier lugar, es necesario controlarlo, por ejemplo, acelerando o desacelerando. Supongamos que en un momento determinado aplicamos una aceleración a de magnitud a, entonces la posición y velocidad en el siguiente instante debería ser:
Por lo tanto, nuestra ecuación de predicción de estado se actualiza a:
En esta nueva ecuación, Bk se denomina matriz de control del estado, y uk es el vector de control del estado. El primero indica cómo la aceleración o desaceleración cambia el estado de Alex, mientras que el segundo indica la magnitud y dirección del control.
4. Consideración de influencias externas del sistema
Sin embargo, existen muchos factores externos que pueden hacer que nuestro control sobre Alex no siempre sea como deseamos. A veces hay viento en contra, otras a favor.
Suponemos que los factores externos de incertidumbre causan un error en el estado de Alex que sigue una distribución gaussiana con media cero. Así obtenemos la ecuación completa de predicción de estado en el filtro de Kalman:
Como la media es cero, algunos artículos podrían omitirla, pero si sabemos que la media no es cero, debemos tener cuidado. Esto depende del escenario específico de la aplicación, y comprender el principio permite apreciar los cambios en cada componente.
5. ¿Qué deberíamos observar en este momento?
Hemos realizado una predicción cuidadosa del estado actual de Alex basándonos en su estado anterior. Ahora debemos considerar qué deberíamos observar con los diversos sensores instalados en Alex.
El estado actual de Alex y los datos observados por los sensores deberían tener una relación específica, supongamos que esta relación se representa mediante una matriz H, como se muestra en la figura:
Dada la predicción que hicimos del estado de Alex, deberíamos observar un valor de sensor de:
De esta manera, hemos completado la predicción del valor observado, que sigue la siguiente distribución gaussiana:
6. Consideración de los resultados de observación reales
Hemos predicho no solo el estado actual de Alex, sino también los datos del sensor que deberíamos observar. Sin embargo, siempre existe una diferencia entre la realidad y lo ideal. Nuestra predicción del resultado observado y el resultado real podrían verse como en la siguiente figura:
La figura muestra el resultado observado real z, pero este resultado también es inexacto, por lo que consideramos que el ruido de observación sigue una distribución gaussiana con media cero y matriz de covarianza R, es decir:
En otras palabras, nuestro valor observado de Alex sigue una distribución gaussiana, y la situación real debería estar dentro de una elipse con centro en z, es decir, el resultado de la observación sigue una distribución gaussiana:
Finalmente llegamos al paso más crucial: lo más importante y fundamental que hace el filtro de Kalman es fusionar los resultados de la predicción y la observación, aprovechando la incertidumbre de ambos para obtener una estimación más precisa.
En términos sencillos, se trata de cómo obtener la distribución gaussiana de la parte amarilla pálida de las dos elipsas anteriores. Parece ser la superposición de las distribuciones gaussianas de predicción y observación, es decir, la parte con mayor probabilidad.
7. Producto de dos funciones de densidad de probabilidad gaussianas
Una distribución gaussiana unidimensional se representa mediante una función de densidad de probabilidad gaussiana, que se asemeja a un sombrero de ala ancha cuando se traza en un eje de coordenadas.
A continuación, mostramos el resultado intuitivo del producto de dos funciones de densidad de probabilidad gaussianas.
En comparación con la función de densidad de probabilidad gaussiana estándar, el resultado del producto es una nueva función de densidad de probabilidad gaussiana multiplicada por un coeficiente específico (este coeficiente se calculará en el código de demostración posterior), y podemos resolver para obtener la media y la varianza de esta nueva distribución gaussiana como:
Podemos usar MATLAB para calcular el producto de dos funciones de densidad de probabilidad gaussianas, y la nueva función de densidad de probabilidad gaussiana calculada mediante las fórmulas anteriores. Aquí está el código relevante y la captura de pantalla:
clear all
x=-1:0.01:2.5;
mu0 = 0.3;
mu1 = 0.8;
sigma0 = 0.2;
sigma1 = 0.5;
sigma0_sq = 0.04;
sigma1_sq = 0.25;
y1=normpdf(x,mu0,sigma0);
y2=normpdf(x,mu1,sigma1);
y3=y1.*y2;
k = sigma0_sq / (sigma0_sq + sigma1_sq);
mu = (mu0*sigma1_sq + mu1*sigma0_sq) / (sigma0_sq + sigma1_sq);
sigma = sqrt((sigma0_sq * sigma1_sq) / (sigma0_sq + sigma1_sq));
scale = (1.0 / (sqrt(2*pi*(sigma0_sq + sigma1_sq)))) * exp(-1.0 * ((mu0-mu1)^2/(2.0 * (sigma0_sq + sigma1_sq))));
y4 = normpdf(x,mu,sigma);% * scale;
figure;
plot(x,y1,x,y2,x,y3, x, y4,'MarkerSize',20,'LineWidth',5);
grid;
tip1 = sprintf('(\\mu_0,\\sigma_0) = (%.4f, %.4f)', mu0, sigma0);
tip2 = sprintf('(\\mu_1,\\sigma_1) = (%.4f, %.4f)', mu1, sigma1);
tip3 = '(\mu_0,\sigma_0)*(\mu_1,\sigma_1)';
tip4 = sprintf('(\\mu^\\prime,\\sigma^\\prime) = (%.4f, %.4f)', mu, sigma);
legend({tip1, tip2, tip3, tip4});
Captura de ejecución:
La función normpdf de MATLAB utiliza parámetros de media y desviación estándar, por lo que el código y la captura de pantalla también usan media y desviación estándar para representar una distribución gaussiana.
El producto directo de las dos formas de onda azul y naranja es la forma de onda amarilla, y en realidad se puede obtener multiplicando la forma de onda púrpura por un coeficiente, que es la variable scale en el código antreior, con la fórmula de cálculo ya proporcionada.
Si al calcular y4 (la forma de onda púrpura) multiplicas por este coeficiente, descubrirás que su forma de onda coincide exactamente con la forma de onda amarilla (y3). Solo necesita modificar las líneas correspondientes:
y4 = normpdf(x,mu,sigma) * scale;
- La nueva distribución gaussiana
Ahora centrémonos en esta nueva función de densidad de probabilidad en el producto. De hecho, describe una nueva distribución gaussiana, que es precisamente la estimación óptima que busca el filtro de Kalman. En las fórmulas de cálculo de la nueva media y varianza, hacemos:
Entonces podemos obtener:
Escribiéndolos en forma matrizial:
Ya hemos obtenido las dos distribuciones gaussianas para los resultados de predicción y observación:
Por lo tanto, podemos realizar el siguiente derivado para obtener la ecuación de cálculo del filtro de Kalman para la estimación óptima del estado actual (basada en predicción y observación):
Bueno, simplificando ambos lados,注意K se puede expandir, por lo que obtenemos:
Aquí, K' es la famosa ganancia de Kalman:
9. Pasos de cálculo en la implementación práctica
Al utilizar el filtro de Kalman en la práctica, los pasos de cálculo generalmente son (aquí se omiten los subíndices, porque en la implementación, incluso si los subíndices son diferentes, en realidad usamos una sola variable, preste atención para comparar con las ecuaciones anteriores):
Aquí, y es el residual de medición, y S es la matriz de covarianza del residual de medición.
Lo más importante es que debemos prestar atención constante a las variables del sistema que se iteran continuamente: el estado del sistema x, su matriz de covarianza de error P, y la ganancia de Kalman K.
En la aplicación práctica, la selección de Q y R debe basarse en la situación real. Se pueden realizar ajustes continuos para encontrar la solución óptima, o pueden ser variables, siempre y cuando el resultado final sea mejor.
- Dificultades principales en la práctica
10.1. Establecimiento del modelo cinemático
Es decir, la ecuación de predicción. Por un lado, esto se puede hacer a través de una identificación de sistema más detallada, lo que requiere combinarse con el contexto de aplicación específico. También es donde generalmente se comete más errores.
10.2. Características estadísticas del ruido
Obtenemos 2 (o más) piezas de información que informan sobre la misma cosa A y B, pero no sabemos cuál creer. Por lo tanto, simplemente, sumamos las dos piezas de información y las dividimos por 2 (promedio ponderado), obteniendo un resultado 0.5A+00.5B. Pero este método es demasiado bruto y no refleja la calidad. Ahora sabemos que una de las piezas de información es más confiable que la otra, por lo que debemos confiar más en la información buena, por ejemplo, si damos un mayor peso a la confiable B, obtenemos 0.2A+0.8B, este resultado será mejor que el primero. Pero todavía parece simple, porque los parámetros de A y B se establecen según la experiencia, no sabemos si son los más razonables o si son óptimos. Por lo tanto, un método avanzado utiliza métodos de optimización, según las características de la distribución gaussiana de A y B, para fusionar la información de A y B. Esta es precisamente la idea del filtro de Kalman.
Porque no conocemos el error de modelado. La ecuación de observación todavía se puede medir para calcular la media y la varianza. Luego viene el etapa favorita de todos: la de ajustar parámetros. Se dice que hay muchas formas creativas de hacerlo.
Referencias
Resumen del algoritmo del filtro de Kalman ¿Cómo se utiliza el filtro de Kalman en proyectos de ingeniería reales? ¿Cómo se utiliza el filtro de Kalman en proyectos de ingeniería reales?