robotica cap1 clase7 Kalman

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