Benemérita Universidad Autónoma

de Puebla

Maestrı́a en Ciencias de la Electrónica

Opción en Automatización

Control Lineal y Servomecanismos

Tarea 15: Filtrado y observadores

Profesor:

Dr. Fernando Reyes Cortés

Alumno:
Rodolfo Emmanuel Fernández Gallardo

Puebla, Pue., 7 de noviembre de 2018

1. Abstract
En el siguiente trabajo mediante un sistema estócastico, se considera como
un sistema en donde se tiene una modificación en la señal de salida debido a
perturbaciones o no se tiene el conocimiento de todos sus estados con certeza,
se plantea el uso de un filtro Kalman el cual nos brinda la capacidad de eliminar
el ruido blanco de una señal, se propone simular la perturbación que genera el
ruido del sistema a partir de una función aleatoria en MATLAB y utilizando
una matriz P variante en el tiempo que ajustará los parámetros, para eliminar
dicha pertubación, obteniendo un error de estimación de valores mucho menores
a 1 en este caso particular de 10−4 .

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]].

El filtro Kalman es un algoritmo que nos sirve para resolver

Problemas de estimación de variables de estado en un sistema dinámico

Este algoritmo resuelve el problema de manera óptima tomando en cuenta

el ruido (o incertidumbre) presente tanto en las variables medidas (valores
entregados por los sensores) como en las variables de control (variables
aplicadas por los actuadores).
En estadı́stica, y especı́ficamente en la teorı́a de la probabilidad, un proceso
estocástico es un concepto matemático que sirve para usar magnitudes aleatorias
que varı́an con el tiempo o para caracterizar una sucesión de variables aleato-
rias (estocásticas) que evolucionan en función de otra variable, generalmente el
tiempo.

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]].

Un proceso estocástico X(t) puede entenderse como una familia paramétrica

de variables aleatorias indexadas mediante el tiempo t. Los procesos estocásticos
permiten tratar procesos dinámicos en los que hay cierta aleatoriedad.

El filtro de Kalman esta basado en las siguientes suposiciones[ [3]]

El sistema es lineal y se tiene una ecuación de estado que describe su
comportamiento dinámico y una ecuación de medición que describe la
relación entre las variables de estado y la salida de los sensores. Ambas
ecuaciones están descritas en tiempo discreto.
Se conoce la estadı́stica del ruido de medición y del ruido del sistema.
Ambos son independientes entre sı́, con media igual a cero y distribución
de probabilidad normal (Gaussiana).

El filtro discreto lo estudiaremos a través de los siguientes puntos:

3
 Proceso de estimación

Bases computacionales del filtrado

Bases probilı́sticas del filtrado
Algoritmo del filtro de Kalman

Parámetros y sintonización del filtro

2.1. Proceso de estimación

El problema que aborda el filtro es tratar de estimar el estado para x ∈ <n
de los procesos de control en tiempo discreto.

xk = Axk−1 + GBuk−1 + wk−1 (1)

con mediciones en z ∈ <m

zk = Hxk + vk (2)
La funciones de distribución de probabilidad
p(w) ∼ N (0, Q)

p(v) ∼ N (0, R)

2.1.1. Distribución Gaussiana

La distribución continua más utilizada en estadı́stica es la distribución gaus-
siana o normal. Una variable aleatoria X con función de densidad de probabi-
lidad
1 −(x−µ)2
f (x) = √ e 2σ2 ; −∞ < x < ∞ (3)
2πσ
es una variable aleatoria normal con parámetros µ, donde ∞ < µ < ∞, y σ > 0,

E(x) = µ y V (X) = σ 2 (4)

y la notación N (µ, σ 2 ) es usada para denotar la distribución. La media y varianza
de X es la constante de normalización necesaria para asegurar que la densidad
se integre a 1 [ [1]]

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.

4. Planteamiento del Problema

4.1. Sistema lineal estocástico
Considere el sistema lineal estocástico descrito por
     −04  
0 1 0 10 0 v1
ẋ = x+ u+ (5)
−ωn2 −2ρωn −ωn2 0 10−04 v2
| {z }
F

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
σ.

Tabla 1: Función en MATLAB para obtener los valores aleatorios normalizados

1 r = normrnd (mu, sigma )

5
 Implementar el Filtro de Kalman en MATLAB, tal que proporcione el
vector de estados estimados por medio de las ecuaciones siguientes [ [2]]:

x̂˙ = Ax̂ + Bu + LcT x̃

ˆ (7)

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
??.

Tabla 2: Programa en MATLAB para obtener el filtrado de Kalman

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 )

La gráfica de la posición y la velocidad se muestran en la figura 1 y 3 respecti-

vamente, en un acercamiento a las gráficas se muestra que el error de estimación
es el aproximadamente de 10−4 mostrado en las gráficas 2 y 4 cómo se plantea
en los objetivos especı́ficos del trabajo valor que mencionaremos posteriormente.

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

Figura 3: Código para obtener la estimación

Figura 4: Código para obtener la estimación

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)

Tabla 3: Norma eculidiana en MATLAB y el valor del error

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

Figura 5: Gráficas de Posición y velocidad

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.

El desarrollo de un modelo de ruido aleatorio para simular las condiciones

reales de las perturbaciones en el sistema demuestra su capacidad de estimación,
ya que como se ha dicho esos errores son constantes en la realidad y representan
un modelo altamente fidedigno de ésta.

En el objetivo de aplicar un filtrado Kalman a este sistema estocástico. Se ha

comprobado que el proceso que utiliza este filtro para aproximar no es del todo
exacto, se presenta como una alternativa con gran precisión en el desarrollo de
estos estimadores matemáticos.

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.

Tabla 4: Función en MATLAB para calcular y estimar el valor de los estados

por medio del 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 ) ’ ) ;

El programa de la función para realizar los cálculos correspondientes de los

10
 estados del sistema y los valores de estimación es el siguiente:

Tabla 5: Función en MATLAB para calcular y estimar el valor de los estados

por medio del filtro Kalman

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

[4] Brown, R. G. and P. Y. C. Hwang. Introduction to Random Signals and

Applied Kalman Filtering, Second Edition, John Wiley and Sons, Inc. 1992

12