Preliminares matemáticos (filtro de Kalman) Facultad de Ciencias de la Electrónica Benemérita Universidad Autónoma de Puebla Licenciatura en Ingeniería Mecatrónica/Licenciatura en Electrónica Dr. Fernando Reyes Cortés Robótica I (FCES 034 001) https://teams.microsoft.com/l/team (PRIMAVERA 2021 - ROBOTICA I - 37321 - SECCION 001) Primavera 2021 Filtros de Kalman Ruido y señales aleatorias Observador de Kalman Parte VII Filtro de Kalman) Contenido 1 Filtros de Kalman Introducción Breve biografía de Kalman (1930-2016) 2 Ruido y señales aleatorias Dr. Fernando Reyes Cortés Robótica I (FCES 034 001) Período: Primavera ( 28 de enero de 2021) Preliminares matemáticos (filtro de Kalman) 3 Ruido Señales aleatorias Distribución Gausiana Distribución de parámetros Observador de Kalman Benemérita Universidad Autónoma de Puebla https://teams.microsoft.com/l/team (PRIMAVERA 2021 - ROBOTICA I - 37321 - SECCION 001) Licenciatura en Ingeniería Mecatrónica/Licenciatura en Electrónica Facultad de Ciencias de la Electrónica 2 / 13 Filtros de Kalman Ruido y señales aleatorias Observador de Kalman Introducción Filtros de Kalman El filtro de Kalman es un observador compuesto por un conjunto de ecuaciones que permiten la estimación de estados en presencia de ruido de sistemas dinámicos. En 1960 Kalman publicó un algoritmo recursivo para resolver el problema de filtrado lineal. La idea fundamental del filtro de Kalman es disminuir notablemente el ruido presente en las señales instrumentales y variables de estado, así como estimar los estados no observables . En diciembre de 1968, las técnicas de filtrado de Kalman fueron utilizadas en la trayectoria del Apollo 8, cuya órbita fue alrededor de la luna. Se presenta una breve introducción del análisis de sistemas estocásticos para el diseño de observadores a través de la metodología de filtrado de Kalman. Dr. Fernando Reyes Cortés Robótica I (FCES 034 001) Período: Primavera ( 28 de enero de 2021) Preliminares matemáticos (filtro de Kalman) Benemérita Universidad Autónoma de Puebla https://teams.microsoft.com/l/team (PRIMAVERA 2021 - ROBOTICA I - 37321 - SECCION 001) Licenciatura en Ingeniería Mecatrónica/Licenciatura en Electrónica Facultad de Ciencias de la Electrónica 3 / 13 Filtros de Kalman Ruido y señales aleatorias Observador de Kalman Breve biografía de Kalman (1930-2016) Breve biografía de Kalman (1930-2016) Rudolf Emil Kalman nació en Budapest, Hungría el 19 de mayo de 1930; a la edad de 13 años emigró a los EE.UU. En 1953 y 1954 obtuvo el grado de ingeniero eléctrico y maestría en ingeniería, respectivamente en el MIT. En 1957, culminó el doctorado en la Universidad de Columbia, New York. Kalman murió el 2 de julio de 2016 en Gainseville, Florida. Rudolf Emil Kalman (1930-2016) : Desarrolló técnicas de filtrado y estimación de estados para sistemas dinámicos, con una gran variedad de aplicaciones, tales como: procesamientos de señales y de imágenes, navegación, control de sistemas mecatrónicos y robótica, etc. Kalman recibió la Medalla de Honor de la IEEE en 1974; la Medalla Centenario de la IEEE en 1984, y el premio de la Academia Nacional de Ingeniería Charles Stark Draper en 2008. Debido a sus contribuciones científicas, Kalman recibió en 2009, la Medalla Nacional de Ciencias directamente del presidente de la Unión Americana Barack Obama. Dr. Fernando Reyes Cortés Robótica I (FCES 034 001) Período: Primavera ( 28 de enero de 2021) Preliminares matemáticos (filtro de Kalman) Figura 1: Rudolf Kalman, científico Húngaro. Benemérita Universidad Autónoma de Puebla https://teams.microsoft.com/l/team (PRIMAVERA 2021 - ROBOTICA I - 37321 - SECCION 001) Licenciatura en Ingeniería Mecatrónica/Licenciatura en Electrónica Facultad de Ciencias de la Electrónica 4 / 13 Filtros de Kalman Ruido y señales aleatorias Observador de Kalman Ruido Ruido Ruido Ruido es una señal aleatoria (en general desconocida) que distorsiona una señal electrónica, o la información que proporcionan las variables de estado. Algunas posibles fuentes de ruido son por fenómenos: ópticos, acústicos, electromagnéticos, mecánicos, ambientales, vibraciones, etc. Animación 1: Señal sinusoidal con ruido Señal sen(t) con ruido 1 π 2 π t −1 Dr. Fernando Reyes Cortés Robótica I (FCES 034 001) Período: Primavera ( 28 de enero de 2021) Preliminares matemáticos (filtro de Kalman) Benemérita Universidad Autónoma de Puebla https://teams.microsoft.com/l/team (PRIMAVERA 2021 - ROBOTICA I - 37321 - SECCION 001) Licenciatura en Ingeniería Mecatrónica/Licenciatura en Electrónica Facultad de Ciencias de la Electrónica 5 / 13 Filtros de Kalman Ruido y señales aleatorias Observador de Kalman Señales aleatorias Señales aleatorias Una señal aleatoria es un resultado numérico tal que distorsiona la señal o información de variables de estados. Matemáticamente, una señal aleatoria ρ es un mapeo de la forma ρ : Ω −→ IR, donde Ω es el espacio de todos los posibles resultados numéricos del los correspondientes procesos aleatorios. Distribuciones más usadas: gaussiana, continua uniforme y normal. Dr. Fernando Reyes Cortés Robótica I (FCES 034 001) Período: Primavera ( 28 de enero de 2021) Preliminares matemáticos (filtro de Kalman) Benemérita Universidad Autónoma de Puebla https://teams.microsoft.com/l/team (PRIMAVERA 2021 - ROBOTICA I - 37321 - SECCION 001) Licenciatura en Ingeniería Mecatrónica/Licenciatura en Electrónica Facultad de Ciencias de la Electrónica 6 / 13 Filtros de Kalman Ruido y señales aleatorias Observador de Kalman Distribución Gausiana Distribución Gausiana Una señal aleatoria gausiana (Gaussian random signal) se define a través de la densidad de probabilidad fσ (t) de la siguiente forma: t−µ 1 √ e− 2 σ2 fσ (t) = σ 2π 2 donde σ, µ ∈ IR+ ; al parámetro µ es conocido como la media (valor medio) y a σ 2 como variancia. La figura 7 muestra la función distribución gaussiana fσ (t) para µ = 0, −2 y σ = 0.2, 0.4. Distribución Gausiana 2 µ = −2, σ = 0.2 µ = 0, σ = 0.4 1 -2 0 t Figura 7: Distribución gaussiana fσ (t). Dr. Fernando Reyes Cortés Robótica I (FCES 034 001) Período: Primavera ( 28 de enero de 2021) Preliminares matemáticos (filtro de Kalman) Benemérita Universidad Autónoma de Puebla https://teams.microsoft.com/l/team (PRIMAVERA 2021 - ROBOTICA I - 37321 - SECCION 001) Licenciatura en Ingeniería Mecatrónica/Licenciatura en Electrónica Facultad de Ciencias de la Electrónica 7 / 13 Filtros de Kalman Ruido y señales aleatorias Observador de Kalman Distribución de parámetros Distribución de parámetros Distribución continua uniforme La distribución continua uniforme de una señal x aleatoria se aplica sobre un intervalo x ∈ [a, b], entonces la probabilidad de densidad es: fσ (x) = signo(x − a) − signo(x − b) 2 b−a (1) Distribución de parámetros La media µ de una señal aleatoria para una distribución gaussiana está dada por: Z ∞ E[x] = xf (x)dx (2) −∞ Cuando la distribución de la señal aleatoria es continua uniforme, entonces u = a+b 2 . La variancia σ 2 significa cuantos valores son esparcidos alrededor de la media µ, y se define de la siguiente manera: 2 σ 2 = var(x) = = E[ x − µ ] = E[x2 ] − µ2 (3) La variancia de una señal aleatoria es σ 2 , si tiene una distribución gaussiana. 2 b − a Cuando tiene distribución continua, entonces σ 2 = . 12 √ La desviación estándar σ es la raíz cuadrada positiva de la variancia: σ = + σ 2 . Dr. Fernando Reyes Cortés Robótica I (FCES 034 001) Período: Primavera ( 28 de enero de 2021) Preliminares matemáticos (filtro de Kalman) Benemérita Universidad Autónoma de Puebla https://teams.microsoft.com/l/team (PRIMAVERA 2021 - ROBOTICA I - 37321 - SECCION 001) Licenciatura en Ingeniería Mecatrónica/Licenciatura en Electrónica Facultad de Ciencias de la Electrónica 8 / 13 Filtros de Kalman Ruido y señales aleatorias Observador de Kalman Distribución de parámetros Vectores aleatorios: sea x ∈ IRn donde cada componente i-ésima xi ∈ IR es una variable aleatoria descrita por su correspondiente función de distribución de probabilidad ρσi : Ω → [0, 1]. Matriz de covariancia: sean x, y ∈ IRn dos vectores aleatorios,entonces h i cov(x, y) = E x − E[x] y − E[y] T ∈ IRn×n (4) Matriz de variancia se encuentra definda como: h i var(x) = cov(x, x) = E x − E[x] x − E[x] T ∈ IRn×n 2 E x 1 − µ1 E x1 h− µ1 x2 −iµ2 E x1 − µ1 x2 − µ2 E x 2 − µ2 2 var(x) = .. .. . . E xn − µn x1 − µ1 E x n − µn x 2 − µ2 ··· E x1 − µ1 ··· E ... x2 − µ2 ··· xn − µn xn − µn (5) .. . h 2 i E x n − µn La diagonal principal de la matriz de variancia var(x) contiene todas las variancias σi2 de las componentes xi ∈ x; las otras componentes representan las covariancias entre dos diferentes elementos xi , xj ∈ x. En general, observe que de la ecuación (5) se cumple: var(x) = E xxT − uuT (6) T donde u = E x = µ1 , µ2 , · · · , µn . Dr. Fernando Reyes Cortés Robótica I (FCES 034 001) Período: Primavera ( 28 de enero de 2021) Preliminares matemáticos (filtro de Kalman) Benemérita Universidad Autónoma de Puebla https://teams.microsoft.com/l/team (PRIMAVERA 2021 - ROBOTICA I - 37321 - SECCION 001) Licenciatura en Ingeniería Mecatrónica/Licenciatura en Electrónica Facultad de Ciencias de la Electrónica 9 / 13 Filtros de Kalman Ruido y señales aleatorias Observador de Kalman Distribución de parámetros Matriz de auto-correlación del vector aleatorio x se denota por Ax , y se encuentra definida como: Ax = E xxT (7) Note que, E xi xj = E xj xi la matriz es simétrica, es decir: Ax = ATx . Sea un vector aleatorio x ∈ IRn con matriz de variancia var(x) ∈ IRn×n y media µ ∈ IRn , la distancia Mahalanobis se define como: q T x − u var−1 (x) x − u DM (x) = (8) Ruido blanco Ruido blanco es una señal aleatoria en el tiempo con un espectro definida dentro de un ancho de banda con media µ = 0 y con matriz de auto-correlación con elementos at1 t2 = E[xt1 xt2 ] = σ 2 · · · δ(t1 − t2 ), siendo δ la función Delta Dirac. Proceso de ruido blanco: defina el error de estimación como x̃ = y − cT x, la matriz de correlación está dada por: 1 j=k Re (i, j) = E[x̃j x̃k ] = W[k]δjk , donde jjk = (9) 0 i 6= j Dr. Fernando Reyes Cortés Robótica I (FCES 034 001) Período: Primavera ( 28 de enero de 2021) Preliminares matemáticos (filtro de Kalman) Benemérita Universidad Autónoma de Puebla https://teams.microsoft.com/l/team (PRIMAVERA 2021 - ROBOTICA I - 37321 - SECCION 001) Licenciatura en Ingeniería Mecatrónica/Licenciatura en Electrónica Facultad de Ciencias de la Electrónica 10 / 13 Filtros de Kalman Ruido y señales aleatorias Observador de Kalman Observador de Kalman Considere el sistema dinámico estocástico: ẋ = Ax + Bu + Fv y = cT x + w donde los parámetros del sistema dinámico son: A ∈ IRn×n , B ∈ IRn×1 , c ∈ IRn×1 ; u es la señal de entrada al sistema, u ∈ IR; y ∈ IR es la respuesta; F ∈ IRn×n es la estructura matemática del disturbio, v ∈ IRn es el vector con las señales del disturbio (gaussiana, distribución normal y de media cero); w ∈ IR es una fuente de ruido blanco (gaussiana, normal y de media cero). Considere la siguiente estructura de observador: x̂˙ = Ax̂ + Bu + L y − cT x̂ ; donde: y = cT x, respuesta ideal sin ruido x̂˙ = Ax̂ + Bu + L cT x − cT x̂ = Ax̂ + Bu + LcT x − x̂ ; donde: x̃ˆ = x − x̂ x̂˙ = Ax̂ + Bu + LcT x̃ˆ (10) El sistema dinámico con error de estimación queda de la siguiente forma: ˙ Tˆ Tˆ Z Z ẋ − x̂˙ = x̃ˆ = Ax + Bu Bu Z + Fv − Ax̂ − Z − Lc x̃ = A x − x̂ + Fv − Lc x̃ ˙ x̃ˆ = A − LcT x̃ˆ + Fv (11) La problemática consiste en encontrar una adecuada estructura para L = L(t) ∈ IRn×1 , tal en ausencia de ruido ˆ ∈ IRn ; y, en presencia de ruido, el se mantenga la convergencia asintótica del error de estimación de estados x̃(t) ˆ error de estimación kx̃(t)k < ρ << 1, con ρ ∈ IR+ . Dr. Fernando Reyes Cortés Robótica I (FCES 034 001) Período: Primavera ( 28 de enero de 2021) Preliminares matemáticos (filtro de Kalman) Benemérita Universidad Autónoma de Puebla https://teams.microsoft.com/l/team (PRIMAVERA 2021 - ROBOTICA I - 37321 - SECCION 001) Licenciatura en Ingeniería Mecatrónica/Licenciatura en Electrónica Facultad de Ciencias de la Electrónica 11 / 13 Filtros de Kalman Ruido y señales aleatorias Observador de Kalman Observador de Kalman Considere la siguiente función de costo: Z t LT (σ) Q − J(t) = 0 1 P(σ)ccT P(σ) rw + F(σ)Rv FT (σ) L(σ) dσ (12) donde Q = AP + PAT ; siendo P(t) ∈ IRn×n , P(t) = E[x̃(t)x̃T (t)], rw ∈ IR; Rv ∈ IRn×n es una matriz diagonal definida positiva. La derivada de la función de costo J(t) toma la forma d J(t) = LT Q − dt Optimizando dJ(t) dt 1 PccT P rw + FRv FT L (13) + FRv FT L = 0 ∈ IRn ⇐⇒ L(0) = 0 ∈ IRn (14) con respecto a L, y minimizando, se tiene: ∂ dJ(t) = Q− ∂L dt 1 PccT P rw Defina a Ṗ(t) a través del siguiente modelo dinámico: Ṗ = AP + PAT − 1 PccT P + FRv FT rw (15) donde las condiciones iniciales par P se obtienen como P(0) = E[x(0)xT (0)]. Defina a L(t) = Dr. Fernando Reyes Cortés Robótica I (FCES 034 001) 1 P(t)c. rw Período: Primavera ( 28 de enero de 2021) Preliminares matemáticos (filtro de Kalman) Benemérita Universidad Autónoma de Puebla https://teams.microsoft.com/l/team (PRIMAVERA 2021 - ROBOTICA I - 37321 - SECCION 001) Licenciatura en Ingeniería Mecatrónica/Licenciatura en Electrónica Facultad de Ciencias de la Electrónica 12 / 13 Filtros de Kalman Ruido y señales aleatorias Observador de Kalman Ejemplo 1: Filtro de Kalman Considere el siguiente sistema lineal estocástico: −04 0 1 0 10 0 v1 ẋ = x+ 2 u+ 2 −04 −wn −2ρwn wn 0 10 v2 | {z } F y = 1 0 x+w (16) (17) √ donde wn = 2 y ρ = 2w3 n ; la señal de entrada u corresponde a un escalón unitario; las señales de ruido 1 v1 , v2 , w ∈ IR son señales aleatorias (ruido con distribución normal), y rw = 10−04 . 1 Implementar en MATLAB el filtro de Kalman: x̂˙ = Ax̂ + Bu + LcT x̃ˆ 1 P(t)ccT P(t) + FRv FT rw −04 10 0 F = Rv = 0 10−4 Ṗ(t) = AP(t) + P(t)AT − L = 1 P(t)c; rw T tal que, proporcione el vector de estados estimados x̂ = x̂1 x̂2 del correspondiente sistema dinámico lineal (16)-(17); considere un periodo de muestreo h = 0.001 s. 2) En presencia de ruido a través de las señales v1 , v2 , w muestre qu su simulación obtiene la norma ˆ euclidiana del error de estimación kx̃(t)k ≈ 10−04 << 1. Dr. Fernando Reyes Cortés Robótica I (FCES 034 001) Período: Primavera ( 28 de enero de 2021) Preliminares matemáticos (filtro de Kalman) Benemérita Universidad Autónoma de Puebla https://teams.microsoft.com/l/team (PRIMAVERA 2021 - ROBOTICA I - 37321 - SECCION 001) Licenciatura en Ingeniería Mecatrónica/Licenciatura en Electrónica Facultad de Ciencias de la Electrónica 13 / 13