FILTRO DE KALMAN EN TIEMPO

DISCRETO
MOTIVACIÓN
 PROBLEMA ORIGINAL

• Consideremos un sistema de control en tiempo discreto representado por la ecuación

𝑥𝑘+1 = 𝐹𝑘 𝑥𝑘
ቊ
𝑦𝑘 = 𝐻𝑘′ 𝑥𝑘

𝑥𝑘+1 𝑥𝑘 𝑦𝑘
𝐷𝑒𝑙𝑎𝑦 𝐻𝑘′

𝐹𝑘
 PROBLEMA ORIGINAL

• El sistema sobre el que estamos trabajando no tiene entradas. Nuestra única motivación es la
observación
• Formalicemos el concepto de observación
• Un observador provee un estimado del estado interno de un sistema real a partir de medidas
de su entrada y salida
• La obtención de una medida implica forzosamente el uso de un sensor
• Los sensores son ruidosos: no conocemos el “valor real” de una magnitud, en el mejor de los casos
conocemos su distribución de probabilidad
• Un modelo de un sistema real puede ser arbitrariamente complejo
• Es imposible modelar todos los factores que influyen sobre nuestro sistema
 ADECUACIÓN DEL MODELO

• En base a lo discutido, ajustaremos nuestro modelo

• Definiremos un proceso 𝑥𝑘 como el conjunto de pares (𝑥𝑘 , 𝑘)|𝑘 ≥ 0
• Agregaremos un proceso ruidoso {𝑣𝑘 }, llamado ruido de medida, que representa el ruido agregado
por el acto de sensar.
• Agregaremos otro proceso ruidoso 𝑤𝑘 , llamado ruido de proceso, que representa el ruido
introducido por las imprecisiones del modelo
 ADECUACIÓN DEL MODELO

• Las ecuaciones del sistema pasan a ser

𝑥𝑘+1 = 𝐹𝑘 𝑥𝑘 + 𝐺𝑘 𝑤𝑘
ቊ
𝑧𝑘 = 𝐻𝑘′ 𝑥𝑘 + 𝑣𝑘

𝑤𝑘 + 𝑥𝑘+1 𝑥𝑘 𝑦𝑘 + 𝑧𝑘
𝐺𝑘 Σ 𝐷𝑒𝑙𝑎𝑦 𝐻𝑘′ Σ
+ +

𝑣𝑘
𝐹𝑘
 PROBLEMA DE FILTRADO

• Solamente podemos obtener medidas la salida (𝑧𝑘 , o lo que es lo mismo, 𝑦𝑘 con ruido
añadido)
• Sea una función 𝑠 . no medible directamente, y una función 𝑧 . , correspondiente a la serie
de medidas disponibles de 𝑠 . , definimos el concepto de filtrado como la recuperación
de información de 𝑠 . a partir de la información disponible hasta el momento de 𝑧 .
• Dadas las medidas {𝑧0 , 𝑧1 , … , 𝑧𝑘 }, queremos estimar de la mejor manera posible 𝑥𝑘
• Resulta bastante intuitivo que no es posible hacer demasiado sin modelar los procesos de
ruido
MODELO ESTADÍSTICO EN TIEMPO
DISCRETO
 DESCRIPCIÓN DEL RUIDO

• 𝑣𝑘 y 𝑤𝑘 son procesos independientes, gaussianos con media y covarianza dadas

según las ecuaciones
• 𝑚𝑣 = 𝐸 𝑣 = 0
• 𝑚𝑤 = 𝐸 𝑤 = 0
• 𝜎𝑣𝑘,𝑙 = 𝐸 𝑣𝑘 − 𝑚𝑣 𝑣𝑙 − 𝑚𝑣 ′ = 𝑅𝐾 𝛿𝑘𝑙

• 𝜎𝑤𝑘,𝑙 = 𝐸 𝑤𝑘 − 𝑚𝑤 𝑤𝑙 − 𝑚𝑤 ′ = 𝑄𝐾 𝛿𝑘𝑙

• Se puede observar que

• Ambos procesos tienen media nula
• Ambos procesos son blancos
 DESCRIPCIÓN DEL ESTADO INICIAL

• Normalmente, la condición inicial de un sistema se da como un vector 𝑥0 dado para 𝑘 = 0

• Esta presunción no funciona en la práctica para este tipo de sistemas
• Si es imposible medir 𝑥𝑘 para un 𝑘 arbitrario, cómo podemos esperar medir 𝑥0 ?
• Para solucionar este inconveniente, se adopta una condición inicial aleatoria, definida según
una variable aleatoria gaussiana.
• 𝐸 𝑥0 = 𝑥ҧ 0
′
• 𝐸 𝑥0 − 𝑥ҧ 0 𝑥0 − 𝑥ҧ 0 = 𝑃0
• Cómo se llega a este resultado?
• Porque funciona. Una mezcla de experiencia y practicidad
 PROPAGACIÓN DE PROPIEDADES
ESTADÍSTIC AS

• 𝑥𝑘 es una variable aleatoria gaussiana

𝑘−1

𝑥𝑘 = Φ𝑘,0 𝑥0 + ෍ Φ𝑘,𝑙+1 𝐺𝑙 𝑤𝑙
𝑙=0

Con Φ𝑘,𝑙 = 𝐹𝑘−1 𝐹𝑘−2 𝐹𝑙 la matriz de evolución de estados (Φ𝑘,𝑘 = 𝐼).

• Notar que si 𝐹 es invariante en el tiempo, Φ𝑘,𝑙 = 𝐹 𝑘−𝑙
• Este resultado se desprende de que 𝑥𝑘 es combinación lineal de 𝑥0 , 𝑤0 , 𝑤1 , … , 𝑤𝑘−1 [p18]
• 𝑥𝑘 es un proceso aleatorio gaussiano. Se desprende del resultado anterior.
• 𝑥𝑘 es un proceso de Markov
• “El futuro de {𝑥𝑘 } es independiente de su pasado, dado su presente”
• 𝑥ҧ𝑘 = 𝐸 𝑥𝑘 = Φ𝑘,0 𝑥ҧ0
′ ′
• 𝑃𝑘 = 𝐸 𝑥𝑘 − 𝑥ҧ𝑘 𝑥𝑘 − 𝑥ҧ𝑘 = Φ𝑘,0 𝑃0 Φ𝑘,0 + σ𝑘−1 ′
𝑚=0 Φ𝑘,𝑚+1 𝐺𝑚 𝑄𝑚 𝐺𝑚 Φ𝑘,𝑚+1
[p20]
 PROPAGACIÓN DE PROPIEDADES
ESTADÍSTIC AS

• 𝑧𝑘 es una variable aleatoria gaussiana

𝑧𝑘 = 𝐻𝑘′ 𝑥𝑘 + 𝑣𝑘
• 𝑧𝑘 es un proceso aleatorio gaussiano, por las mismas razones que 𝑥𝑘
• De hecho, 𝑥𝑘 y 𝑧𝑘 son conjuntamente gaussianos
• 𝑧𝑘 NO es un proceso de Markov
• Esto se debe a que {𝑦𝑘 } y {𝑥𝑘 } no son blancos. La correlación entre dos valores consecutivos no es
nula
• 𝑧𝑘ҧ = 𝐸 𝑧𝑘 = 𝐻𝑘′ 𝑥ҧ𝑘
𝐻𝑘′ Φ𝑘,𝑙 𝑃𝑙 𝐻𝑙 + 𝑅𝑘 𝛿𝑘𝑙 𝑠𝑖 𝑘 ≥ 𝑙 [p20]
• 𝐸 𝑧𝑘 − 𝑧𝑘ҧ 𝑧𝑙 − 𝑧𝑙ҧ =൝ ′ ′
𝐻𝑘 𝑃𝑘 Φ𝑙,𝑘 𝐻𝑙 + 𝑅𝑘 𝛿𝑘𝑙 𝑠𝑖 𝑘 ≤ 𝑙
 PROPAGACIÓN DE PROPIEDADES
ESTADÍSTIC AS

• Hasta el momento
• Planteamos la estructura del sistema
• Evidenciamos el problema de no poder medir el vector de estados 𝑥𝑘
• Planteamos las características de los procesos 𝑣𝑘 y 𝑤𝑘 , y dedujimos las características de 𝑥𝑘 y
𝑧𝑘
• En este escenario, lo mejor que podemos hacer es estimar 𝑥𝑘 , usando la información que
tenemos disponible
• Puede resultar evidente que existe una correlación entre los valores que toma el proceso {𝑦𝑘 } y los
que adquiere 𝑥𝑘 (después de todo, los sensores no son tan malos)
• Cómo podemos formalizar esta información que identificamos informalmente?
 PROBABILIDAD CONDICIONAL

• Probabilidad condicional
• Dadas dos variables aleatorias, que nos dice el saber que 𝑌 = 𝑦 sobre el valor que adquiere 𝑋?
• Densidad de probabilidad condicional
𝑝𝑋,𝑌 (𝑥, 𝑦)
𝑝𝑋|𝑌 𝑥 𝑦 =
𝑝𝑌 𝑦
La notación puede ser un poco confusa. No confundir las variables aleatorias (mayúsculas) con los
valores que toma (minúsculas)
𝑋
• Sea 𝑍 = . Entonces 𝑝𝑧 𝑧 = 𝑝𝑋,𝑌 𝑥, 𝑦
𝑌
𝑥ҧ
• 𝑧ҧ =
𝑦ത
Σ𝑥𝑥 Σxy
• Σ=
Σ𝑦𝑥 Σyy

• dim 𝑋 = 𝑁, dim 𝑌 = 𝑀
 PROBABILIDAD CONDICIONAL

1 𝑥−𝑥ҧ 𝑥−𝑥ҧ ′
1 −2 𝑦−𝑦ത Σ−1 𝑦−𝑦ത
• 𝑝𝑋,𝑌 𝑥, 𝑦 = 𝑁+𝑀 1 𝑒
2𝜋 2 Σ2
1
1 −2 𝑦−𝑦ത Σ−1 𝑦−𝑦ത ′
• 𝑝𝑌 𝑦 = 𝑀 1 𝑒
2𝜋 2 Σyy 2

• Operando se puede demostrar que 𝑝𝑋|𝑌 𝑥 𝑦 es gaussiana [p25], con

−1
• 𝑥|𝑦 = 𝑥ҧ + Σ𝑥𝑦 Σ𝑦𝑦 𝑦 − 𝑦ത
−1
• Σ𝑥|𝑦 = Σ𝑥𝑥 − Σ𝑦𝑥 Σ𝑦𝑦 Σ𝑥𝑦

• 𝑝𝑋|𝑌 𝑥 𝑦 resume toda la información que el hecho 𝑌 = 𝑦 contiene sobre 𝑋, pero no nos da
un estimado del valor de 𝑥. Cómo podemos obtener un estimado de 𝑥?
• Una opción obvia es elegir 𝑥 ∗ | 𝑝𝑋|𝑌 𝑥 ∗ 𝑦 = max 𝑝𝑋|𝑌 𝑥 𝑦
• En nuestro caso, vamos a usar otra
 ESTIMADOR DE MÍNIMA VARIANZA

• Sea 𝑥ො un estimado del valor tomado por 𝑋 (obtenido de alguna manera)

• En la enorme mayoría de los casos, 𝑥ො no será igual al valor que tomará 𝑋
• Podemos obtener una media del error con la ecuación

2
𝐸 𝑋 − 𝑥ො |𝑌 = 𝑦
• Esta ecuación nos permite obtener una medida de la “calidad” de una estimación
• Estimador de mínima varianza 𝒙
ෝ
• Definimos al estimador de mínima varianza como aquel que cumple

2
𝐸 𝑋 − 𝑥ො |𝑌 = 𝑦 ≤ 𝐸 𝑋 − 𝑥 ∗ 2
|𝑌 = 𝑦
• Puede probarse[p26] que

+∞
𝑥ො = 𝐸 𝑋|𝑌 = 𝑦 = න 𝑥𝑝𝑥|𝑦 (𝑥|𝑦) 𝑑𝑥
−∞
 ESTIMADOR DE MÍNIMA VARIANZA

• Sea 𝑥ො un estimado del valor tomado por 𝑋 (obtenido de alguna manera)

• En la enorme mayoría de los casos, 𝑥ො no será igual al valor que tomará 𝑋
• Podemos obtener una media del error con la ecuación

2
𝐸 𝑋 − 𝑥ො |𝑌 = 𝑦
• Esta ecuación nos permite obtener una medida de la “calidad” de una estimación
• Estimador de mínima varianza 𝒙
ෝ
• Definimos al estimador de mínima varianza como aquel que cumple

2
𝐸 𝑋 − 𝑥ො |𝑌 = 𝑦 ≤ 𝐸 𝑋 − 𝑥 ∗ 2
|𝑌 = 𝑦
• Puede probarse[p26] que

+∞
𝑥ො = 𝐸 𝑋|𝑌 = 𝑦 = න 𝑥𝑝𝑥|𝑦 (𝑥|𝑦) 𝑑𝑥
−∞
 ESTIMADOR DE MÍNIMA VARIANZA

• Acabamos de determinar una función capaz de estimar el valor de x minimizando la varianza

del error

𝑋෠ = 𝐸 𝑋|𝑌
• 𝑋෠ es un estimador de 𝑋 en términos de 𝑌
PROBLEMA DE FILTRADO DE
KALMAN
 PROBLEMA DE FILTRADO DE KALMAN EN
TIEMPO DISCRETO

• Estamos en condiciones de plantear formalmente el problema de filtrado de Kalman en

tiempo discreto
• Sea el sistema lineal en tiempo discreto descrito por el sistema de ecuaciones
𝑥𝑘+1 = 𝐹𝑘 𝑥𝑘 + 𝐺𝑘 𝑤𝑘
ቊ , ∀𝑘 ≥ 0
𝑧𝑘 = 𝐻𝑘′ 𝑥𝑘 + 𝑣𝑘
Donde:
• 𝑤𝑘 y {𝑣𝑘 } son procesos independientes, gaussianos, de media nula y con 𝐸 𝑣𝑘 𝑣𝑙′ = 𝑅𝑘 𝛿𝑘𝑙 ,
𝐸 𝑤𝑘 𝑤𝑙′ = 𝑄𝑘 𝛿𝑘𝑙
• 𝑥0 es una variable aleatoria, gaussiana, independiente de los dos procesos anteriores, con media 𝑥ҧ 0
y covarianza 𝑃0
• Determinar los estimados
𝑥ො𝑘|𝑘−1 = 𝐸 𝑥𝑘 |𝑍𝑘−1 𝑥ො𝑘|𝑘 = 𝐸 𝑥𝑘 |𝑍𝑘
Y sus matrices de covarianza asociadas, Σ𝑘|𝑘−1 y Σ𝑘|𝑘
 PROBLEMA DE FILTRADO DE KALMAN EN
TIEMPO DISCRETO

• La solución al problema es
𝑥ො𝑘+1|𝑘 = 𝐹𝑘 − 𝐾𝑘 𝐻𝑘′ 𝑥ො𝑘|𝑘−1 + K k zk
𝑥ො0|−1 = 𝑥ҧ0

𝑥ො𝑘+1|𝑘
+
𝑧𝑘 𝐾𝑘 Σ 𝐷𝑒𝑙𝑎𝑦 𝑥ො𝑘|𝑘−1

𝐹𝑘 − 𝐾𝑘 𝐻𝑘′
 PROBLEMA DE FILTRADO DE KALMAN EN
TIEMPO DISCRETO

• La matriz 𝐾𝑘 se denomina ganancia de Kalman

−1
𝐾𝑘 = 𝐹𝑘 Σ𝑘|𝑘−1 𝐻𝑘 𝐻𝑘′ Σ𝑘|𝑘−1 𝐻𝑘 + 𝑅𝑘

• La matriz de covarianza del error condicional se calcula como

−1
Σ𝑘+1|𝑘 = 𝐹𝑘 Σ𝑘|𝑘−1 − Σ𝑘|𝑘−1 𝐻𝑘 𝐻𝑘′ Σ𝑘|𝑘−1 𝐻𝑘 + 𝑅𝑘 𝐻𝑘′ Σ𝑘|𝑘−1 Fk′ + Gk Q k Gk′

• Estas ecuaciones se inicializan con

Σ0|−1 = 𝑃0
• 𝑥ො𝑘|𝑘 y Σ𝑘|𝑘 se calculan como
−1
𝑥ො𝑘|𝑘 = 𝑥ො𝑘|𝑘−1 + Σ𝑘|𝑘−1 𝐻𝑘 𝐻𝑘′ Σ𝑘|𝑘−1 𝐻𝑘 + 𝑅𝑘 𝑧𝑘 − 𝐻𝑘′ 𝑥𝑘|𝑘−1
−1
Σ𝑘|𝑘 = Σ𝑘|𝑘−1 − Σ𝑘|𝑘−1 𝐻𝑘 𝐻𝑘′ Σ𝑘|𝑘−1 𝐻𝑘 + 𝑅𝑘 𝐻𝑘′ Σ𝑘|𝑘−1
 AÑADIENDO LA ENTRADA

• Hasta ahora no hemos considerado la existencia de una entrada

• Modifiquemos la expresión del sistema a
𝑥𝑘+1 = 𝐹𝑘 𝑥𝑘 + Γ𝑢𝑘 + 𝐺𝑘 𝑤𝑘
ቊ
𝑧𝑘 = 𝐻𝑘′ 𝑥𝑘 + 𝐷𝑢𝑘 + 𝑣𝑘
• Con 𝑤𝑘 , {𝑣𝑘 } y 𝑥0 definidas de la misma forma que antes.
• Tanto 𝑢𝑘 como Γ y 𝐷 son conocidas en el instante 𝑘
• No es necesario estimar

• Agregar la entrada solamente implica una traslación de 𝑥ො𝑘+1|𝑘 y 𝑥ො𝑘|𝑘

 AÑADIENDO LA ENTRADA

• Las expresiones para las ecuaciones del filtro pasan a ser

• 𝑥ො𝑘+1|𝑘 = 𝐹𝑘 − 𝐾𝑘 𝐻𝑘′ 𝑥ො𝑘|𝑘−1 + K k zk − 𝐷𝑢𝑘 + Γ𝑢𝑘
−1
• 𝐾𝑘 = 𝐹𝑘 Σ𝑘|𝑘−1 𝐻𝑘 𝐻𝑘′ Σ𝑘|𝑘−1 𝐻𝑘 + 𝑅𝑘
−1
• Σ𝑘+1|𝑘 = 𝐹𝑘 Σ𝑘|𝑘−1 − Σ𝑘|𝑘−1 𝐻𝑘 𝐻𝑘′ Σ𝑘|𝑘−1 𝐻𝑘 + 𝑅𝑘 𝐻𝑘′ Σ𝑘|𝑘−1 Fk′ + Gk Q k Gk′

• 𝑥ො0|−1 = 𝑥ҧ0 , Σ0|−1 = 𝑃0

−1
• 𝑥ො𝑘|𝑘 = 𝑥ො𝑘|𝑘−1 + Σ𝑘|𝑘−1 𝐻𝑘 𝐻𝑘′ Σ𝑘|𝑘−1 𝐻𝑘 + 𝑅𝑘 𝑧𝑘 − 𝐷𝑢𝑘 − 𝐻𝑘′ 𝑥𝑘|𝑘−1
−1
• Σ𝑘|𝑘 = Σ𝑘|𝑘−1 − Σ𝑘|𝑘−1 𝐻𝑘 𝐻𝑘′ Σ𝑘|𝑘−1 𝐻𝑘 + 𝑅𝑘 𝐻𝑘′ Σ𝑘|𝑘−1
 PRUEBA DEL RESULTADO

𝑥0
• Consideremos la variable aleatoria 𝑌 = 𝑧
0

𝑥0 𝑃0 𝑃0 𝐻0
• 𝑦0 = ′ , Σ𝑦 = ′ ′
𝐻0 𝑥0 𝐻0 𝑃0 𝐻0 𝑃0 𝐻0 + 𝑅0

• De un resultado anterior tenemos que 𝑥0 condicionado en 𝑧0 tiene

• 𝑥ො0|0 = 𝑥ҧ 0 + 𝑃0 𝐻0 𝐻0′ 𝑃0 𝐻0 + 𝑅0 −1
𝑧0 − 𝐻0′ 𝑥0
• Σ0|0 = 𝑃0 − 𝑃0 𝐻0 𝐻0′ 𝑃0 𝐻0 + 𝑅0 −1
𝐻0′ 𝑃0

• Dadas las ecuaciones del problema y las hipótesis de independencia, 𝑥1 condicionado en 𝑧0

tiene
• 𝑥ො1|0 = 𝐹0 𝑥ො0|0
• Σ1|0 = 𝐹0 Σ0|0 𝐹0′ + 𝐺0 𝑄0 𝐺0′
 PRUEBA DEL RESULTADO

• Dadas las ecuaciones del problema, 𝑧1 condicionado en 𝑧0 tiene

Ƹ = 𝐻1′ 𝑥ො1|0
• 𝑧1|0
• Σ𝑧1|0 = 𝐻1′ Σ1|0 𝐻1 + 𝑅1

• Además
′
• 𝐸 𝑥1 − 𝑥ො1|0 𝑧1 − 𝑧1|0
Ƹ |𝑧0 = Σ1|0 𝐻1

• De esto se desprende que la variable aleatoria 𝑥1′ 𝑧1′ ′ condicionada en 𝑧0 tiene

𝑥ො1|0 Σ1|0 Σ1|0 𝐻1
• 𝑦1 = ′ , Σ𝑦 = ′
𝐻1 𝑥ො1|0 𝐻1 Σ1|0 𝐻1′ Σ1|0 𝐻1 + 𝑅1

• Aplicando el mismo resultado de la diapositiva anterior, 𝑥1|1 tiene

−1
• 𝑥ො1|1 = 𝑥ො1|0 + Σ1|0 𝐻1 𝐻1′ Σ1|0 𝐻1 + 𝑅1 𝑧1 − 𝐻1′ 𝑥ො1|0
−1
• Σ1|1 = Σ1|0 − Σ1|0 𝐻1 𝐻1′ Σ1|0 𝐻1 + 𝑅1 𝐻1′ 𝑃1
 PRUEBA DEL RESULTADO

• Se comprueba por inducción que

−1
• 𝑥ො𝑘|𝑘 = 𝑥ො𝑘|𝑘−1 + Σ𝑘|𝑘−1 𝐻𝑘 𝐻𝑘′ Σ𝑘|𝑘−1 𝐻𝑘 + 𝑅𝑘 𝑧𝑘 − 𝐻𝑘′ 𝑥𝑘|𝑘−1
• 𝑥ො𝑘+1|𝑘 = 𝐹𝑘 𝑥ො𝑘|𝑘
−1
• Σ𝑘|𝑘 = Σ𝑘|𝑘−1 − Σ𝑘|𝑘−1 𝐻𝑘 𝐻𝑘′ Σ𝑘|𝑘−1 𝐻𝑘 + 𝑅𝑘 𝐻𝑘′ Σ𝑘|𝑘−1
• Σ𝑘+1|𝑘 = 𝐹𝑘 Σ𝑘|𝑘 Fk′ + Gk Q k Gk′
 ALGUNAS PROPIEDADES DEL FILTRO

• El filtro de Kalman es un filtro lineal, en tiempo discreto, de dimensión finita.

• Excelentes propiedades para un filtro que queremos implementar de forma práctica!
• Particularmente favorable para implementación computacional
• La entrada del filtro es el proceso {𝑧𝑘 }, mientras que la salida es el proceso 𝑥ො𝑘|𝑘−1

• 𝑥ො𝑘|𝑘−1 está condicionado a {𝑧0 , … , 𝑧𝑘−1 }

• Repasando las expresiones de 𝐾𝑘 y Σ𝑘+1|𝑘 vemos que
−1
𝐾𝑘 = 𝐹𝑘 Σ𝑘|𝑘−1 𝐻𝑘 𝐻𝑘′ Σ𝑘|𝑘−1 𝐻𝑘 + 𝑅𝑘
−1
Σ𝑘+1|𝑘 = 𝐹𝑘 Σ𝑘|𝑘−1 − Σ𝑘|𝑘−1 𝐻𝑘 𝐻𝑘′ Σ𝑘|𝑘−1 𝐻𝑘 + 𝑅𝑘 𝐻𝑘′ Σ𝑘|𝑘−1 Fk′ + Gk Q k Gk′

• 𝐾𝑘 y Σ𝑘+1|𝑘 no dependen de ninguna forma de 𝑍𝑘−1

• De hecho, ambas matrices podrían ser pre-computadas
 ALGUNAS PROPIEDADES DEL FILTRO

𝑥𝑘
• Como ya vimos, es un proceso gaussiano.
{𝑧𝑘 }
• Por un resultado anterior, 𝑥𝑘 condicionada en 𝑍𝑘−1 es también gaussiano
• Su densidad de probabilidad es dada por 𝑥ො𝑘|𝑘−1 y Σ𝑘|𝑘−1
• El filtro de Kalman nos da un método de actualizar la densidad de probabilidad de 𝑥𝑘|𝑘−1
• Supongamos que el problema sobre el que estamos trabajando es estacionario
• 𝐹𝑘 , Gk , Hk , Q k son constantes
• Σ𝑘|𝑘−1 y 𝐾𝑘 no son constantes.
• Pregunta: Es uniformemente convergente?
 FILTRO DE KALMAN COMO MEJOR
ESTIMADOR LINEAL

𝑥0
• Hasta este momento, supusimos que 𝑣𝑘 es gaussiano. Supongamos que no lo es
𝑤𝑘

• Reformulemos el problema de Kalman

𝑥𝑘+1 = 𝐹𝑘 𝑥𝑘 + 𝐺𝑘 𝑤𝑘
• Sea ቊ , ∀𝑘 ≥ 0
𝑦𝑘 = 𝐻𝑘′ 𝑥𝑘 + 𝑣𝑘
• Con 𝐸 𝑣𝑘 = 0, 𝐸 𝑤𝑘 = 0, 𝐸 𝑥0 = 𝑥ҧ 0 , 𝐸 𝑣𝑘 𝑣𝑙′ = 𝑅𝑘 𝛿𝑘𝑙 , 𝐸 𝑤𝑘 𝑤𝑙′ = 𝑄𝑘 𝛿𝑘𝑙
′
• 𝐸 𝑥0 − 𝑥ҧ 0 𝑥0 − 𝑥ҧ 0 = 𝑃0 , 𝑣𝑘 , 𝑤𝑘 y 𝑥0 no tienen correlación
• Puede probarse[p46] que, dada una clase de filtros definida por
𝑒 𝑒
• 𝑥𝑘+1|𝑘 = 𝐹𝑘 − 𝐾𝑘𝑒 𝐻𝑘′ 𝑥𝑘|𝑘−1 + 𝐾𝑘𝑒 𝑧𝑘
• La solución dada por Kalman (𝑥0|−1 , 𝐾𝑘𝑒 = 𝐾𝑘 ) es óptima en el sentido que minimiza el error
cuadrático medio
ADAPTACIÓN PARA SISTEMAS NO
LINEALES
 EL MUNDO REAL

• Hasta ahora hemos definido una serie de resultados y herramientas adecuadas para
representaciones de sistemas lineales.
• En la práctica, muy pocos sistemas pueden ser representados de manera mínimamente aceptable
mediante este tipo de representaciones
• Tenemos herramientas muy sólidas y prácticas pero poco aplicables
• No obstante, podemos adaptarlas a sistemas no lineales (con ciertas características)
 FILTRADO NO LINEAL

• Trabajemos con una representación de la forma

𝑥 = 𝑓𝑘 𝑥𝑘 + 𝑔𝑘 𝑥𝑘 𝑤𝑘
ቊ 𝑘+1
𝑧𝑘 = ℎ𝑘 𝑥𝑘 + 𝑣𝑘
• En este modelo, 𝑓𝑘 . , 𝑔𝑘 . , ℎ𝑘 . son funciones no lineales variables en el tiempo.
• 𝑤𝑘 ~𝑁 0, 𝑄𝑘
• 𝑣𝑘 ~𝑁 0, 𝑅𝑘
• 𝑥0 ~𝑁 𝑥0 , 𝑃0
• A partir de ahora se usa la siguiente notación
𝛿𝑓𝑘 𝑥 𝛿ℎ𝑘 𝑥
𝐹𝑘 = ቤ , 𝐻𝑘′ = ቤ , 𝐺𝑘 = 𝑔𝑘 (𝑥ො𝑘|𝑘 )
𝛿𝑥 𝑥=𝑥ො 𝛿𝑥 𝑥=𝑥ො
𝑘|𝑘 𝑘|𝑘
 FILTRO DE KALMAN EXTENDIDO

• Si las funciones 𝑓𝑘 𝑥𝑘 , 𝑔𝑘 (𝑥𝑘 ), y ℎ𝑘 𝑥𝑘 son diferenciables, pueden ser expresadas según su

desarrollo de Taylor en torno a 𝑥ො𝑘|𝑘 o 𝑥ො𝑘|𝑘+1
• 𝑓𝑘 𝑥𝑘 = 𝑓𝑘 𝑥ො𝑘|𝑘 + 𝐹𝑘 𝑥𝑘 − 𝑥ො𝑘|𝑘 + …
• 𝑔𝑘 𝑥𝑘 = 𝐺𝑘 + …
• ℎ𝑘 𝑥𝑘 = ℎ𝑘 𝑥ො𝑘|𝑘−1 + 𝐻𝑘′ 𝑥𝑘 − 𝑥ො𝑘|𝑘−1 + …

• Si despreciamos los términos de mayor orden, podemos expresar

𝑥𝑘+1 = 𝑓𝑘 𝑥ො𝑘|𝑘 + 𝐹𝑘 𝑥𝑘 − 𝑥ො𝑘|𝑘 + 𝐺𝑘 𝑤𝑘

൝
𝑧𝑘 = ℎ𝑘 𝑥ො𝑘|𝑘−1 + 𝐻𝑘′ 𝑥𝑘 − 𝑥ො𝑘|𝑘−1 + 𝑣𝑘
 FILTRO DE KALMAN EXTENDIDO

𝑥𝑘+1 = 𝑓𝑘 𝑥ො𝑘|𝑘 + 𝐹𝑘 𝑥𝑘 − 𝐹𝑘 𝑥ො𝑘|𝑘 + 𝐺𝑘 𝑤𝑘

• ൝
𝑧𝑘 = ℎ𝑘 𝑥ො𝑘|𝑘−1 + 𝐻𝑘′ 𝑥𝑘 − 𝐻𝑘′ 𝑥ො𝑘|𝑘−1 + 𝑣𝑘
• Definiendo
• 𝑢𝑘 = 𝑓𝑘 𝑥ො𝑘|𝑘 − 𝐹𝑘 𝑥ො𝑘|𝑘

• 𝑦𝑘 = ℎ𝑘 𝑥ො𝑘|𝑘−1 −𝐻𝑘′ 𝑥ො𝑘|𝑘−1

𝑥𝑘+1 = 𝐹𝑘 𝑥𝑘 + 𝐺𝑘 𝑤𝑘 + 𝑢𝑘
• ቊ
𝑧𝑘 = 𝐻𝑘′ 𝑥𝑘 + 𝑣𝑘 + 𝑦𝑘
• Este nuevo modelo aproximado es totalmente adecuado para usar los resultados que ya obtuvimos
• Lo plantearemos de forma levemente distinta para derivar algunos resultados
 FILTRO DE KALMAN EXTENDIDO

𝑥𝑘+1 = 𝐹𝑘 𝑥𝑘 + 𝐺𝑘 𝑤𝑘 + 𝑢𝑘
ቊ
𝑧𝑘 = 𝐻𝑘′ 𝑥𝑘 + 𝑣𝑘 + 𝑦𝑘
• 𝑥ො𝑘|𝑘 = 𝑥ො𝑘|𝑘−1 + 𝐿𝑘 𝑧𝑘 − ℎ𝑘 𝑥ො𝑘|𝑘−1
• 𝑥ො𝑘+1|𝑘 = 𝑓𝑘 𝑥ො𝑘|𝑘
• 𝐿𝑘 = Σ𝑘|𝑘−1 𝐻𝑘 Ω−1
k

• Ω𝑘 = 𝐻𝑘′ Σ𝑘|𝑘−1 𝐻𝑘 + 𝑅𝑘
−1
• Σ𝑘|𝑘 = Σ𝑘|𝑘−1 - Σ𝑘|𝑘−1 𝐻𝑘 𝐻𝑘′ Σ𝑘|𝑘−1 𝐻𝑘 + 𝑅𝑘 𝐻𝑘′ Σ𝑘|𝑘−1
• 𝛴𝑘+1|𝑘 = 𝐹𝑘 𝛴𝑘|𝑘 𝐹𝑘′ = 𝐺𝑘 𝑄𝑘 𝐺𝑘′
• Inicialización dada por Σ0|−1 = 𝑃0 , 𝑥ො0|−1 = 𝑥ҧ0
 ALGUNOS RESULTADOS SOBRE EKF

• 𝑥ො𝑘+1|𝑘 y Σ𝑘+1|𝑘 mantienen su significado anterior respecto al nuevo sistema linealizado,

pero pierden su valor respecto a la distribución de 𝑥𝑘 del sistema no lineal
• Las ecuaciones de estimación y actualización del filtro se acoplan.Ya no es posible
precomputar.
2
• La linealización es mas exacta conforme se hacen menores 𝑥𝑘 − 𝑥ො𝑘|𝑘 y ฮ𝑥𝑘 −
2
𝑥ො𝑘|𝑘−1 ฮ . Estas magnitudes se pueden medir en base a 𝑡𝑟(Σ𝑘|𝑘 ) y 𝑡𝑟(Σ𝑘|𝑘−1 )