The \( \alpha -\beta -\gamma \) filter
Ejemplo 1 – Pesando Oro
Ahora estamos listos para el primer ejemplo simple. En este ejemplo, vamos a estimar el estado de un sistema estático. El sistema estático es un sistema que no cambia su estado durante un período de tiempo razonable. Por ejemplo, el sistema estático puede ser una torre, y el estado es su altura.
En este ejemplo, vamos a estimar el peso de la barra de oro. Tenemos una balanza no sesgada, es decir, las mediciones no tienen un error sistemático, pero incluyen ruido aleatorio.
En este ejemplo, el sistema es la barra de oro y el estado del sistema es el peso de la barra de oro. El modelo dinámico del sistema es constante, ya que suponemos que el peso no cambia en un período corto de tiempo.
Para estimar el estado del sistema (es decir, el valor del peso), podemos hacer múltiples mediciones y promediarlas.
Al momento \( n \), la estimación \( \hat{x}_{n,n} \) sería el promedio de todas las mediciones anteriores:
| \( x \) | Es el valor verdadero del peso |
| \( z_{n} \) | Es el valor de la medición en el instante \( n \) |
| \( \hat{x}_{n,n} \) | Es la estimación de \( x \) en el tiempo \( n \) (la estimación se hace luego de tomar \( z_{n} \) mediciones) |
| \( \hat{x}_{n,n-1} \) | Es la estimación previa de \( x \) que se realizó en el tiempo \( n-1 \) (la estimación se realizó después de tomar la medición \( z_{n-1} \)) |
| \( \hat{x}_{n+1,n} \) | Es la estimación del estado futuro (\( n+1 \) ) de \( x \). La estimación se realiza en el momento \( n \), inmediatamente después de haber obtenido la medición \( z_{n} \). Dicho de otra manera, \( \hat{x}_{n+1,n} \) es un estado predicho |
El modelo dinámico en este ejemplo es constante, por lo tanto \( \hat{x}_{n+1,n}= \hat{x}_{n,n} \).
Para estimar \( \hat{x}_{n,n} \) necesitamos recordar todas las mediciones históricas. Supongamos que no tenemos un bolígrafo y un papel para anotar las mediciones, tampoco podemos confiar en nuestra memoria y recordar todas las mediciones históricas. Solo queremos usar la estimación anterior y agregar un pequeño ajuste (en una aplicación de la vida real, queremos ahorrar memoria de la computadora). Podemos hacer eso con un pequeño truco matemático:
| Notes | |
|---|---|
| \( \hat{x}_{n,n}= \frac{1}{n} \sum _{i=1}^{n} \left( z_{i} \right) = \) | Formula del promedio: sumatoria de \( n \) términos sobre la cantidad de mediciones |
| \( = \frac{1}{n} \left( \sum _{i=1}^{n-1} \left( z_{i} \right) + z_{n} \right) = \) | Sumatoria de los \( n - 1 \) términos anteriores más la última medición |
| \( = \frac{1}{n} \sum _{i=1}^{n-1} \left( z_{i} \right) + \frac{1}{n} z_{n} = \) | Aplico distributiva de \( \frac{1}{n} \) |
| \( = \frac{1}{n}\frac{n-1}{n-1} \sum _{i=1}^{n-1} \left( z_{i} \right) + \frac{1}{n} z_{n} = \) | Multiplico y divido el primer término por \( n - 1 \) |
| \( \frac{n-1}{n}\color{#FF8C00}{\frac{1}{n-1} \sum _{i=1}^{n-1} \left( z_{i} \right)} + \frac{1}{n} z_{n} = \) |
Reordeno Reordeno y observo que el término "naranja" es el promedio del instante anterior \( n - 1 \) |
| \( = \frac{n-1}{n}\color{#FF8C00}{\hat{x}_{n-1,n-1}} + \frac{1}{n} z_{n} = \) | Reescribiendo la sumatoria como su estimado |
| \( = \hat{x}_{n-1,n-1}- \frac{1}{n}\hat{x}_{n-1,n-1}+ \frac{1}{n} z_{n} = \) | Distribuyo el termino \( \frac{n-1}{n} \) |
| \( = \hat{x}_{n-1,n-1}+ \frac{1}{n} \left( z_{n}- \hat{x}_{n-1,n-1} \right) \) | Reorder |
\( \hat{x}_{n-1,n-1} \) es el estado estimado de \( x \) en el momento \( n - 1\), basado en la medición en el momento \( n-1 \).
Encontremos \( \hat{x}_{n,n-1} \) (el estado predicho de \( x \) en el momento \( n \) ), con base en \( \hat{x}_{n-1,n-1} \) (la estimación en el momento \( n - 1 \)). En otras palabras, nos gustaría extrapolar \( \hat{x}_{n-1,n-1} \) al momento n.
Dado que el modelo dinámico de este ejemplo es estático, es decir, el peso de la barra de oro no cambia con el tiempo, el estado predicho de \( x \) es igual al estado estimado de \( x \): \( \hat{x}_{n,n-1} = \hat{x}_{n-1,n-1} \).
Con base en lo anterior, la estimación del estado actual \( \hat{x}_{n,n} \), se puede escribir de la siguiente manera:
La ecuación anterior es una de las cinco ecuaciones de filtro de Kalman. Se llama la ecuación de actualización de estado. Significa lo siguiente:
El factor \( \frac{1}{n} \) es específico para nuestro ejemplo. Discutiremos el importante papel de este factor más adelante, pero en este momento me gustaría señalar que en el filtro de Kalman, este factor se llama Ganancia de Kalman. ISe denota por \( K_{n} \). El subíndice \( n \) indica que la ganancia de Kalman puede cambiar con cada iteración.
El descubrimiento del \( K_{n} \) fue una de las principales contribuciones de Rudolf Kalman.
Mientras tanto, antes de entrar en las entrañas del filtro de Kalman, utilizaremos la letra griega \( \alpha _{n} \) en lugar de \( K_{n} \).
Entonces, la ecuación de actualización de estado se verá así:
El término \( \left( z_{n}- x_{n,n-1} \right) \) es una medida residual, también llamada innovación. La innovación contiene la nueva información.
En nuestro ejemplo, \( \frac{1}{n} \) disminuye a medida que \( n \) aumenta. Esto significa que al principio, no tenemos suficiente información sobre el valor del peso, por lo tanto, basamos la estimación en las mediciones. A medida que continuamos, cada medición sucesiva tiene menos peso en el proceso de estimación, ya que \( \frac{1}{n} \) disminuye. En algún momento, la contribución de las nuevas mediciones sería insignificante.
Vamos a continuar con el ejemplo. Antes de hacer la primera medición, podemos adivinar (o estimar aproximadamente) el peso de la barra de oro simplemente leyendo el sello en la barra de oro. Se llama una conjetura inicial y será nuestra primera estimación.
Como veremos más adelante, el filtro de Kalman requiere la suposición inicial como valor predeterminado, y puede ser muy compleja.
Algoritmo de estimación
El siguiente esquema muestra el algoritmo de estimación que se utiliza en este ejemplo.
Ahora estamos listos para comenzar el proceso de medición y estimación.
Ejemplo Numerico
Iteracion Cero
Inicializacion
Nuestra estimación inicial del peso de la barra de oro es de 1000 gramos. La suposición inicial se usa solo una vez para el inicio del filtro, por lo que no será necesaria para las próximas iteraciones.
\[ \hat{x}_{0,0}=1000g \]
Predicción
El peso de la barra de oro no debe cambiar. Por lo tanto, el modelo dinámico del sistema es en realidad estático. Nuestra próxima estimación de estado (predicción) es igual a la inicialización:
\[ \hat{x}_{1,0} = \hat{x}_{0,0}=1000g \]
Primera Iteracion
Paso 1
Hacer la medición de peso con la balanza:
\[ z_{1}= 1030g \]
Paso 2
Cálculo de la ganancia. En nuestro ejemplo \( \alpha _{n}= \frac{1}{n} \) , entonces:
\[ \alpha _{1}= \frac{1}{1}=1 \]
Cálculo de la estimación actual utilizando la ecuación de actualización de estado:
\[ \hat{x}_{1,1}= \hat{x}_{1,0}+ \alpha _{1} \left( z_{1}- \hat{x}_{1,0} \right) =1000+1 \left( 1030-1000 \right) =1030g \]
Paso 3
El modelo dinámico del sistema es estático, por lo que no se supone que cambie el peso de la barra de oro. Nuestra próxima estimación del estado (predicción) es igual a la estimación del estado actual:
\[ \hat{x}_{2,1}= \hat{x}_{1,1}=1030g \]
Segunda Iteracion
Después de período de tiempo, la estimación prevista de la iteración anterior se convierte en la estimación en la iteración actual:
\[ \hat{x}_{2,1}=1030g \]
Paso 1
Hacemos la segunda medición del peso:
\[ z_{2}= 989g \]
Paso 2
Cálculo de la ganancia:
\[ \alpha _{2}= \frac{1}{2} \]
Cálculo de la estimación actual:
\[ \hat{x}_{2,2}= \hat{x}_{2,1}+ \alpha _{2} \left( z_{2}- \hat{x}_{2,1} \right) =1030+\frac{1}{2} \left( 989-1030 \right) =1009.5g \]
Paso 3
\[ \hat{x}_{3,2}= \hat{x}_{2,2}=1009.5g \]
Tercera Iteración
\[ z_{3}= 1017g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha _{3}= \frac{1}{3} \] \[ \hat{x}_{3,3}=~ 1009.5+\frac{1}{3} \left( 1017-1009.5 \right) =1012g \] \[ \hat{x}_{4,3}=1012g \]
Cuarta Iteración
\[ z_{4}= 1009g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha _{4}= \frac{1}{4} \] \[ \hat{x}_{4,4}= 1012+\frac{1}{4} \left( 1009-1012 \right) =1011.25g \] \[ \hat{x}_{5,4}=1011.25g \]
Quinta Iteración
\[ z_{5}= 1013g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha _{5}= \frac{1}{5} \] \[ \hat{x}_{5,5}= 1011.25+\frac{1}{5} \left( 1013-1011.25 \right) =1011.6g \] \[ \hat{x}_{6,5}=1011.6g \]
Sexta Iteración
\[ z_{6}= 979g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha _{6}= \frac{1}{6} \] \[ \hat{x}_{6,6}= 1011.6+\frac{1}{6} \left( 979-1011.6 \right) =1006.17g \] \[ \hat{x}_{7,6}=1006.17g \]
Séptima Iteración
\[ z_{7}=1008g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha _{7}= \frac{1}{7} \] \[ \hat{x}_{7,7}= 1006.17+\frac{1}{7} \left( 1008-1006.17 \right) =1006.43g \] \[ \hat{x}_{8,7}=1006.43g \]
Octava Iteración
\[ z_{8}=1042g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha _{8}= \frac{1}{8} \] \[ \hat{x}_{8,8}= 1006.43+\frac{1}{8} \left( 1042-1006.43 \right) =1010.87g \] \[ \hat{x}_{9,8}=1010.87g \]
Novena Iteración
\[ z_{9}=1012g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha _{9}= \frac{1}{9} \] \[ \hat{x}_{9,9}= 1010.87+\frac{1}{9} \left( 1012-1010.87 \right) =1011g \] \[ \hat{x}_{10,9}=1011g \]
Décima Iteración
\[ z_{10}=1011g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha _{10}= \frac{1}{10} \] \[ \hat{x}_{10,10}= 1011+\frac{1}{10} \left( 1011-1011 \right) =1011g \] \[ \hat{x}_{11,10}=1011g \]
Podemos parar aquí. La ganancia disminuye con cada medición. Por lo tanto, la contribución de cada medición sucesiva es menor que la contribución de la medición anterior. Nos acercamos bastante a un peso verdadero que es 1010 gramos. Si estuviéramos haciendo más mediciones, nos acercaríamos aún mas.
La siguiente tabla resume nuestras medidas y estimaciones, y la gráfica compara los valores medidos, las estimaciones y el valor verdadero.
| \( n \) | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 |
|---|---|---|---|---|---|---|---|---|---|---|
| \( \alpha _{n} \) | \( 1 \) | \( \frac{1}{2} \) | \( \frac{1}{3} \) | \( \frac{1}{4} \) | \( \frac{1}{5} \) | \( \frac{1}{6} \) | \( \frac{1}{7} \) | \( \frac{1}{8} \) | \( \frac{1}{9} \) | \( \frac{1}{10} \) |
| \( z_{n} \) | 1030 | 989 | 1017 | 1009 | 1013 | 979 | 1008 | 1042 | 1012 | 1011 |
| \( \hat{x}_{n,n} \) | 1030 | 1009.5 | 1012 | 1011.25 | 1011.6 | 1006.17 | 1006.43 | 1010.87 | 1011 | 1011 |
| \( \hat{x}_{n+1,n} \) | 1030 | 1009.5 | 1012 | 1011.25 | 1011.6 | 1006.17 | 1006.43 | 1010.87 | 1011 | 1011 |
Podemos ver que nuestro algoritmo de estimación tiene un efecto de suavizado en las mediciones y converge hacia el valor verdadero.
Resumen del ejemplo
En este ejemplo, hemos desarrollado un algoritmo de estimación simple para un sistema estático. También hemos derivado la ecuación de actualización de estado, que es una de las cinco ecuaciones de filtro de Kalman.
Ejemplo 2 – Seguimiento de un avión con velocidad constante en una dimensión
Ahora, es hora de examinar un sistema dinámico que cambia su estado con el tiempo. En este ejemplo, vamos a rastrear el avión de velocidad constante en una dimensión usando el filtro α-β.
Asumamos un mundo unidimensional. Suponemos que un avión se está alejando radialmente del radar (o hacia el radar). En el mundo unidimensional, el ángulo del radar es constante y la altitud de la aeronave es constante, como se muestra en la siguiente figura.
\( x_{n} \) representa la posición de la aeronave en el momento n. La velocidad del avión se define como la tasa de cambio de la posición con respecto al tiempo, por lo tanto, la velocidad es una derivada de la posición:
El radar envía un haz de seguimiento en la dirección del objetivo a una tasa constante. El período del envío del haz es \( \Delta t \).
Suponiendo una velocidad constante, el modelo dinámico del sistema se puede describir mediante dos ecuaciones de movimiento:
Según las ecuaciones, la posición del avión en el siguiente ciclo de seguimiento es igual a la posición en el ciclo de seguimiento actual más la velocidad objetivo multiplicada por el intervalo de tiempo del seguimiento. En este ejemplo, asumimos una velocidad constante. Por lo tanto, la velocidad en el siguiente ciclo es igual a la velocidad en el ciclo actual.
El sistema de ecuaciones anterior se llama ecuación de extrapolación de estado (también llamada ecuación de transición o ecuación de predicción) y esta es una de las cinco ecuaciones de filtro de Kalman. Este sistema de ecuaciones extrapola el estado actual al siguiente estado (predicción).
Ya hemos usado la ecuación de extrapolación de estado en el ejemplo anterior, donde asumimos que el peso en el siguiente estado es igual al peso en el estado actual.
Hay una forma general de la ecuación de extrapolación de estado en notación matricial, la aprenderemos más adelante.
En este ejemplo, utilizaremos el conjunto de ecuaciones anterior que es específico para nuestro caso.
- Ecuación de actualización de estado
- Ecuación de extrapolación de estado
Ahora vamos a modificar la ecuación de actualización de estado para nuestro ejemplo.
El filtro \( \alpha - \beta \)
Asumimos que el período entre envíos del haz de ( \( \Delta t \) ) del radar sea de 5 segundos. Supongamos que en el momento \( n - 1 \) la posición estimado del avión es de 30,000m y la velocidad estimada es de 40 m/s.
Usando las Ecuaciones de Extrapolación de Estado podemos predecir la posición objetivo en el momento \( n \):
\[ \hat{x}_{n,n-1}= \hat{x}_{n-1,n-1}+ \Delta t\hat{\dot{x}}_{n-1,n-1}=30000+5\ast40=30200m \]
La velocidad objetivo en el momento \( n \):
\[ \hat{\dot{x}}_{n,n-1}= \hat{\dot{x}}_{n-11,n-1}=40m/s \]
Sin embargo, en el momento \( n \) el radar mide el rango ( \( z_{n} \) ) de 30,110m y no de 30,200m como se esperaba. Hay una brecha de 90m entre el rango esperado (previsto) y el rango medido. Hay dos posibles razones para esta brecha:
- Las mediciones de radar no son precisas
- La velocidad del avión ha cambiado. La nueva velocidad del avión es: \( \frac{30,110-30,000}{5}=22m/s \)
¿Cuál de las dos afirmaciones es verdadera?
Anotemos la ecuación de actualización de estado para la velocidad:
El valor de un factor \( \beta \) depende del nivel de precisión del radar. Suponga que la precisión \( 1 \sigma \) del radar es de 20m. Lo más probable es que la brecha de 90 metros entre el rango predicho y el rango medido sea el resultado del cambio en la velocidad de la aeronave. En este caso, establecemos un valor alto para el factor \( \beta \) Si establecemos \( \beta \) = 0.9, entonces la velocidad estimada sería:
\[ \hat{\dot{x}}_{n,n}= \hat{\dot{x}}_{n,n-1}+ \beta \left( \frac{z_{n}-\hat{x}_{n,n-1}}{ \Delta t} \right) =40+0.9 \left( \frac{30110-30200}{5} \right) =23.8m/s \]
Suponga que la precisión \( 1 \sigma \) del radar es de 150m. Lo más probable es que la brecha de 90 metros se deba al error de medición del radar. En este caso, establecemos un valor bajo para el factor \( \beta \). Si establecemos \( \beta \) = 0.1, entonces la velocidad estimada sería:
\[ \hat{\dot{x}}_{n,n}= \hat{\dot{x}}_{n,n-1}+ \beta \left( \frac{z_{n}-\hat{x}_{n,n-1}}{ \Delta t} \right) =40+0.1 \left( \frac{30110-30200}{5} \right) =38.2m/s \]
La ecuación de actualización de estado para la posición de la aeronave es similar a la ecuación que se derivó en el ejemplo anterior:
Al contrario del ejemplo anterior, donde el factor \( \alpha \) se recalcula en cada iteración ( \( \alpha _{n}= \frac{1}{N} \) ), en este ejemplo el factor \( \alpha \) es constante.
La magnitud del factor \( \alpha \) depende de la precisión de la medición del radar. Para el radar de alta precisión, elegiremos un alto \( \alpha \) , dando un alto peso a las mediciones. Si \( \alpha = 1 \) , entonces el rango estimado es igual al rango medido:
\[ \hat{x}_{n,n}=~ \hat{x}_{n,n-1}+ 1 \left( z_{n}- \hat{x}_{n,n-1} \right) = z_{n} \]
Si \( \alpha =0 \) , entonces la medición no tiene significado:
\[ \hat{x}_{n,n}=~ \hat{x}_{n,n-1}+ 0 \left( z_{n}- \hat{x}_{n,n-1} \right) = x_{n,n-1} \]
Entonces, hemos derivado un sistema de ecuaciones que compone la ecuación de estado de actualización para el rastreador de radar. También se denominan ecuaciones de actualización de seguimiento \( \alpha - \beta \) or ecuaciones de filtrado de seguimiento \( \alpha - \beta \).
La ecuación de actualización de estado para la velocidad:
Algoritmo de Estimación
El siguiente esquema muestra el algoritmo de estimación que se utiliza en este ejemplo.
Al contrario del ejemplo anterior, los valores de ganancia \( \alpha \) y \( \beta \) se dan para este ejemplo. En el filtro de Kalman, la \( \alpha \) y \( \beta \) β se reemplazan por la ganancia de Kalman que se calcula en cada iteración, pero lo aprenderemos más adelante.
Ahora estamos listos para comenzar el ejemplo numérico.
Ejemplo numerico
Supongamos un avión que se está alejando radialmente del radar (o hacia el radar) en un sistema unidimensional.
Los parámetros del filtro \( \alpha - \beta \) son:
- \( \alpha =0.2 \)
- \( \beta =0.1 \)
El intervalo entre envíos de haz de radar es de es 5 segundos.
Iteracion Cero
Inicializacion
Se dan las condiciones iniciales para el tiempo \( n=0 \):
\[ \hat{x}_{0,0}=30000m \] \[ \hat{\dot{x}}_{0,0}=40m/s \]
Predicción
La suposición inicial se extrapolará al primer ciclo ( \( n=1 \) )utilizando las Ecuaciones de extrapolación de estado:
\[ \hat{x}_{n+1,n}= \hat{x}_{n,n}+ \Delta t\hat{\dot{x}}_{n,n} \rightarrow \hat{x}_{1,0}= \hat{x}_{0,0}+ \Delta t\hat{\dot{x}}_{0,0} =30000+5 \times 40=30200m \] \[ \hat{\dot{x}}_{n+1,n}= \hat{\dot{x}}_{n,n} \rightarrow \hat{\dot{x}}_{1,0}= \hat{\dot{x}}_{0,0} =40m/s \]
Primera Iteracion
En el primer ciclo ( \( n=1 \) ), la suposición inicial es la estimación anterior:
\[ \hat{x}_{n,n-1}=~ \hat{x}_{1,0}=30200m \] \[ \hat{\dot{x}}_{n,n-1}=~ \hat{\dot{x}}_{1,0}=40m/s \]
Paso 1
El radar mide la distancia al avión:
\[ z_{1}= 30110m \]
Paso 2
Cálculo de la estimación actual utilizando la ecuación de actualización de estado:
\[ \hat{x}_{1,1}=~ \hat{x}_{1,0}+ \alpha \left( z_{1}- \hat{x}_{1,0} \right) =30200+0.2 \left( 30110-30200 \right) =30182m \] \[ \hat{\dot{x}}_{1,1}= \hat{\dot{x}}_{1,0}+ \beta \left( \frac{z_{1}-\hat{x}_{1,0}}{ \Delta t} \right) = 40+0.1 \left( \frac{30110-30200}{5} \right) =38.2m/s \]
Paso 3
Cálculo de la próxima estimación de estado utilizando las Ecuaciones de extrapolación de estado:
\[ \hat{x}_{2,1}= \hat{x}_{1,1}+ \Delta t\hat{\dot{x}}_{1,1} =30182+5 \times 38.2=30373m \] \[ \hat{\dot{x}}_{2,1}= \hat{\dot{x}}_{1,1} =38.2m/s \]
Segunda Iteracion
Después de un lapso, 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}=30373m \] \[ \hat{\dot{x}}_{2,1}= 38.2m/s \]
Paso 1
El radar mide la distancia al avión:
\[ z_{2}= 30265m \]
Paso 2
Cálculo de la estimación actual utilizando la ecuación de actualización de estado:
\[ \hat{x}_{2,2}=~ \hat{x}_{2,1}+ \alpha \left( z_{2}- \hat{x}_{2,1} \right) =30373+0.2 \left( 30265-30373 \right) =30351.4m \] \[ \hat{\dot{x}}_{2,2}= \hat{\dot{x}}_{2,1}+ \beta \left( \frac{z_{2}-\hat{x}_{2,1}}{ \Delta t} \right) = 38.2+0.1 \left( \frac{30265-30373}{5} \right) =36m/s \]
Paso 3
Cálculo de la próxima estimación de estado utilizando las Ecuaciones de extrapolación de estado:
\[ \hat{x}_{3,2}= \hat{x}_{2,2}+ \Delta t\hat{\dot{x}}_{2,2} =30351.4+5 \times 36=30531.6m \] \[ \hat{\dot{x}}_{3,2}= \hat{\dot{x}}_{2,2} =36m/s \]
Tercera Iteración
\[ z_{3}= 30740 \] \[ \hat{x}_{3,3}=~ 30531.6+0.2 \left( 30740~ -30531.6 \right) =30573.3m \] \[ \hat{\dot{x}}_{3,3}= 36+0.1 \left( \frac{30740~ -30531.6}{5} \right) =40.2m/s \] \[ \hat{x}_{4,3}= 30573.3+5 \times 40.2=30774.3m \] \[ \hat{\dot{x}}_{4,3}= 40.2m/s \]
Cuarta Iteración
\[ z_{4}= 30750 \] \[ \hat{x}_{4,4}=~ 30774.3+0.2 \left( 30750-30774.3 \right) =30769.5m \] \[ \hat{\dot{x}}_{4,4}= 40.2+0.1 \left( \frac{30750-30774.3}{5} \right) =39.7m/s \] \[ \hat{x}_{5,4}= 30769.5+5 \times 39.7=30968.1m \] \[ \hat{\dot{x}}_{5,4}= 39.7m/s \]
Quinta Iteración
\[ z_{5}= 31135 \] \[ \hat{x}_{5,5}=~ 30968.1+0.2 \left( 31135 -30968.1 \right) =31001.5m \] \[ \hat{\dot{x}}_{5,5}= 39.7+0.1 \left( \frac{31135 -30968.1}{5} \right) =43.1m/s \] \[ \hat{x}_{6,5}= 31001.5+5 \times 43.1=31216.8m \] \[ \hat{\dot{x}}_{6,5}= 43.1m/s \]
Sexta Iteración
\[ z_{6}= 31015 \] \[ \hat{x}_{6,6}=~ 31216.8+0.2 \left( 31015 -31216.8 \right) =31176.4m \] \[ \hat{\dot{x}}_{6,6}= 43.1+0.1 \left( \frac{31015 -31216.8}{5} \right) =39m/s \] \[ \hat{x}_{7,6}= 31176.4+5 \times 39=31371.5m \] \[ \hat{\dot{x}}_{7,6}= 39m/s \]
Séptima Iteración
\[ z_{7}= 31180 \] \[ \hat{x}_{7,7}=~ 31371.5+0.2 \left( 31180 -31371.5 \right) =31333.2m \] \[ \hat{\dot{x}}_{7,7}= 39+0.1 \left( \frac{31180 -31371.5}{5} \right) =35.2m/s \] \[ \hat{x}_{8,7}= 31333.2+5 \times 35.2=31509.2m \] \[ \hat{\dot{x}}_{8,7}= 35.2m/s \]
Octava Iteración
\[ z_{8}= 31610 \] \[ \hat{x}_{8,8}=~ 31509.2+0.2 \left( 31610 -31509.2 \right) =31529.4m \] \[ \hat{\dot{x}}_{8,8}= 35.2+0.1 \left( \frac{31610 -31509.2}{5} \right) =37.2m/s \] \[ \hat{x}_{9,8}= 31529.4+5 \times 37.2=31715.4m \] \[ \hat{\dot{x}}_{9,8}= 37.2m/s \]
Novena Iteración
\[ z_{9}= 31960 \] \[ \hat{x}_{9,9}=~ 31715.4+0.2 \left( 31960 -31715.4 \right) =31764.3m \] \[ \hat{\dot{x}}_{9,9}= 37.2+0.1 \left( \frac{31960 -31715.4}{5} \right) =42.1m/s \] \[ \hat{x}_{10,9}= 31764.3+5 \times 42.1=31974.8m \] \[ \hat{\dot{x}}_{10,9}=42.1m/s \]
Décima Iteración
\[ z_{10}= 31865 \] \[ \hat{x}_{10,10}=~ 31974.8+0.2 \left( 31865 -31974.8 \right) =31952.9m \] \[ \hat{\dot{x}}_{10,10}= 42.1+0.1 \left( \frac{31865 -31974.8}{5} \right) =39.9m/s \] \[ \hat{x}_{11,10}= 31952.9+5 \times 39.9=32152.4m \] \[ \hat{\dot{x}}_{11,10}= 39.9m/s \]
La siguiente tabla resume nuestras mediciones y estimaciones.
| \( n \) | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 |
|---|---|---|---|---|---|---|---|---|---|---|
| \( z_{n} \) | 30110 | 30265 | 30740 | 30750 | 31135 | 31015 | 31180 | 31610 | 31960 | 31865 |
| \( \hat{x}_{n,n} \) | 30182 | 30351.4 | 30573.3 | 30769.5 | 31001.5 | 31176.4 | 31333.2 | 31529.4 | 31764.3 | 31952.9 |
| \( \dot{\hat{x}}_{n,n} \) | 38.2 | 36 | 40.2 | 39.7 | 43.1 | 39 | 35.2 | 37.2 | 42.1 | 39.9 |
| \( \hat{x}_{n+1,n} \) | 30373 | 30531.6 | 30774.3 | 30968.1 | 31216.8 | 31371.5 | 31509.2 | 31715.4 | 31974.8 | 32152.4 |
| \( \dot{\hat{x}}_{n+1,n} \) | 38.2 | 36.0 | 40.2 | 39.7 | 43.1 | 39 | 35.2 | 37.2 | 42.1 | 39.9 |
El siguiente grafico compara el valor verdadero, los valores medidos y las estimaciones.
Podemos ver que nuestro algoritmo de estimación tiene un efecto de suavizado en las mediciones y converge hacia el valor verdadero.
Utilizando alta \( \alpha \) y \( \beta \)
El siguiente gráfico muestra el valor verdadero, los valores medidos y las estimaciones para \( \alpha = 0.8 \) y \( \beta = 0.5 \).
El grado de "suavizado" de este filtro es mucho más bajo. La "estimación actual" está muy cerca de los valores medidos y los errores de estimación pronosticados son bastante grandes.
Entonces, ¿debemos elegir siempre valores bajos para \( \alpha \) y \( \beta \) ?
La respuesta es NO. El valor de \( \alpha \) y \( \beta \) dependerá de la precisión de la medición. Si utilizamos equipos muy precisos, como el radar láser, preferiríamos un alto \( \alpha \) y \( \beta \) que sigan las mediciones. En este caso, el filtro respondería rápidamente al cambio de velocidad del objetivo. Por otro lado, si la precisión de la medición es baja, preferiríamos una \( \alpha \) y \( \beta \) . En este caso, el filtro suavizará la incertidumbre (errores) en las mediciones. Sin embargo, la reacción del filtro a los cambios de velocidad del objetivo será mucho más lenta.
Dado que el cálculo de \( \alpha \) y \( \beta \) es un tema importante, lo discutiremos con más detalle más adelante.
Resumen del ejemplo
En este ejemplo, hemos derivado la ecuación de actualización del estado del filtro \( \alpha - \beta \). También hemos aprendido la ecuación de extrapolación de estado. Desarrollamos un algoritmo de estimación para un sistema dinámico unidimensional basado en el filtro \( \alpha - \beta \) y resolvimos un ejemplo numérico para el seguimiento de un objetivo de velocidad constante.
Ejemplo 3 – Seguimiento de un avión acelerando en una dimensión
En este ejemplo, vamos a rastrear el avión que se mueve con aceleración constante con el filtro \( \alpha - \beta \) que ya se explicó en el ejemplo anterior.
En el ejemplo anterior, hemos rastreado un avión que se mueve a una velocidad constante de 40m/s. Los siguientes gráficos muestran la posición y la velocidad del avión en función del tiempo.
Como puede ver, la función de posición es lineal.
Ahora examinemos otro avión. Este avión se mueve con una velocidad constante de 50m/s durante 15 segundos. Luego acelera con una aceleración constante de 8m/s2 durante otros 35 segundos.
El siguiente grafico muestra la posición, la velocidad y la aceleración frente al tiempo.
Como puede ver en el gráfico, la velocidad del avión es constante durante los primeros 15 segundos y luego crece linealmente. La posición crece linealmente durante los primeros 15 segundos y luego crece de forma cuadrática.
Vamos a rastrear este avión con el filtro α-β que se utilizó en el ejemplo anterior.
The numerical example
El avión se mueve radialmente hacia el radar (o lejos de) radar en un sistema unidimensional.
Los parámetros del filtro \( \alpha - \beta \) son:
- \( \alpha =0.2 \)
- \( \beta =0.1 \)
El intervalo entre envíos de haz de radar es de es 5 segundos.
Iteracion Cero
Inicializacion
Se dan las condiciones iniciales para el tiempo \( n=0 \) :
\[ \hat{x}_{0,0}=30000m \] \[ \hat{\dot{x}}_{0,0}=50m/s \]
Predicción
La suposición inicial se extrapolará al primer ciclo ( \( n=1 \) ) utilizando las Ecuaciones de extrapolación de estado:
\[ \hat{x}_{n+1,n}= \hat{x}_{n,n}+ \Delta t\hat{\dot{x}}_{n,n} \rightarrow \hat{x}_{1,0}= \hat{x}_{0,0}+ \Delta t\hat{\dot{x}}_{0,0} =30000+5 \times 50=30250m \] \[ \hat{\dot{x}}_{n+1,n}= \hat{\dot{x}}_{n,n} \rightarrow \hat{\dot{x}}_{1,0}= \hat{\dot{x}}_{0,0} =50m/s \]
Todas las iteraciones de filtro se resumen en la siguiente tabla:
| \( n \) | \( z_{n} \) | Estimación del estado actual ( \( \hat{x}_{n,n} \), \( \hat{\dot{x}}_{n,n} \) ) | Predicción ( \( \hat{x}_{n+1,n} \), \( \hat{\dot{x}}_{n+1,n} \) ) |
|---|---|---|---|
| 1 | \( 30160m \) | \[ \hat{x}_{1,1}=~ 30250+0.2 \left( 30160- 30250 \right) =30232m \] \[ \hat{\dot{x}}_{1,1}= 50+0.1 \left( \frac{30160 -30250}{5} \right) =48.2m/s \] | \[ \hat{x}_{2,1}= 30232+5 \times 48.2=30473m \] \[ \hat{\dot{x}}_{2,1}= 48.2m/s \] |
| 2 | \( 30365m \) | \[ \hat{x}_{2,2}=~ 30473+0.2 \left( 30365 -30473 \right) =30451.1m \] \[ \hat{\dot{x}}_{2,2}= 48.2+0.1 \left( \frac{30365 -30473}{5} \right) =46m/s \] | \[ \hat{x}_{3,2}= 30451.1+5 \times 46=30681.6m \] \[ \hat{\dot{x}}_{3,2}= 46m/s \] |
| 3 | \( 30890m \) | \[ \hat{x}_{3,3}=~ 30681.6+0.2 \left( 30890 -30681.6 \right) =30723.3m \] \[ \hat{\dot{x}}_{3,3}= 46+0.1 \left( \frac{30890 -30681.6}{5} \right) =50.2m/s \] | \[ \hat{x}_{4,3}= 30723.3+5 \times 50.2=30974.3m \] \[ \hat{\dot{x}}_{4,3}= 50.2m/s \] |
| 4 | \( 31050m \) | \[ \hat{x}_{4,4}=~ 30974.3+0.2 \left( 31050 -30974.3 \right) =30989.5m \] \[ \hat{\dot{x}}_{4,4}= 50.2+0.1 \left( \frac{31050 -30974.3}{5} \right) =51.7m/s \] | \[ \hat{x}_{5,4}= 30989.5+5 \times 51.7=31248.1m \] \[ \hat{\dot{x}}_{5,4}= 51.7m/s \] |
| 5 | \( 31785m \) | \[ \hat{x}_{5,5}=~ 31248.1+0.2 \left( 31785 -31248.1 \right) =31355.5m \] \[ \hat{\dot{x}}_{5,5}= 51.7+0.1 \left( \frac{31785 -31248.1}{5} \right) =62.5m/s \] | \[ \hat{x}_{6,5}= 31355.5+5 \times 62.5=31667.8m \] \[ \hat{\dot{x}}_{6,5}= 62.5m/s \] |
| 6 | \( 32215m \) | \[ \hat{x}_{6,6}=~ 31667.8+0.2 \left( 32215 -31667.8 \right) =31777.2m \] \[ \hat{\dot{x}}_{6,6}= 62.5+0.1 \left( \frac{32215 -31667.8}{5} \right) =73.4m/s \] | \[ \hat{x}_{7,6}= 31777.2+5 \times 73.4=32144.2m \] \[ \hat{\dot{x}}_{7,6}= 73.4m/s \] |
| 7 | \( 33130m \) | \[ \hat{x}_{7,7}=~ 32144.2+0.2 \left( 33130 -32144.2 \right) =32341.4m \] \[ \hat{\dot{x}}_{7,7}= 73.4+0.1 \left( \frac{33130 -32144.2}{5} \right) =93.1m/s \] | \[ \hat{x}_{8,7}= 32341.4+5 \times 93.1=32807m \] \[ \hat{\dot{x}}_{8,7}= 93.1m/s \] |
| 8 | \( 34510m \) | \[ \hat{x}_{8,8}=~ 32807+0.2 \left( 34510 -32807 \right) =33147.6m \] \[ \hat{\dot{x}}_{8,8}= 93.1+0.1 \left( \frac{34510 -32807}{5} \right) =127.2m/s \] | \[ \hat{x}_{9,8}= 33147.6+5 \times 127.2=33783.5m \] \[ \hat{\dot{x}}_{9,8}= 127.2m/s \] |
| 9 | \( 36010m \) | \[ \hat{x}_{9,9}=~ 33783.5+0.2 \left( 36010 -33783.5 \right) =34228.8m \] \[ \hat{\dot{x}}_{9,9}= 127.2+0.1 \left( \frac{36010 -33783.5}{5} \right) =171.7m/s \] | \[ \hat{x}_{10,9}= 34228.8+5 \times 171.7=35087.4m \] \[ \hat{\dot{x}}_{10,9}= 171.7m/s \] |
| 10 | \( 37265m \) | \[ \hat{x}_{10,10}=~ 35087.4+0.2 \left( 37265 -35087.4 \right) =35522.9m \] \[ \hat{\dot{x}}_{10,10}= 171.7+0.1 \left( \frac{37265 -35087.4}{5} \right) =215.3m/s \] | \[ \hat{x}_{11,10}= 35522.9+5 \times 215.3=36559.2m \] \[ \hat{\dot{x}}_{11,10}= 215.3m/s \] |
Los siguientes gráficos comparan el valor verdadero, los valores medidos y las estimaciones para la posición y la velocidad durante los primeros 75 segundos.
Puede ver una brecha constante entre los valores verdaderos o medidos y los valores estimados. La brecha se llama error de retraso. Otros nombres comunes para el error de retraso son:
- Error dinámico
- Error sistemático
- Error de sesgo
- Error de truncamiento
Resumen del ejemplo
En este ejemplo, hemos examinado el error de retraso causado por la aceleración constante.
Ejemplo 4 – Seguimiento de un avión acelerando con el filtro \( \alpha -\beta -\gamma \)
En este ejemplo, vamos a rastrear el avión que se mueve con aceleración constante con el filtro \( \alpha -\beta -\gamma \).
El filtro \( \alpha -\beta -\gamma \)
El filtro \( \alpha -\beta -\gamma \) (a veces llamado el filtro g-h-k) considera la aceleración del objetivo, por lo que las ecuaciones de extrapolación de estado se convierten en:
Los siguientes cuadros comparan el valor verdadero, los valores medidos y las estimaciones para el rango, la velocidad y la aceleración durante los primeros 50 segundos.
Como puede ver, el filtro \( \alpha -\beta -\gamma \) con ecuaciones de modelo dinámico que incluyen aceleración puede seguir al objetivo con aceleración constante y eliminar el error de retraso.
Pero, ¿qué sucederá en el caso de un objetivo de maniobra? El objetivo puede cambiar repentinamente la dirección del vuelo haciendo un giro. El verdadero modelo dinámico de destino también puede incluir un tirón (cambio de aceleración). En tales casos, el filtro \( \alpha -\beta -\gamma \) con coeficientes \( \alpha -\beta -\gamma \) constantes producirá los errores de estimación y, en algunos casos, perderá al objetivo.
El filtro de Kalman puede manejar la incertidumbre en el modelo dinámico, y será nuestro próximo tema justo después del resumen.
Resumen del filtro \( \alpha -\beta -(\gamma) \)
Hay muchos tipos de filtros \( \alpha -\beta -(\gamma) \) y se basan en el mismo principio:
- La estimación del estado actual se basa en las ecuaciones de actualización del estado.
- La siguiente estimación de estado (predicción) se basa en las ecuaciones del modelo dinámico.
La principal diferencia entre estos filtros es la selección de coeficientes de ponderación \( \alpha -\beta -(\gamma) \). Algunos tipos de filtro usan coeficientes de ponderación constantes, otros coeficientes de ponderación de cálculo para cada iteración (ciclo) del filtro.
La elección de \( \alpha \), \( \beta \) y \( \gamma \) es crucial para la funcionalidad adecuada del algoritmo de estimación. Otra cuestión importante es el inicio del filtro, es decir, proporcionar el valor inicial para la primera iteración del filtro.
La siguiente lista incluye los filtros \( \alpha -\beta -(\gamma) \) más populares:
- Filtro Wiener
- Filtro Bayes
- Filtro polinomial de memoria de desvanecimiento
- Filtro polinomial de memoria expansiva (o memoria creciente)
- Filtro de mínimos cuadrados
- Benedict – Bordner Filter
- Filtro agrupado
- Mínimos cuadrados con descuento \( \alpha -\beta \) Filtro
- Filtro \( \alpha -\beta \) críticamente amortiguado
- Filtro de memoria creciente
- Filtro Kalman
- Filtro Kalman extendido
- Filtro Kalman sin perfume Unscented Kalman Filter
- Filtro Kalman complejo extendido
- Gauss-Hermite Kalman Filter
- Cubature Kalman Filter
- Filtro de partículas
Espero escribir un tutorial sobre algunos de estos filtros en el futuro. Pero este tutorial trata sobre el filtro de Kalman y este es el tema de nuestro próximo ejemplo.