Filtro kalman en una dimensión
Este capítulo describe el filtro de Kalman en una dimensión. El objetivo principal de este capítulo es explicar el concepto de filtro de Kalman de una manera simple e intuitiva sin utilizar herramientas matemáticas que pueden parecer complejas y confusas.
Vamos a avanzar hacia las ecuaciones del filtro de Kalman paso a paso.
- Primero, vamos a obtener las ecuaciones del filtro de Kalman para un ejemplo simple, sin el ruido del proceso.
- En segundo lugar, agregaremos el ruido del proceso.
Filtro de Kalman unidimensional sin ruido de proceso
Como mencioné anteriormente, el filtro de Kalman se basa en cinco ecuaciones. Ya estamos familiarizados con dos de ellos:
- Las ecuaciones de actualización de estado.
- Las ecuaciones del modelo dinámico.
En este capítulo, vamos a obtener otras tres ecuaciones de filtro de Kalman.
Recordemos nuestro primer ejemplo (medición del peso de la barra de oro), realizamos múltiples mediciones y calculamos la estimación promediando las lecturas.
Hemos llegado al siguiente resultado:
En el gráfico anterior, se puede ver el valor verdadero, el valor estimado y las mediciones, en relación al número de mediciones.
Las diferencias entre las mediciones (muestras azules) y el valor verdadero (línea verde) son errores de medición. Como los errores de medición son aleatorios, podemos describirlos por varianza ( \( \sigma ^{2} \) ). El proveedor de la balanza puede proporcionar la variación de los errores de medición o puede obtenerse mediante un procedimiento de calibración. La varianza de los errores de medición es en realidad la incertidumbre de medición.
Denotaremos la incertidumbre de medición por \( r \) .
La diferencia entre la estimación (la línea roja) y el valor verdadero (línea verde) es el error de estimación. Como puede ver, el error estimado se hace cada vez más pequeño a medida que hacemos más mediciones, y converge hacia cero, mientras que el valor estimado converge hacia el valor verdadero. No sabemos cuál es el error de estimación, pero podemos estimar la incertidumbre en la estimación.
Denotaremos la incertidumbre estimada por \( p \) .
Echemos un vistazo al PDF de medidas de peso (Función de densidad de probabilidad)
En la siguiente gráfica podemos ver diez medidas del peso de la barra de oro.
- Las medidas están descritas por la línea azul.
- El verdadero valor es descrito por la línea roja discontinua.
- La línea verde describe la función de densidad de probabilidad de la medición.
- El área verde en negrita es la desviación estándar ( \( \sigma \) ) de la medición, es decir, hay una probabilidad de 68.26% de que el valor verdadero se encuentre dentro de esta área.
Como puede ver, 8 de cada 10 mediciones están lo suficientemente cerca del valor verdadero, por lo que el valor verdadero se encuentra dentro de los límites de \( 1 \sigma \).
La incertidumbre de medición ( \( r \) ) es la varianza de la medición ( \( \sigma ^{2} \) ).
La ecuación de ganancia de Kalman en 1d
Vamos a obtener la tercera ecuación, que es la ecuación de ganancia de Kalman. En este momento, presentaré la derivación intuitiva de la ecuación de ganancia de Kalman. La derivación matemática se mostrará en los siguientes capítulos.
En un filtro de Kalman, los parámetros \( \alpha \) -\( \beta \) (-\( \gamma \) ) se calculan dinámicamente para cada iteración del filtro. Estos parámetros se denominan Ganancia de Kalman y denotados por \( K_{n} \).
La ecuación de ganancia de Kalman es la siguiente:
| \( p_{n,n-1} \) | es la incertidumbre de la estimación extrapolada |
| \( r_{n} \) | es la incertidumbre de la medición |
La ganancia de Kalman es un número entre cero y uno:
\[ 0 \leq K_{n} \leq 1 \]
Reescribamos la ecuación de actualización de estado:
Como se puede ver, la Ganancia de Kalman \( \left( K_{n} \right) \) es el peso que le damos a la medición. Y \( \left( 1-K_{n} \right) \) es el peso que le damos a la estimación.
Cuando la incertidumbre de medición es muy grande y la incertidumbre estimada es muy pequeña, la ganancia de Kalman es cercana a cero. Por lo tanto, le damos un gran peso a la estimación y un pequeño peso a la medición.
Por otro lado, cuando la incertidumbre de medición es muy pequeña y la incertidumbre estimada es muy grande, la ganancia de Kalman es cercana a uno. Por lo tanto, le damos un pequeño peso a la estimación y un gran peso a la medición.
Si la incertidumbre de la medición es igual a la incertidumbre estimada, entonces la ganancia de Kalman es igual a 0.5.
La ecuación de ganancia de Kalman es la tercera ecuación de filtro de Kalman.
La actualización de incertidumbre estimada en 1d
La siguiente ecuación define la actualización de incertidumbre estimada:
| \( K_{n} \) | es la ganancia de Kalman |
| \( p_{n,n-1} \) | es la incertidumbre estimada que se calculó durante la estimación previa |
| \( p_{n,n} \) | es la incertidumbre estimada del estado actual |
Esta ecuación actualiza la incertidumbre estimada del estado actual. Se llama la ecuación de actualización de covarianza. ¿Por qué covarianza? Lo veremos en los siguientes capítulos.
IDe la ecuación queda bastante claro que la incertidumbre estimada siempre se hace más pequeña con cada iteración de filtro, ya que \( \left( 1-K_{n} \right) \leq 1 \) . Cuando la incertidumbre de medición es grande, la ganancia de Kalman será baja, por lo tanto, la convergencia de la incertidumbre estimada sería lenta. Sin embargo, cuando la incertidumbre de medición es pequeña, la ganancia de Kalman será alta y la incertidumbre estimada convergería rápidamente hacia cero.
La ecuación de actualización de covarianza es la cuarta ecuación de filtro de Kalman.
La extrapolación de incertidumbre estimada en 1d
Al igual que la extrapolación de estado, la extrapolación de incertidumbre estimada se realiza con las ecuaciones del modelo dinámico.
En nuestro segundo ejemplo, en un caso de radar unidimensional, la posición prevista del objetivo es:
Es decir, la posición predicha es igual a la posición estimada actual más la velocidad estimada actual multiplicada por el tiempo. La velocidad predicha es igual a la estimación de velocidad actual (suponiendo el modelo de velocidad constante).
La extrapolación de incertidumbre estimada sería:
| \( p^{x} \) | es la incertidumbre estimada para la posición |
| \( p^{v} \) | es la incertidumbre estimada para la velocidad |
Es decir, la incertidumbre estimada de la posición predicha es igual a la incertidumbre estimada de la posición actual más la incertidumbre estimada de la velocidad actual multiplicada por el tiempo al cuadrado. La incertidumbre estimada de la velocidad prevista es igual a la incertidumbre estimada de la velocidad actual (suponiendo el modelo de velocidad constante).
En nuestro primer ejemplo (medición del peso de la barra de oro) la dinámica del sistema es constante. Por lo tanto, la extrapolación de incertidumbre estimada sería:
| \( p \) | es la estimación de la incertidumbre del peso de la barra de oro |
La ecuación de extrapolación de incertidumbre estimada se llama ecuación de extrapolación de covarianza y ésta es la quinta ecuación del filtro de Kalman.
Poniendo todo junto
En este capítulo, vamos a combinar todas las piezas en un solo algoritmo. Al igual que el filtro \( \alpha \) , \( \beta \), (\( \gamma \) ) el filtro Kalman utiliza el algoritmo "Medir, Actualizar, Predecir".
El siguiente cuadro proporciona una descripción esquemática de bajo nivel del algoritmo:
Las entradas del filtro son:
- Inicialización
- Estado inicial del sistema ( \( \hat{x}_{1,0} \) )
- Incertidumbre inicial del estado ( \( p_{1,0} \) )
- Medición
- Medición del estado del sistema ( \( z_{n} \) )
- Incertidumbre de medida ( \( r_{n} \) )
La inicialización se realiza solo una vez y proporciona dos parámetros:
Los parámetros de inicialización pueden ser proporcionados por otro sistema, otro proceso (por ejemplo, proceso de búsqueda en radar) o conjeturas basadas en la experiencia o el conocimiento teórico. Incluso si los parámetros de inicialización no son precisos, el filtro de Kalman podrá converger cerca del valor real.
La medición se realiza para cada ciclo de filtro y proporciona dos parámetros:
Además del valor medido, el filtro Kalman requiere los parámetros de incertidumbre de medición. Por lo general, este parámetro lo proporciona el proveedor del equipo o puede obtenerse mediante la calibración del equipo de medición. La incertidumbre de la medición del radar depende de varios parámetros, como SNR (relación señal / ruido), ancho del haz, ancho de banda, tiempo en el objetivo, estabilidad del reloj y más. Cada medición de radar tiene diferentes SNR, ancho de haz y tiempo en el objetivo. Por lo tanto, ese radar calcula la incertidumbre de medición para cada medición y la informa al rastreador.
Las salidas del filtro son:
- Estimación del estado del sistema ( \( \hat{x}_{n,n} \) )
- Incertidumbre estimada ( \( p_{n,n} \) )
Además del sistema de estimación del estado, el filtro de Kalman también proporciona la incertidumbre de estimación. Como ya he mencionado, la incertidumbre estimada viene dada por:
\[ p_{n,n}=~ \left( 1-K_{n} \right) p_{n,n-1} \]
y \( p_{n,n} \) siempre se hace más pequeño con cada iteración de filtro, ya que \( \left( 1-K_{n} \right) \leq 1 \)
Por lo tanto, depende de nosotros decidir cuántas medidas tomar. Si estamos midiendo la altura del edificio y estamos interesados en una precisión de 3 centímetros ( \( \sigma \) ), haremos las mediciones hasta que la incertidumbre de la estimación ( \( \sigma ^{2} \) ) sea inferior a 9 centímetros.
La siguiente tabla resume las cinco ecuaciones del filtro de Kalman.
| Ecuación | Nombre |
|---|---|
| \( \hat{x}_{n,n}=~ \hat{x}_{n,n-1}+ K_{n} \left( z_{n}- \hat{x}_{n,n-1} \right) \) | Actualización de estado |
|
\( \hat{x}_{n+1,n}= \hat{x}_{n,n}+ \Delta t\hat{\dot{x}}_{n,n} \) \( \hat{\dot{x}}_{n+1,n}= \hat{\dot{x}}_{n,n} \) (para sistemas de velocidad constante) |
Extrapolación de estado |
| \( K_{n}= \frac{p_{n,n-1}}{p_{n,n-1}+r_{n}} \) | Ganancia de Kalman |
| \( p_{n,n}=~ \left( 1-K_{n} \right) p_{n,n-1} \) | Actualización de covarianza |
|
\( p_{n+1,n}= p_{n,n} \) (para sistemas estáticos) |
Extrapolación de covarianza |
La siguiente figura proporciona una descripción detallada del diagrama de bloques del filtro de Kalman.
- Paso 0: Inicialización
- Estado inicial del sistema ( \( \hat{x}_{1,0} \) )
- Incertidumbre inicial del estado ( \( p_{1,0} \) )
- Paso 1: Medición
- Medición del estado del sistema ( \( z_{n} \) )
- Incertidumbre de medida ( \( r_{n} \) )
- Paso 2: Actualización del estado
- Valor medido ( \( z_{n} \) )
- La incertidumbre de medición ( \( r_{n} \) )
- Estimación del estado del sistema anterior ( \( \hat{x}_{n,n-1} \) )
- Incertidumbre estimada ( \( p_{n,n-1} \) )
- Estimación del estado actual del sistema ( \( \hat{x}_{n,n} \) )
- Incertidumbre estimada del estado actual ( \( p_{n,n} \) )
- Paso 3: Predicción
Como se mencionó anteriormente, la inicialización se realiza solo una vez y proporciona dos parámetros:
La inicialización es seguida por la predicción.
El proceso de medición debe proporcionar dos parámetros:
El proceso de actualización de estado es responsable de la estimación del estado actual del sistema.
Las entradas del proceso de actualización de estado son:
Según las entradas, el proceso de actualización de estado calcula la ganancia de Kalman y proporciona dos salidas:
Estos parámetros son las salidas del filtro Kalman.
El proceso de predicción extrapola el estado actual del sistema y la incertidumbre de la estimación del estado actual del sistema al siguiente estado del sistema, según el modelo dinámico del sistema.
En la primera iteración del filtro, las salidas de inicialización se tratan como estimación e incertidumbre del estado anterior.
En las siguientes iteraciones de filtro, las salidas de predicción se convierten en la estimación de estado anterior y la incertidumbre.
La intuición de la ganancias de Kalman
La ganancia de Kalman define una ponderación de la medición y una ponderación de la estimación anterior al formar una nueva estimación.
Alta ganancia de Kalman
Una incertidumbre de medición baja en relación con la incertidumbre estimada, daría como resultado una ganancia de Kalman alta (cercana a 1). Como resultado, la nueva estimación estaría cerca de la medición. La siguiente figura ilustra la influencia de la alta ganancia de Kalman en la estimación en la aplicación de seguimiento de aeronaves.
Baja ganancia de Kalman
Una incertidumbre de medición alta en relación con la incertidumbre estimada, daría como resultado una ganancia de Kalman baja (cercana a 0). Como resultado, la nueva estimación estaría cerca de la estimación anterior. La siguiente figura ilustra la influencia de la baja ganancia de Kalman en la estimación en la aplicación de seguimiento de aeronaves.
Ahora, entendemos el algoritmo de filtro de Kalman y estamos listos para el primer ejemplo numérico.
Ejemplo 5 – Estimación de la altura de un edificio
Supongamos que nos gustaría estimar la altura del edificio utilizando un altímetro muy impreciso.
Sabemos con certeza que la altura del edificio no cambia con el tiempo, al menos durante el breve proceso de medición.
Ejemplo numerico
- La verdadera altura del edificio es de 50 metros.
- El error de medición del altímetro (desviación estándar) es de 5 metros.
- El conjunto de diez medidas es: 48.54m, 47.11m, 55.01m, 55.15m, 49.89m, 40.85m, 46.72m, 50.05m, 51.27m, 49.95m.
Iteracion Cero
Inicializacion
Podemos estimar la altura del edificio simplemente mirándolo.
La altura estimada del edificio es:
\[ \hat{x}_{0,0}=60m \]
Ahora vamos a inicializar la incertidumbre estimada. El error de estimación humana (desviación estándar) es de unos 15 metros: \( \sigma =15 \) . En consecuencia, la varianza es 225: \( \sigma ^{2}=225 \) .
\[ p_{0,0}=225 \]
Predicción
Ahora, predeciremos el siguiente estado en función de los valores de inicialización.
Dado que el modelo dinámico del sistema que usa es constante, es decir, el edificio no cambia su altura, entonces:
\[ \hat{x}_{1,0}=\hat{x}_{0,0}= 60m \]
La incertidumbre (varianza) estimada extrapolada tampoco cambia:
\[ p_{1,0}= p_{0,0}=225 \]
Primera Iteracion
Paso 1 - Medida
La primera medida es: \( z_{1}=48.54m \) .
Dado que la desviación estándar ( \( \sigma \) ) del error de medición del altímetro es 5, la varianza ( \( \sigma ^{2} \) ) sería 25, por lo que la incertidumbre de medición es: \( r_{1}=25 \) .
Paso 2 - Actualización
Cálculo de ganancia de Kalman:
\[ K_{1}= \frac{p_{1,0}}{p_{1,0}+r_{1}}= \frac{225}{225+25}=0.9 \]
Estimando el estado actual:
\[ \hat{x}_{1,1}=~ \hat{x}_{1,0}+ K_{1} \left( z_{1}- \hat{x}_{1,0} \right) =60+0.9 \left( 48.54-60 \right) =49.69m \]
Actualizando la incertidumbre actual estimada:
\[ p_{1,1}=~ \left( 1-K_{1} \right) p_{1,0}= \left( 1-0.9 \right) 225=22.5 \]
Paso 3 - Predecir
Dado que el modelo dinámico del sistema que usa es constante, es decir, el edificio no cambia su altura, entonces:
\[ \hat{x}_{2,1}=\hat{x}_{1,1}= 49.69m \]
La incertidumbre (varianza) estimada extrapolada tampoco cambia:
\[ p_{2,1}= p_{1,1}=22.5 \]
Segunda Iteracion
Después de un retraso de unidad de tiempo, la estimación pronosticada de la iteración anterior se convierte en una estimación previa en la iteración actual:
\[ \hat{x}_{2,1}=49.69m \]
La incertidumbre de estimación extrapolada se convierte en la incertidumbre de estimación previa:
\[ p_{2,1}= 22.5 \]
Paso 1 - Medida
La segunda medida es: \( z_{2}=47.11m \)
La incertidumbre de medición es: \( r_{2}=25 \)
Paso 2 - Actualización
Cálculo de ganancia de Kalman:
\[ K_{2}= \frac{p_{2,1}}{p_{2,1}+r_{2}}= \frac{22.5}{22.5+25}=0.47 \]
Estimando el estado actual:
\[ \hat{x}_{2,2}=~ \hat{x}_{2,1}+ K_{2} \left( z_{2}- x_{2,1} \right) =49.69+0.47 \left( 47.11-49.69 \right) =48.47m \]
Actualice la incertidumbre actual estimada:
\[ p_{2,2}=~ \left( 1-K_{2} \right) p_{2,1}= \left( 1-0.47 \right) 22.5=11.84 \]
Paso 3 - Predecir
Dado que el modelo dinámico del sistema que usa es constante, es decir, el edificio no cambia su altura, entonces:
\[ \hat{x}_{3,2}=\hat{x}_{2,2}= 48.47m \]
La incertidumbre (varianza) estimada extrapolada tampoco cambia:
\[ p_{3,2}= p_{2,2}=11.84 \]
Iteraciones 3-10
Los cálculos para las siguientes iteraciones se resumen en la siguiente tabla:
| \( n \) | \( z_{n} \) | Estimación del estado actual ( \( K_{n} \) , \( \hat{x}_{n,n} \) , \( p_{n,n} \) ) | Predicción ( \( \hat{x}_{n+1,n} \) , \( p_{n+1,n} \) ) |
|---|---|---|---|
| 3 | \( 55.01m \) | \[ K_{3}= \frac{11.84}{11.84+25}=0.32 \] \[ \hat{x}_{3,3}=~ 48.47+0.32 \left( 55.01 -48.47 \right) =50.57m \] \[ p_{3,3}= \left( 1-0.32 \right) 11.84=8.04 \] | \[ \hat{x}_{4,3}= \hat{x}_{3,3}=50.57m \] \[ p_{4,3}= p_{3,3}=8.04 \] |
| 4 | \( 55.15m \) | \[ K_{4}= \frac{8.04}{8.04+25}=0.24 \] \[ \hat{x}_{4,4}=~ 50.57+0.24 \left( 55.15 -50.57 \right) =51.68m \] \[ p_{4,4}= \left( 1-0.24 \right) 8.04=6.08 \] | \[ \hat{x}_{5,4}= \hat{x}_{4,4}=51.68m \] \[ p_{5,4}= p_{4,4}=6.08 \] |
| 5 | \( 49.89m \) | \[ K_{5}= \frac{6.08}{6.08+25}=0.2 \] \[ \hat{x}_{5,5}= 51.68+0.2 \left( 49.89 -51.68 \right) =51.33m \] \[ p_{5,5}= \left( 1-0.2 \right) 6.08=4.89 \] | \[ \hat{x}_{6,5}= \hat{x}_{5,5}=51.33m \] \[ p_{6,5}= p_{5,5}=4.89 \] |
| 6 | \( 40.85m \) | \[ K_{6}= \frac{4.89}{4.89+25}=0.16 \] \[ \hat{x}_{6,6}=~ 51.33+0.16 \left( 40.85 -51.33 \right) =49.62m \] \[ p_{6,6}= \left( 1-0.16 \right) 4.89=4.09 \] | \[ \hat{x}_{7,6}= \hat{x}_{6,6}=49.62m \] \[ p_{7,6}= p_{6,6}=4.09 \] |
| 7 | \( 46.72m \) | \[ K_{7}= \frac{4.09}{4.09+25}=0.14 \] \[ \hat{x}_{7,7}=~ 49.62+0.14 \left( 46.72 -49.62 \right) =49.21m \] \[ p_{7,7}= \left( 1-0.14 \right) 4.09=3.52 \] | \[ \hat{x}_{8,7}= \hat{x}_{7,7}=49.21m \] \[ p_{8,7}= p_{7,7}=3.52 \] |
| 8 | \( 50.05m \) | \[ K_{8}= \frac{3.52}{3.52+25}=0.12 \] \[ \hat{x}_{8,8}= 49.21+0.12 \left( 50.05 -49.21 \right) =49.31m \] \[ p_{8,8}= \left( 1-0.12 \right) 3.52=3.08 \] | \[ \hat{x}_{9,8}= \hat{x}_{8,8}=49.31m \] \[ p_{9,8}= p_{8,8}=3.08 \] |
| 9 | \( 51.27m \) | \[ K_{9}= \frac{3.08}{3.08+25}=0.11 \] \[ \hat{x}_{9,9}=~ 49.31+0.11 \left( 51.27 -49.31 \right) =49.53m \] \[ p_{9,9}= \left( 1-0.11 \right) 3.08=2.74 \] | \[ \hat{x}_{10,9}= \hat{x}_{9,9}=49.53m \] \[ p_{10,9}= p_{9,9}=2.74 \] |
| 10 | \( 49.95m \) | \[ K_{10}= \frac{2.74}{2.74+25}=0.1 \] \[ \hat{x}_{10,10}=~ 49.53+0.1 \left( 49.95 -49.53 \right) =49.57m \] \[ p_{10,10}= \left( 1-0.1 \right) 2.74=2.47 \] | \[ \hat{x}_{11,10}= \hat{x}_{10,10}=49.57m \] \[ p_{11,10}= p_{10,10}=2.47 \] |
La siguiente gráfica compara el valor verdadero, los valores medidos y las estimaciones.
Como puede ver, el valor estimado converge aproximadamente 49.5 metros después de 7 mediciones.
El siguiente gráfico compara la incertidumbre de medición y la incertidumbre estimada.
En la primera iteración del filtro, la incertidumbre estimada está cerca de la incertidumbre de medición, y disminuye rápidamente. Después de 10 mediciones, la incertidumbre estimada ( \( \sigma ^{2} \) ) es 2.47, es decir, la desviación estándar del error estimado es: \( \sigma = \sqrt[]{2.47}=1.57m \)
Entonces podemos decir que la altura estimada del edificio es: \( 49.57 \pm 1.57m \)
El siguiente gráfico muestra la ganancia de Kalman.
Como puede ver, la ganancia de Kalman está disminuyendo, lo que hace que el peso de medición sea cada vez más pequeño.
Resumen del ejemplo
En este ejemplo, hemos medido la altura del edificio utilizando el filtro de Kalman unidimensional. A diferencia del filtro \( \alpha -\beta -(\gamma) \), la ganancia de Kalman es dinámica y depende de la precisión del dispositivo de medición.
Al principio, la inicialización del filtro de Kalman no es precisa. Por lo tanto, el peso de las mediciones en la ecuación de actualización de estado es alto y la incertidumbre estimada es alta.
Con cada iteración, el peso de medición es menor y la incertidumbre estimada es menor.
La salida del filtro de Kalman incluye la estimación y la incertidumbre de la estimación.
El modelo completo del filtro de Kalman unidimensional
Ahora, vamos a actualizar la ecuación de extrapolación de covarianza con la variable de ruido del proceso.
El ruido del proceso
En un mundo real, existen incertidumbres en el modelo dinámico del sistema. Por ejemplo, cuando queremos estimar el valor de resistencia de la resistencia, asumimos el modelo dinámico constante, es decir, la resistencia no cambia entre las mediciones. Sin embargo, la resistencia puede cambiar ligeramente debido a la fluctuación de la temperatura ambiente. Al rastrear misiles balísticos con el radar, la incertidumbre del modelo dinámico incluye cambios aleatorios en la aceleración del objetivo. Para la aeronave, las incertidumbres son mucho mayores debido a las posibles maniobras de la aeronave.
Por otro lado, cuando estimamos la ubicación de un objeto estático usando un receptor GPS, la incertidumbre del modelo dinámico es cero, ya que el objeto estático no se mueve. La incertidumbre del modelo dinámico se llama ruido de proceso. En la literatura, también se denomina ruido de la planta, ruido de conducción, ruido dinámico, ruido del modelo y ruido del sistema. El ruido del proceso produce errores de estimación.
En el ejemplo anterior, hemos estimado la altura del edificio. La altura del edificio no cambia. Por lo tanto, no tomamos en cuenta el ruido del proceso.
La Varianza del ruido de proceso se denota con la letra \( q \).
La Ecuación de extrapolación de covarianza incluirá la Varianza del ruido de proceso.
La ecuación de extrapolación de covarianza para dinámica constante es:
\[ p_{n+1,n}= p_{n,n}+ q_{n} \]
Estas son las ecuaciones de filtro de Kalman actualizadas en una dimensión:
| Ecuación | Nombre |
|---|---|
| \( \hat{x}_{n,n}=~ \hat{x}_{n,n-1}+ K_{n} \left( z_{n}- \hat{x}_{n,n-1} \right) \) | Actualización de estado |
|
\( \hat{x}_{n+1,n}= \hat{x}_{n,n}+ \Delta t\hat{\dot{x}}_{n,n} \) \( \hat{\dot{x}}_{n+1,n}= \hat{\dot{x}}_{n,n} \) (para sistemas de velocidad constante) |
Extrapolación de estado |
| \( K_{n}= \frac{p_{n,n-1}}{p_{n,n-1}+r_{n}} \) | Ganancia de Kalman |
| \( p_{n,n}=~ \left( 1-K_{n} \right) p_{n,n-1} \) | Actualización de covarianza |
|
\( p_{n+1,n}= p_{n,n} + q_{n} \) (para sistemas estáticos) |
Extrapolación de covarianza |
Ejemplo 6 – Estimación de la temperatura del líquido en un tanque
Nos gustaría estimar la temperatura del líquido en un tanque.
Suponemos que en el estado estacionario la temperatura del líquido es constante. Sin embargo, son posibles algunas fluctuaciones en la temperatura real del líquido. Podemos describir la dinámica del sistema mediante la siguiente ecuación:
\[ x_{n}=T+ w_{n} \]
dónde:
\( T \) es la temperatura constante
\( w_{n} \) es un ruido de proceso aleatorio con varianza \( q \)
Ejemplo numerico
- Supongamos la verdadera temperatura de 50 grados centígrados.
- Creemos que tenemos un modelo preciso, por lo tanto, establecemos la varianza del ruido del proceso ( \( q \) ) en 0.0001.
- Creemos que tenemos un modelo preciso, por lo tanto, establecemos la varianza del ruido del proceso.
- Las medidas se toman cada 5 segundos.
- La temperatura real del líquido en los puntos de medición es: 49.979\( ^{o}C \), 50.025\( ^{o}C \), 50\( ^{o}C \), 50.003\( ^{o}C \), 49.994\( ^{o}C \), 50.002\( ^{o}C \), 49.999\( ^{o}C \), 50.006\( ^{o}C \), 49.998\( ^{o}C \), y 49.991\( ^{o}C \).
- El conjunto de medidas es: 49.95\( ^{o}C \), 49.967\( ^{o}C \), 50.1\( ^{o}C \), 50.106\( ^{o}C \), 49.992\( ^{o}C \), 49.819\( ^{o}C \), 49.933\( ^{o}C \), 50.007\( ^{o}C \), 50.023\( ^{o}C \), y 49.99\( ^{o}C \).
La siguiente grafica compara la temperatura real del líquido y las mediciones.
Iteracion cero
Antes de la primera iteración, debemos inicializar el filtro de Kalman y predecir el siguiente estado (que es el primer estado).
Inicializacion
No sabemos cuál es la temperatura del líquido, y nuestra estimación es de 10\( ^{o}C \).
\[ \hat{x}_{0,0}=10^{o}C \]
Nuestra suposición es muy imprecisa, establecemos nuestro error de estimación de inicialización \( \sigma \) en 100. La incertidumbre estimada de la inicialización es la varianza del error \( \left( \sigma ^{2} \right) \):
\[ p_{0,0}=100^{2}=10,000 \]
Esta variación es muy alta. Si inicializamos con un valor más significativo, obtendremos una convergencia más rápida del filtro de Kalman.
Predicción
Ahora, predeciremos el siguiente estado en función de los valores de inicialización.
Dado que nuestro modelo es de dinámica constante, la estimación pronosticada es igual a la estimación actual:
\[ \hat{x}_{1,0}=10^{o}C \]
La incertidumbre de estimación extrapolada (varianza):
\[ p_{1,0}= p_{0,0}+q=10000+ 0.0001=10000.0001 \]
Primera iteracion
Paso 1 - Medida
El valor de medición:
\[ z_{1}=~ 49.95^{o}C \]
Como el error de medición es 0.1 ( \( \sigma \) ), la varianza ( \( \sigma ^{2} \) ) sería 0.01, por lo tanto, la incertidumbre de medición es:
\[ r_{1}= 0.01 \]
Paso 2 - Actualización
Cálculo de ganancia de Kalman:
\[ K_{1}= \frac{p_{1,0}}{p_{1,0}+r_{1}}= \frac{10000.0001}{10000.0001+0.01} = 0.999999 \]
La ganancia de Kalman es casi 1, es decir, nuestro error de estimación es mucho mayor que el error de medición. Por lo tanto, el peso estimado es insignificante, mientras que el peso de medición es casi 1.
Estimando el estado actual:
\[ \hat{x}_{1,1}=~ \hat{x}_{1,0}+ K_{1} \left( z_{1}- \hat{x}_{1,0} \right) =10+0.999999 \left( 49.95-10 \right) =49.95^{o}C \]
Actualice la incertidumbre actual estimada:
\[ p_{1,1}=~ \left( 1-K_{1} \right) p_{1,0}= \left( 1-0.999999 \right) 10000.0001=0.01 \]
Paso 3 - Predecir
Dado que el modelo dinámico del sistema que usa es constante, es decir, la temperatura del líquido no cambia, entonces:
\[ \hat{x}_{2,1}=\hat{x}_{1,1}= 49.95^{o}C \]
La incertidumbre de estimación extrapolada (varianza) es:
\[ p_{2,1}= p_{1,1}+q=0.01+ 0.0001=0.0101 \]
Second Iteration
Paso 1 - Medida
El valor de medición:
\[ z_{2}=~ 49.967^{o}C \]
Como el error de medición es 0.1 ( \( \sigma \) ), la varianza ( \( \sigma^{2} \) ) sería 0.01, por lo tanto, la incertidumbre de medición es:
\[ r_{2}= 0.01 \]
Paso 2 - Actualización
Cálculo de ganancia de Kalman:
\[ K_{2}= \frac{p_{2,1}}{p_{2,1}+r_{2}}= \frac{0.0101}{0.0101+0.01} = 0.5 \]
La ganancia de Kalman es 0.5, es decir, el peso estimado y el peso de medición son iguales.
Estimando el estado actual:
\[ \hat{x}_{2,2}=~ \hat{x}_{2,1}+ K_{2} \left( z_{2}- \hat{x}_{2,1} \right) =49.95+0.5 \left( 49.967-49.95 \right) =49.959^{o}C \]
Actualice la incertidumbre actual estimada:
\[ p_{2,2}=~ \left( 1-K_{2} \right) p_{2,1}= \left( 1-0.5 \right) 0.0101=0.005 \]
Paso 3 - Predecir
Dado que el modelo dinámico del sistema que usa es constante, es decir, la temperatura del líquido no cambia, entonces:
\[ \hat{x}_{3,2}=\hat{x}_{2,2}= 49.959^{o}C \]
La incertidumbre de estimación extrapolada (varianza) es:
\[ p_{3,2}= p_{2,2}+q=0.005+ 0.0001=0.0051 \]
Iteraciones 3-10
Los cálculos para las siguientes iteraciones se resumen en la siguiente tabla:
| \( n \) | \( z_{n} \) | Estimación del estado actual ( \( K_{n} \) , \( \hat{x}_{n,n} \) , \( p_{n,n} \) ) | Predicción ( \( \hat{x}_{n+1,n} \) , \( p_{n+1,n} \) ) |
|---|---|---|---|
| 3 | \( 50.1^{o}C \) | \[ K_{3}= \frac{0.0051}{0.0051+0.01}=0.3388 \] \[ \hat{x}_{3,3}=~ 49.959+0.3388 \left( 50.1-49.959 \right) =50.007^{o}C \] \[ p_{3,3}= \left( 1-0.3388 \right)0.0051 =0.0034 \] | \[ \hat{x}_{4,3}= \hat{x}_{3,3}=50.007^{o}C \] \[ p_{4,3}= 0.0034+0.0001=0.0035 \] |
| 4 | \( 50.106^{o}C \) | \[ K_{4}= \frac{0.0035}{0.0035+0.01}=0.2586 \] \[ \hat{x}_{4,4}=~ 50.007+0.2586 \left( 50.106-50.007 \right) =50.032^{o}C \] \[ p_{4,4}= \left( 1-0.2586 \right) 0.0035=0.0026 \] | \[ \hat{x}_{5,4}= \hat{x}_{4,4}=50.032^{o}C \] \[ p_{5,4}= 0.0026+0.0001=0.0027 \] |
| 5 | \( 49.992^{o}C \) | \[ K_{5}= \frac{0.0027}{0.0027+0.01}=0.2117 \] \[ \hat{x}_{5,5}= 50.032+0.2117 \left( 49.992-50.032 \right) =50.023^{o}C \] \[ p_{5,5}= \left( 1-0.2117 \right) 0.0027=0.0021 \] | \[ \hat{x}_{6,5}= \hat{x}_{5,5}=50.023^{o}C \] \[ p_{6,5}= 0.0021+0.0001=0.0022 \] |
| 6 | \( 49.819^{o}C \) | \[ K_{6}= \frac{0.0022}{0.0022+0.01}=0.1815 \] \[ \hat{x}_{6,6}=~ 50.023+0.1815 \left( 49.819-50.023 \right) =49.987^{o}C \] \[ p_{6,6}= \left( 1-0.1815 \right) 0.0022=0.0018 \] | \[ \hat{x}_{7,6}= \hat{x}_{6,6}=49.987^{o}C \] \[ p_{7,6}= 0.0018+0.0001=0.0019 \] |
| 7 | \( 49.933^{o}C \) | \[ K_{7}= \frac{0.0019}{0.0019+0.01}=0.1607 \] \[ \hat{x}_{7,7}=~ 49.987+0.1607 \left( 49.933-49.987 \right) =49.978^{o}C \] \[ p_{7,7}= \left( 1-0.1607 \right) 0.0019=0.0016 \] | \[ \hat{x}_{8,7}= \hat{x}_{7,7}=49.978^{o}C \] \[ p_{8,7}= 0.0016+0.0001=0.0017 \] |
| 8 | \( 50.007^{o}C \) | \[ K_{8}= \frac{0.0017}{0.0017+0.01}=0.1458 \] \[ \hat{x}_{8,8}= 49.978+0.1458 \left( 50.007-49.978 \right) =49.983^{o}C \] \[ p_{8,8}= \left( 1-0.1458 \right) 0.0017=0.0015 \] | \[ \hat{x}_{9,8}= \hat{x}_{8,8}=49.983^{o}C \] \[ p_{9,8}= 0.0015+0.0001=0.0016 \] |
| 9 | \( 50.023^{o}C \) | \[ K_{9}= \frac{0.0016}{0.0016+0.01}=0.1348 \] \[ \hat{x}_{9,9}=~ 49.983+0.1348 \left( 50.023-49.983 \right) =49.988^{o}C \] \[ p_{9,9}= \left( 1-0.1348 \right) 0.0016=0.0014 \] | \[ \hat{x}_{10,9}= \hat{x}_{9,9}=49.988^{o}C \] \[ p_{10,9}= 0.0014+0.0001=0.0015 \] |
| 10 | \( 49.99^{o}C \) | \[ K_{10}= \frac{0.0015}{0.0015+0.01}=0.1265 \] \[ \hat{x}_{10,10}=~ 49.988+0.1265 \left( 49.99 -49.988 \right) =49.988^{o}C \] \[ p_{10,10}= \left( 1-0.1265 \right) 0.0015=0.0013 \] | \[ \hat{x}_{11,10}= \hat{x}_{10,10}=49.988^{o}C \] \[ p_{11,10}= 0.0013+0.0001=0.0014 \] |
La siguiente gráfica compara el valor verdadero, los valores medidos y las estimaciones.
Como puede ver, el valor estimado converge hacia el valor verdadero.
El siguiente gráfico muestra la incertidumbre estimada.
La incertidumbre estimada disminuye rápidamente. Después de 10 mediciones, la incertidumbre estimada ( \( \sigma ^{2} \) ) es 0.0013, es decir, la desviación estándar del error estimado es: \( \sigma = \sqrt[]{0.0013}=0.036^{o}C \)
Entonces podemos decir que la temperatura estimada del líquido es: \( 49.988 \pm 0.036_{ }^{o}C \)
La siguiente tabla muestra la ganancia de Kalman.
Como puede ver, la ganancia de Kalman está disminuyendo, lo que hace que el peso de medición sea cada vez más pequeño.
Resumen del ejemplo
En este ejemplo, hemos medido la temperatura del líquido utilizando el filtro Kalman unidimensional. Aunque la dinámica del sistema incluye un ruido de proceso aleatorio, el filtro de Kalman puede proporcionar una buena estimación.
Ejemplo 7 – Estimación de la temperatura del líquido de calefacción Ⅰ
Como en el ejemplo anterior, en este ejemplo vamos a estimar la temperatura del líquido en el tanque. La dinámica del sistema no es constante, el líquido se calienta a una tasa de 0.1\( ^{o}C \) por segundo.
Los parámetros del filtro Kalman son similares al ejemplo anterior:
- Creemos que tenemos un modelo preciso, por lo tanto, establecemos la varianza del ruido del proceso (q) en 0.0001.
- El error de medición (desviación estándar) es 0.1\( ^{o}C \).
- Las medidas se toman cada 5 segundos.
- La dinámica del sistema es constante.
Preste atención, aunque la dinámica real del sistema no es constante (ya que el líquido se está calentando), vamos a tratar el sistema como un sistema con dinámica constante (la temperatura no cambia).
- La temperatura real del líquido en los puntos de medición es: 50.479\( ^{o}C \), 51.025\( ^{o}C \), 51.5\( ^{o}C \), 52.003\( ^{o}C \), 52.494\( ^{o}C \), 53.002\( ^{o}C \), 53.499\( ^{o}C \), 54.006\( ^{o}C \), 54.498\( ^{o}C \) y 54.991\( ^{o}C \).
- El conjunto de medidas es: 50.45\( ^{o}C \), 50.967\( ^{o}C \), 51.6\( ^{o}C \), 52.106\( ^{o}C \), 52.492\( ^{o}C \), 52.819\( ^{o}C \), 53.433\( ^{o}C \), 54.007\( ^{o}C \), 54.523\( ^{o}C \) y 54.99\( ^{o}C \).
La siguiente figura compara la temperatura real del líquido y las mediciones.
Iteracion cero
La iteración cero es similar al ejemplo anterior.
Antes de la primera iteración, debemos inicializar el filtro de Kalman y predecir el siguiente estado (que es el primer estado).
Inicializacion
No sabemos cuál es la temperatura del líquido en un tanque y nuestra suposición es de 10\( ^{o}C \).
\[ \hat{x}_{0,0}=10^{o}C \]
Nuestra suposición es muy imprecisa, establecemos nuestro error de estimación de inicialización ( \( \sigma \) ) en 100. La incertidumbre estimada de la inicialización es la varianza del error \( \left( \sigma ^{2} \right) \):
\[ p_{0,0}=100^{2}=10,000 \]
Esta variación es muy alta. Si inicializamos con un valor más significativo, obtendremos una convergencia más rápida del filtro de Kalman.
Prediction
Ahora, predeciremos el siguiente estado en función de los valores de inicialización.
Dado que nuestro modelo es de dinámica constante, la estimación pronosticada es igual a la estimación actual:
\[ \hat{x}_{1,0}=10^{o}C \]
La incertidumbre de estimación extrapolada (varianza):
\[ p_{1,0}= p_{0,0}+q=10000+ 0.0001=10000.0001 \]
Iteraciones 1-10
Los cálculos para las siguientes iteraciones se resumen en la siguiente tabla:
| \( n \) | \( z_{n} \) | Estimación del estado actual ( \( K_{n} \) , \( \hat{x}_{n,n} \) , \( p_{n,n} \) ) | Predicción ( \( \hat{x}_{n+1,n} \) , \( p_{n+1,n} \) ) |
|---|---|---|---|
| 1 | \( 50.45^{o}C \) | \[ K_{1}= \frac{10000.0001}{10000.0001+0.01} = 0.999999 \] \[ \hat{x}_{1,1}=~ 10+0.999999 \left( 50.45-10 \right) =50.45^{o}C \] \[ p_{1,1}= \left( 1-0.999999 \right) 10000.0001=0.01 \] | \[ \hat{x}_{2,1}= \hat{x}_{1,1}=50.45^{o}C \] \[ p_{2,1}= 0.01+0.0001=0.0101 \] |
| 2 | \( 50.967^{o}C \) | \[ K_{2}= \frac{0.0101}{0.0101+0.01}=0.5025 \] \[ \hat{x}_{2,2}=~ 50.45+0.5025 \left( 50.967-50.45 \right) =50.71^{o}C\] \[ p_{2,2}= \left( 1-0.5025 \right) 0.0101=0.005 \] | \[ \hat{x}_{3,2}= \hat{x}_{2,2}=50.71^{o}C \] \[ p_{3,2}= 0.005+0.0001=0.0051 \] |
| 3 | \( 51.6^{o}C \) | \[ K_{3}= \frac{0.0051}{0.0051+0.01}=0.3388 \] \[ \hat{x}_{3,3}=~ 50.71+0.3388 \left( 51.6-50.71 \right) =51.011^{o}C\] \[ p_{3,3}= \left( 1-0.3388 \right) 0.0051=0.0034 \] | \[ \hat{x}_{4,3}= \hat{x}_{3,3}=51.011^{o}C \] \[ p_{4,3}= 0.0034+0.0001=0.0035 \] |
| 4 | \( 52.106^{o}C \) | \[ K_{4}= \frac{0.0035}{0.0035+0.01}=0.2586 \] \[ \hat{x}_{4,4}=~ 51.011+0.2586 \left( 52.106-51.011 \right) =51.295^{o}C \] \[ p_{4,4}= \left( 1-0.2586 \right) 0.0035=0.0026 \] | \[ \hat{x}_{5,4}= \hat{x}_{4,4}=51.295^{o}C \] \[ p_{5,4}= 0.0026+0.0001=0.0027 \] |
| 5 | \( 52.492^{o}C \) | \[ K_{5}= \frac{0.0027}{0.0027+0.01}=0.2117 \] \[ \hat{x}_{5,5}= 51.295+0.2117 \left( 52.492-51.295 \right) =51.548^{o}C \] \[ p_{5,5}= \left( 1-0.2117 \right) 0.0027=0.0021 \] | \[ \hat{x}_{6,5}= \hat{x}_{5,5}=51.548^{o}C \] \[ p_{6,5}= 0.0021+0.0001=0.0022 \] |
| 6 | \( 52.819^{o}C \) | \[ K_{6}= \frac{0.0022}{0.0022+0.01}=0.1815 \] \[ \hat{x}_{6,6}=~ 51.548+0.1815 \left( 52.819-51.548 \right) =51.779^{o}C \] \[ p_{6,6}= \left( 1-0.1815 \right) 0.0022=0.0018 \] | \[ \hat{x}_{7,6}= \hat{x}_{6,6}=51.779^{o}C \] \[ p_{7,6}= 0.0018+0.0001=0.0019 \] |
| 7 | \( 53.433^{o}C \) | \[ K_{7}= \frac{0.0019}{0.0019+0.01}=0.1607 \] \[ \hat{x}_{7,7}=~ 51.779+0.1607 \left( 53.433-51.779 \right) =52.045^{o}C \] \[ p_{7,7}= \left( 1-0.1607 \right) 0.0019=0.0016 \] | \[ \hat{x}_{8,7}= \hat{x}_{7,7}=52.045^{o}C \] \[ p_{8,7}= 0.0016+0.0001=0.0017 \] |
| 8 | \( 54.007^{o}C \) | \[ K_{8}= \frac{0.0017}{0.0017+0.01}=0.1458 \] \[ \hat{x}_{8,8}= 52.045+0.1458 \left( 54.007-52.045 \right) =52.331^{o}C \] \[ p_{8,8}= \left( 1-0.1458 \right) 0.0017=0.0015 \] | \[ \hat{x}_{9,8}= \hat{x}_{8,8}=52.331^{o}C \] \[ p_{9,8}= 0.0015+0.0001=0.0016 \] |
| 9 | \( 54.523^{o}C \) | \[ K_{9}= \frac{0.0016}{0.0016+0.01}=0.1348 \] \[ \hat{x}_{9,9}=~ 52.331+0.1348 \left( 54.523-52.331 \right) =52.626^{o}C \] \[ p_{9,9}= \left( 1-0.1348 \right) 0.0016=0.0014 \] | \[ \hat{x}_{10,9}= \hat{x}_{9,9}=52.626^{o}C \] \[ p_{10,9}= 0.0014+0.0001=0.0015 \] |
| 10 | \( 54.99^{o}C \) | \[ K_{10}= \frac{0.0015}{0.0015+0.01}=0.1265 \] \[ \hat{x}_{10,10}=~ 2.626+0.1265 \left( 54.99 -52.626 \right) =52.925^{o}C \] \[ p_{10,10}= \left( 1-0.1265 \right) 0.0015=0.0013 \] | \[ \hat{x}_{11,10}= \hat{x}_{10,10}=52.925^{o}C \] \[ p_{11,10}= 0.0013+0.0001=0.0014 \] |
La siguiente grafica compara el valor verdadero, los valores medidos y las estimaciones.
Como puede ver, el filtro de Kalman no ha podido proporcionar una estimación confiable. Hay un error de retraso en la estimación del filtro de Kalman. Ya hemos encontrado el error de retraso en el Ejemplo 3, donde estimamos la posición de las aeronaves en aceleración usando el filtro \( \alpha - \beta \) que supone una velocidad constante de la aeronave. Eliminamos el error de retraso en el Ejemplo 4, donde reemplazamos el filtro \( \alpha - \beta \) por el filtro \( \alpha -\beta -\gamma \) que supone la aceleración.
Hay dos razones que causan un error de retraso en nuestro ejemplo de filtro de Kalman:
- El modelo dinámico no se ajusta al caso.
- La fiabilidad del modelo de proceso. Hemos elegido un ruido de proceso muy bajo \( \left( q=0.0001 \right) \) mientras que las fluctuaciones de temperatura reales son mucho mayores.
Hay dos formas posibles de corregir el error de retraso:
- Si sabemos que la temperatura del líquido puede cambiar linealmente, podemos definir un nuevo modelo que tenga en cuenta un posible cambio lineal en la temperatura del líquido. Lo hicimos en el Ejemplo 4. Este es un método preferido. Sin embargo, si el cambio de temperatura no se puede modelar, este método no mejorará el rendimiento del filtro Kalman.
- Por otro lado, dado que nuestro modelo no está bien definido, podemos ajustar la confiabilidad del modelo del proceso aumentando el ruido del proceso \( \left( q \right) \). Vea el siguiente ejemplo para más detalles.
Resumen del ejemplo
En este ejemplo, hemos medido la temperatura del líquido de calentamiento usando el Filtro Kalman unidimensional con modelo dinámico constante. Hemos observado el error de retraso en la estimación del filtro de Kalman. El error de retraso es causado por una definición de modelo dinámico incorrecto y una definición de modelo de proceso incorrecta.
El error de retraso se puede solucionar mediante la definición adecuada del modelo dinámico o modelo de proceso.
Ejemplo 8 – Estimación de la temperatura del líquido de calefacción Ⅱ
Este ejemplo es similar al ejemplo anterior con solo un cambio. Como nuestro proceso no está bien definido, aumentaremos la incertidumbre del proceso \( \left( q \right) \) de 0.0001 a 0.15.
Iteracion cero
Antes de la primera iteración, debemos inicializar el filtro de Kalman y predecir el siguiente estado (que es el primer estado).
Inicializacion
La inicialización cero es similar al ejemplo anterior.
No sabemos cuál es la temperatura del líquido en un tanque y nuestra suposición es de 10\( ^{o}C \).
\[ \hat{x}_{0,0}=10^{o}C \]
Nuestra suposición es muy imprecisa, establecemos nuestro error de estimación de inicialización ( \( \sigma \) ) en 100. La incertidumbre estimada de la inicialización es la varianza del error \( \left( \sigma ^{2} \right) \):
\[ p_{0,0}=100^{2}=10,000 \]
Esta variación es muy alta. Si inicializamos con un valor más significativo, obtendremos una convergencia más rápida del filtro de Kalman.
Predicción
Ahora, predeciremos el siguiente estado en función de los valores de inicialización.
Dado que nuestro modelo es de dinámica constante, la estimación pronosticada es igual a la estimación actual:
\[ \hat{x}_{1,0}=10^{o}C \]
La incertidumbre de estimación extrapolada (varianza):
\[ p_{1,0}= p_{0,0}+q=10000+ 0.15=10000.15 \]
Iteraciones 1-10
Los cálculos para las siguientes iteraciones se resumen en la siguiente tabla:
| \( n \) | \( z_{n} \) | Estimación del estado actual ( \( K_{n} \) , \( \hat{x}_{n,n} \) , \( p_{n,n} \) ) | Predicción ( \( \hat{x}_{n+1,n} \) , \( p_{n+1,n} \) ) |
|---|---|---|---|
| 1 | \( 50.45^{o}C \) | \[ K_{1}= \frac{10000.15}{10000.15+0.01} = 0.999999 \] \[ \hat{x}_{1,1}=~ 10+0.999999 \left( 50.45-10 \right) =50.45^{o}C \] \[ p_{1,1}= \left( 1-0.999999 \right)10000.15=0.01 \] | \[ \hat{x}_{2,1}= \hat{x}_{1,1}=50.45^{o}C \] \[ p_{2,1}= 0.01+0.15=0.16 \] |
| 2 | \( 50.967^{o}C \) | \[ K_{2}= \frac{0.16}{0.16+0.01}=0.9412 \] \[ \hat{x}_{2,2}=~ 50.45+0.9412 \left( 50.967-50.45 \right) =50.94^{o}C\] \[ p_{2,2}= \left( 1-0.9412 \right) 0.16=0.0094 \] | \[ \hat{x}_{3,2}= \hat{x}_{2,2}=50.94^{o}C \] \[ p_{3,2}= 0.0094+0.15=0.1594 \] |
| 3 | \( 51.6^{o}C \) | \[ K_{3}= \frac{0.1594}{0.1594+0.01}=0.941 \] \[ \hat{x}_{3,3}=~ 50.94+0.941 \left( 51.6-50.94 \right) =51.56^{o}C\] \[ p_{3,3}= \left( 1-0.941 \right) 0.1594=0.0094 \] | \[ \hat{x}_{4,3}= \hat{x}_{3,3}=51.56^{o}C \] \[ p_{4,3}= 0.0094+0.15=0.1594 \] |
| 4 | \( 52.106^{o}C \) | \[ K_{4}= \frac{0.1594}{0.1594+0.01}=0.941 \] \[ \hat{x}_{4,4}=~ 51.56+0.941 \left( 52.106-51.56 \right) =52.07^{o}C \] \[ p_{4,4}= \left( 1-0.941 \right) 0.1594=0.0094 \] | \[ \hat{x}_{5,4}= \hat{x}_{4,4}=52.07^{o}C \] \[ p_{5,4}= 0.0094+0.15=0.1594 \] |
| 5 | \( 52.492^{o}C \) | \[ K_{5}= \frac{0.1594}{0.1594+0.01}=0.941 \] \[ \hat{x}_{5,5}= 52.07+0.941 \left( 52.492-52.07 \right) =52.47^{o}C \] \[ p_{5,5}= \left( 1-0.941 \right) 0.1594=0.0094 \] | \[ \hat{x}_{6,5}= \hat{x}_{5,5}=52.47^{o}C \] \[ p_{6,5}= 0.0094+0.15=0.1594 \] |
| 6 | \( 52.819^{o}C \) | \[ K_{6}= \frac{0.1594}{0.1594+0.01}=0.941 \] \[ \hat{x}_{6,6}=~ 52.47+0.941 \left( 52.819-52.47 \right) =52.8^{o}C \] \[ p_{6,6}= \left( 1-0.941 \right) 0.1594=0.0094 \] | \[ \hat{x}_{7,6}= \hat{x}_{6,6}=52.8^{o}C \] \[ p_{7,6}= 0.0094+0.15=0.1594 \] |
| 7 | \( 53.433^{o}C \) | \[ K_{7}= \frac{0.1594}{0.1594+0.01}=0.941 \] \[ \hat{x}_{7,7}=~ 52.8+0.941 \left( 53.433-52.8 \right) =53.4^{o}C \] \[ p_{7,7}= \left( 1-0.941 \right) 0.1594=0.0094 \] | \[ \hat{x}_{8,7}= \hat{x}_{7,7}=53.4^{o}C \] \[ p_{8,7}= 0.0094+0.15=0.1594 \] |
| 8 | \( 54.007^{o}C \) | \[ K_{8}= \frac{0.1594}{0.1594+0.01}=0.941 \] \[ \hat{x}_{8,8}= 53.4+0.941 \left( 54.007-53.4 \right) =53.97^{o}C \] \[ p_{8,8}= \left( 1-0.941 \right) 0.1594=0.0094 \] | \[ \hat{x}_{9,8}= \hat{x}_{8,8}=53.97^{o}C \] \[ p_{9,8}= 0.0094+0.15=0.1594 \] |
| 9 | \( 54.523^{o}C \) | \[ K_{9}= \frac{0.1594}{0.1594+0.01}=0.941 \] \[ \hat{x}_{9,9}=~ 53.97+0.941 \left( 54.523-53.97 \right) =54.49^{o}C \] \[ p_{9,9}= \left( 1-0.941 \right) 0.1594=0.0094 \] | \[ \hat{x}_{10,9}= \hat{x}_{9,9}=54.49^{o}C \] \[ p_{10,9}= 0.0094+0.15=0.1594 \] |
| 10 | \( 54.99^{o}C \) | \[ K_{10}= \frac{0.1594}{0.1594+0.01}=0.941 \] \[ \hat{x}_{10,10}=~ 54.49+0.941 \left( 54.99 -54.49 \right) =54.96^{o}C \] \[ p_{10,10}= \left( 1-0.941 \right) 0.1594=0.0094 \] | \[ \hat{x}_{11,10}= \hat{x}_{10,10}=54.96^{o}C \] \[ p_{11,10}= 0.0094+0.15=0.1594 \] |
La siguiente gráfica compara el valor verdadero, los valores medidos y las estimaciones.
Como puede ver, las estimaciones siguen las mediciones. No hay error de retraso.
La siguiente gráfica muestra la ganancia de Kalman.
Debido a la alta incertidumbre del proceso, el peso de medición es mucho más alto que el peso estimado, por lo tanto, la ganancia de Kalman es alta y converge a 0.94.
Resumen del ejemplo
Podemos deshacernos del error de retraso configurando la alta incertidumbre del proceso. Sin embargo, dado que nuestro modelo no está bien definido, obtenemos estimaciones ruidosas que son casi iguales a las mediciones, y perdemos el objetivo del filtro de Kalman.
La mejor implementación del filtro Kalman implicará el modelo que está muy cerca de la realidad, dejando un pequeño espacio para el ruido del proceso. Sin embargo, el modelo preciso no siempre está disponible, por ejemplo, el piloto del avión puede decidir realizar una maniobra repentina que cambiará la trayectoria prevista del avión. En este caso, se incrementará el ruido del proceso.