Documenti di Didattica
Documenti di Professioni
Documenti di Cultura
de Puebla
Profesor:
Alumno:
Rodolfo Emmanuel Fernández Gallardo
2
2. Introducción
El filtro de Kalman es un algoritmo desarrollado por Rudolf E. Kalman en
1960 que sirve para poder identificar el estado oculto (no medible) de un sistema
dinámico lineal, además podemos encontrar cuando el sistema está sometido a
ruido blanco aditivo. La ganancia del filtro de Kalman es capaz de escogerla
de forma óptima cuando se conocen las varianzas de los ruidos que afectan al
sistema. Ya que el Filtro de Kalman es un algoritmo recursivo, este puede correr
en tiempo real usando únicamente las mediciones de entrada actuales, el estado
calculado previamente y su matriz de incertidumbre, no requiere ninguna otra
información adicional [ [3]].
Cada una de las variables aleatorias del proceso tiene su propia función de
distribución de probabilidad y pueden o no estar correlacionadas entre sı́. Ca-
da variable o conjunto de variables sometidas a influencias o efectos aleatorios
constituye un proceso estocástico [ [4]].
3
Proceso de estimación
p(v) ∼ N (0, R)
4
3. Objetivos
3.1. General
Entender el sistema estocástico y adicionar una señal de ruido aleatoria.
Utilizar el filtro de Kalman para eliminar el ruido blanco de los estados y estimar
el valor del sistema lineal propuesto.
3.2. Especı́ficos
Implementar el Filtro de Kalman en MATLAB, tal que proporcione el
vector de estados estimados x̂?[x̂1 x̂2 ]T del correspondiente sistema dinámi-
co lineal 5
Verificar que el rror e estimación sea máximo 10−15
Implemente un procedimiento en MATLAB para estimar la velocidad
x2 (t) del correspondiente sistema dinámico lineal (1) a través de un filtro-
observador (en su forma escalar, obtener un error de velocidad con una
cota superior menor a< 0.07rad/seg).
Modificar el punto anterior para diseñar un filtro tipo hiperbólico, de tal
forma que el error de velocidad tenga un cota superior menor a 0.2rad/seg.
y = [10]x + ω (6)
Obteniendo la solución analı́tica del sistema y agregando una señal de ruido
normailizada por la siguiente función de MATLAB que genera un número alea-
torio de la distribución normal con media µ y desviación estándar de parámetro
σ.
5
Implementar el Filtro de Kalman en MATLAB, tal que proporcione el
vector de estados estimados por medio de las ecuaciones siguientes [ [2]]:
1
Ṗ (t) = AP (t) + P (t)AT − P (t)ccT P (t) + T Rv F T (8)
rw
−04
1 10 0
L= P (t)c; F = Rv = (9)
rw 0 10−04
Aplicando las ecuaciones 7, 8, 9, a través de matlab como se muestra en la tabla
??.
2 p=[p1 , p3 ; p2 , p4 ] ;
3 xEs=[ xEs1 ; xEs2 ] ;
4 pPun=A∗p+p∗A’−RwInv∗p∗ c ∗ c ’ ∗ p+F∗Rv∗F ; %Matriz P
5 L= RwInv∗p∗ c ; %Matriz L
6 xEsp=A∗xEs+B∗u+L∗ c ’ ∗ ( xp−xEs ) ; %E s t i m a c i o n v e c t o r de
e s t a d o s E r r o r ( xp−xEs )
Figura 1: Gráfica de los valores analı́tico del sistema() y el valor estimado por
el filtro de Kalman
6
Figura 2: Gráfica de los valores analı́tico del sistema() y el valor estimado por
el filtro de Kalman
7
4.2. Ejercicio 2
El error de estimación lo obtenemos restando los vectores En presencia de
ruido a través de las señales v1 , v2 , w se obtiene la norma euclidiana mostrada
en la tabla ??. El valor del error graficado a través del tiempo para cada uno
de los puntos del sistema se muestra en la figura ??onde tenemos la posición y
la velocidad. El valor normalizado de cada uno de éstos esta dado por:
˙
||x̂(t)|| ≈ 10−04 1 (10)
7 e r r o r n o r m p o s=norm ( x ( : , 1 )−x ( : , 7 ) , 2 )
8 e r r o r n o r m v e l=norm ( x ( : , 2 )−x ( : , 8 ) , 2 )
9
10 errornorm pos =
11
12 1 . 8 2 9 2 e −04
13
14 errornorm vel =
15
16 8 . 8 8 1 2 e −05
8
5. Conclusiones
Con este estudio se ha intentado mostrar las ventajas que supone la imple-
mentación de un filtro Kalman en la estimación de un estado. Se ha comprobado
que se trata de una herramienta muy potente que no solo es capaz de manejar
más de una variable al mismo tiempo, sino que la estimación que ofrece se acerca
bastante a lo observado mediante la experiencia.
9
6. Anexos
El siguiente código es el programa principal que ejecuta MATLAB para
calcular los valores de los estados del sistema y los valores estimados de los
estados por el filtro Kalman.
17 c l c ; % l i m p i a ventana de comandos .
18 close all ;
19 format s h o r t
20 %−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
21 t i =0; h = 0 . 0 0 1 ; t f = 1 0 ; %tiempo i n i c i a l , paso de
i n t e g r a c i o n y tiempo f i n a l , r e s p e c t i v a m e n t e .
22 t =( t i : h : t f ) ; %v e c t o r columna tiempo ( s e g u n d o s ) .
23 xo = [ 0 ; 0 ] ; %c o n d i c i o n e s i n i c i a l e s
24 xoP = [ 0 ; 0 ; 0 ; 0 ; 0 ; 0 ; 0 ; 0 ] ;
25 c o n f i g u r a=o d e s e t ( ’ RelTol ’ , 1 e −06 , ’ AbsTol ’ , 1 e −06 , ’
I n i t i a l S t e p ’ , h , ’ MaxStep ’ , h ) ; % c o n f i g u r a ode45 ( . . . )
26 %−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
27 [ t , x]= ode45 ( ’ f i l t r o k a l m a n ’ , t , xoP , c o n f i g u r a ) ;
28 %i n t e g r a c i o n numerica Runge−Kutta 4/5
29 e r r o r p=x ( : , 1 )−x ( : , 7 ) ;
30 e r r o r v=x ( : , 2 )−x ( : , 8 ) ;
31 e r r o r n o r m p o s=norm ( x ( : , 1 )−x ( : , 7 ) , 2 )
32 e r r o r n o r m v e l=norm ( x ( : , 2 )−x ( : , 8 ) , 2 )
33 %−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
34 figure (1)
35 plot ( t , x (: ,1) , t , x (: ,7) )
36 t i t l e ( ’ E s t i m a c i o n de l a p o s i c i o n por f i l t r o de Kalman ’ )
37 legend ( ’ P o s i c i o n a n a l i t i c a ’ , ’ P o s i c i o n estimada ’ )
38 x l a b e l ( ’ Tiempo s e g u n d o s ( s ) ’ ) ;
39 figure (2)
40 plot ( t , x (: ,2) , t , x (: ,8) )
41 t i t l e ( ’ E s t i m a c i o n de l a v e l o c i d a d por f i l t r o de Kalman ’ )
42 legend ( ’ Velocidad a n a l i t i c a ’ , ’ Velocidad estimada ’ )
43 x l a b e l ( ’ Tiempo s e g u n d o s ( s ) ’ ) ;
44 figure (3)
45 plot ( t , errorp , t , errorv )
46 t i t l e ( ’ G r a f i c a d e l e r r o r a n a l i t i c a vs Kalman en t ’ )
47 legend ( ’ Error p o s i c i o n ’ , ’ Error velocidad ’ )
48 x l a b e l ( ’ Tiempo s e g u n d o s ( s ) ’ ) ;
10
estados del sistema y los valores de estimación es el siguiente:
49 f u n c t i o n x e s t a d o s=f i l t r o k a l m a n ( t , x )
50 u ( t <=0)=0;
51 u ( t >0)=1;
52 x1=x ( 1 , 1 ) ;
53 x2=x ( 2 , 1 ) ;
54 p1=x ( 3 , 1 ) ;
55 p2=x ( 4 , 1 ) ;
56 p3=x ( 5 , 1 ) ;
57 p4=x ( 6 , 1 ) ;
58 xEs1=x ( 7 , 1 ) ;
59 xEs2=x ( 8 , 1 ) ;
60 %−−−−−−−−−−−−−−Parametros d e l s i s t e m a −−−−−−−−−−−−−−−−
61 omegan=s q r t ( 2 ) ; %F r e c u e n c i a n a t u r a l de r e s o n a n c i a
62 rho =3/(2∗ omegan ) ; %F a c t o r de amortiguamiento
63 A=[0 ,1; − omegan ˆ 2 , −2∗rho ∗omegan ] ;
64 B= [ 0 ; omegan ˆ 2 ] ;
65 c =[1;0];
66 F=[10ˆ( −04) , 0 ; 0 , 10ˆ( −04) ] ;
67 Rv=F ;
68 mu= 0 . 5 ;
69 sigma = 0 . 3 ;
70 v=[ normrnd (mu, sigma ) ; normrnd (mu, sigma ) ] ;
71 RwInv=10ˆ( −04) ;
72 %−−−−−−−−−−S i s t e m a a n a l i t i c o −−−−−−−−−−−−−−−−−−−−−−−−−−
73 x=[ x1 ; x2 ] ;
74 xp=A∗x+B∗u+F∗v ; %s i s t e m a dinamico con r u i d o .
75 %−−−−−−−−−−S i s t e m a estimado−−−−−−−−−−−−−−−−−−−−−−−−−−
76 p=[p1 , p3 ; p2 , p4 ] ;
77 xEs=[ xEs1 ; xEs2 ] ;
78 pPun=A∗p+p∗A’−RwInv∗p∗ c ∗ c ’ ∗ p+F∗Rv∗F ; %Matriz P
79 L= RwInv∗p∗ c ; %Matriz L
80 xEsp=A∗xEs+B∗u+L∗ c ’ ∗ ( xp−xEs ) ; %E s t i m a c i o n v e c t o r de
estados
81 %E r r o r ( xp−xEs )
82 x e s t a d o s =[xp ( 1 , 1 ) ; xp ( 2 , 1 ) ; pPun ( 1 , 1 ) ; pPun ( 1 , 2 ) ; pPun ( 2 , 1 )
; pPun ( 2 , 2 ) ; xEsp ( 1 , 1 ) ; xEsp ( 2 , 1 ) ] ;
83 end
11
Referencias
[1] Wakerly, Dennis, et.al. Estádistica matemática con aplicaciones,CENGAGE,
México, 2009
[2] Reyes, Fernando Clase 23: Filtros de Kalman, MCEA-Buap, 2018
[3] Welch, Gary An Introductionto Kalman Filter Deparment of computer
science, University of North Carolina 2006
12