Robotica Cap1 Clase 8 Filtro de 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
Microsoft Teams (Otoño 2023 ROBOTICA I, NRC: 20639)
Otoño 2023
Filtro de Kalman Ruido y señales aleatorias Observador de Kalman
Parte VI
Filtro de Kalman
Contenido
1 Filtro de Kalman Señales aleatorias

Introducción Distribución Gausiana
Breve biografía de Kalman (1930-2016) Distribución de parámetros
2 Ruido y señales aleatorias 3 Observador de Kalman
Ruido Dinámica lineal
Dr. Fernando Reyes Cortés Período: Otoño ( 21 de agosto de 2023) Benemérita Universidad Autónoma de Puebla Licenciatura en Ingeniería Mecatrónica/Electrónica
Robótica I (FCES 034 003, NRC: 20639) Preliminares matemáticos: filtro de Kalman Microsoft Teams (Otoño 2023 ROBOTICA I, NRC: 20639) Facultad de Ciencias de la Electrónica 2 / 15
Introducción
Filtro 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.
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 Rudolf Emil Kalman (1930-2016)
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.
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ó Figura 1: Rudolf Kalman, científico Húngaro.
en 2009, la Medalla Nacional de Ciencias directamente
del presidente de la Unión Americana Barack Obama.
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
π t
2
π
−1
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.
Distribución Gausiana
Una señal aleatoria gausiana (Gaussian random signal) se define a través de la densidad de probabilidad fσ (t):
2
1 t−µ
fσ (t) = √ e− 2 σ 2
σ 2π
donde σ, µ ∈ IR+ ; al parámetro µ es conocido como la media (valor medio) y a σ 2 como variancia.
µ = −2, σ = 0.2 1 µ = 0, σ = 0.4
t
-2 0
Figura 7: Distribución gaussiana fσ (t), para µ = 0, −2 y σ = 0.2, 0.4.
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:
signo(x − a) − signo(x − b)
fσ (x) = (1)
2 b−a
La media µ de una señal aleatoria para una distribución gaussiana está dada por:
Z ∞
E[x] = xf (x)dx (2)
−∞
a+b
Cuando la distribución de la señal aleatoria es continua uniforme, entonces u = 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 .
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 xn − µn
E x 2 − µ2 2
 
 E x1 − µ1 x2 − µ2 x2 − µ2 xn − µn 

··· E
var(x) =   (5)
 
.. .. ... ..

 . . h . 

2 i
E xn − µn x1 − µ1 E x n − µn x 2 − µ2 ··· 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 .
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
DM (x) = x − u var−1 (x) x − u (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
Observador de Kalman
Considere el sistema dinámico estocástico:
ẋ = −ax + bu + fruido
y = c x + druido
donde los parámetros del sistema dinámico son: a, b, c ∈ IR+ ; u ∈ IR es la señal de entrada al sistema; y ∈ IR es
la respuesta; fruido , druido ∈ IR son las señales de ruido blanco (disturbio gaussiano, distribución normal y de media
cero).
Diseño del observador:
x̂˙ = −ax̂ + b u + l y − cx̂

x̂˙ = −ax̂ + bu + l cx − cx̂ = −ax̂ + bu + l c x − x̂ ; sea: x̃ˆ = x − x̂

x̂˙ = −ax̂ + bu + l c x̃ˆ (10)
El error de estimación queda como:
˙
x̃ˆ = ẋ − x̂˙ = −a x + Z ˆ = −a x − x̂ + fruido − l c x̃ˆ

+ fruido + ax̂ − Z
bu
Z − l c x̃
bu
Z
˙
x̃ˆ = − a + l c x̃ˆ + fruido

(11)
La problemática consiste en encontrar una adecuada estructura para l = l(t) ∈ IR+ , tal en ausencia de ruido se
ˆ
mantenga la convergencia asintótica del error de estimación de estados x̃(t); y en presencia de ruido, el error de
ˆ
estimación |x̃(t)| < ρ << 1, con ρ ∈ IR+ .
Diseño de la ganancia l(t) del observador de Kalman

Considere la siguiente función de costo:
Z t
1 2
l2 (σ) 2 a p2 (σ) − p (σ)c2 2

J (t) = rw
+ fruido (σ)rv dσ (12)
0
donde p(t) ∈ IR+ , 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) 1 2
= l2 (t) 2 a p2 (t) − p (t)c2 2

rw
+ fruido (t)rv (13)
dt
dJ (t)
Minimizando a dt
con respecto a l:
∂ dJ (t) 1 2
= l2 (t) 2 a p2 (t) − p (t)c2 2

rw
+ fruido (t)rv = 0 ⇐⇒ l(0) = 0 (14)
∂l dt
Defínase a ṗ(t) a través del siguiente modelo dinámico:
1 2 2
ṗ = 2 a p2 − 2
p c + fruido (t)rv (15)
rw
donde las condiciones iniciales par p se obtienen como p(0) = E[x(0)xT (0)].
1
Se propone a l(t) = rw
c p(t).
Dinámica lineal
Código MATLAB 1: KalmanEscalar

ẋ = −a x + b u + ruido(t)
Robótica I (FCES 034 003), período otoño (21 de agosto de 2023). x̂˙ = −ax̂ + b u + l y − cx̂ + fruido

Licenciatura en Mecatrónica/Electrónica.
Dr. Fernando Reyes Cortés. Facultad de Ciencias de la Electrónica, BUAP.
KalmanEscalar.m MATLAB versión 2023A
1 function oep =KalmanEscalar(t, oe)
2 global sr
3 x=oe(1,1); %posición.
4 xe=oe(2,1); % posición estimada.
5 p=oe(3,1); % estado de la ganancia de sintonía.
6 xtilde=oe(4,1); % error de estimación.
7 a=2; b=2; c=1; rw=1e-02; u(t>=0)=1;
8 ruido1=sr*sin(random( Normal ,0,1));
´
9 ruido2=sr*cos(rand(1,1)); ruido3=sr*rand(1,1);
10 xp=-a*x+b*u+ruido1;
11 y=c*x+ruido2;
12 pp=2*a*p*p-(1/rw)*p*p*c*c+rw*ruido3;
13 l=c*p;
14 xep=-a*xe+b*u+l*(y-c*xe);
15 xtildep=xp-xep;
16 oep=[xp; % dinámica del sistema xp=-a x+ b u+ruido(t).
17 xep; % estimador xep=-a*xe+n*u+l*(y-c’*xe).
18 pp; % estado de la ganancia p.
19 xtildep]; %
20 end
Dinámica lineal
Código MATLAB 2: simuKalmanEscalar

Robótica I (FCES 034 003), período otoño (21 de agosto de 2023). Licenciatura en Mecatrónica/Electrónica.
Dr. Fernando Reyes Cortés. Facultad de Ciencias de la Electrónica, BUAP.
simuKalmanEscalar.m MATLAB versión 2023A

1 clc;
2 clearvars;
3 close all;
4 format short
5 global sr
6 sr=0; % con ruido sr=1; sin perturbación s=0.
7 ti=0; h=0.001; tf = 15;
8 ts=(ti:h:tf) ; % vector columna tiempo.
´
9 opciones=odeset( RelTol ,1e-03, AbsTol ,1e-03, InitialStep ,h, MaxStep ,h);

´
´
´
´
10 oe0=[0; % x(0).
11 0; % xe(0).
12 0; % p(0).
13 0]; % xtilde(0).
14 % sistema dinámico con filtro de Kalman y ruido.
15 [t, oe]=ode45( KalmanEscalar , ts, oe0, opciones);
´
16 x=oe(:,1); xe=oe(:,2); p=oe(:,3); xtilde=oe(:,4);

17 figure(1), plot(t, x, r , t, xe, b ) % x vs xe.
´´
´
´
18 figure(2), plot(t, p, g ) % p
´
´
19 figure(3), plot(t, xtilde, b ) % error de estimación.

´
´
Dinámica lineal
Filtrado de Kalman: estimación de x(t) a través de x̂(t)
Figura 2: Respuesta del filtro de Kalman.

Robotica Cap1 Clase 8 Filtro de Kalman

Cargado por

Información del documento

Descripción original:

Derechos de autor

Formatos disponibles

Compartir este documento

Compartir o incrustar documentos

Opciones para compartir

¿Le pareció útil este documento?

¿Este contenido es inapropiado?

Copyright:

Formatos disponibles

Robotica Cap1 Clase 8 Filtro de Kalman

Cargado por

Copyright:

Formatos disponibles

Preliminares matemáticos: filtro de Kalman

Facultad de Ciencias de la Electrónica

Licenciatura en Ingeniería Mecatrónica/Licenciatura en Electrónica

1 Filtro de Kalman Señales aleatorias

Breve biografía de Kalman (1930-2016)

Breve biografía de Kalman (1930-2016)

Rudolf Emil Kalman nació en Budapest, Hungría el 19

Animación 1: Señal sinusoidal con ruido

Señal sen(t) con ruido

Matemáticamente, una señal aleatoria ρ es un mapeo de la forma ρ : Ω −→ IR, donde Ω es el espacio de

Distribuciones más usadas: gaussiana, continua uniforme y normal.

µ = −2, σ = 0.2 1 µ = 0, σ = 0.4

Figura 7: Distribución gaussiana fσ (t), para µ = 0, −2 y σ = 0.2, 0.4.

La variancia de una señal aleatoria es σ 2 , si tiene una distribución gaussiana.

Matriz de variancia se encuentra definda como:

var(x) = E xxT − uuT

Note que, E xi xj = E xj xi la matriz es simétrica, es decir: Ax = ATx .

x̂˙ = −ax̂ + b u + l y − cx̂

x̂˙ = −ax̂ + bu + l cx − cx̂ = −ax̂ + bu + l c x − x̂ ; sea: x̃ˆ = x − x̂

x̂˙ = −ax̂ + bu + l c x̃ˆ (10)

El error de estimación queda como:

Diseño de la ganancia l(t) del observador de Kalman

La derivada de la función de costo J (t) toma la forma

Código MATLAB 1: KalmanEscalar

Código MATLAB 2: simuKalmanEscalar

simuKalmanEscalar.m MATLAB versión 2023A

9 opciones=odeset( RelTol ,1e-03, AbsTol ,1e-03, InitialStep ,h, MaxStep ,h);

16 x=oe(:,1); xe=oe(:,2); p=oe(:,3); xtilde=oe(:,4);

19 figure(3), plot(t, xtilde, b ) % error de estimación.

Filtrado de Kalman: estimación de x(t) a través de x̂(t)

Figura 2: Respuesta del filtro de Kalman.

También podría gustarte