Benemérita Universidad Autónoma

de Puebla

Facultad Ciencias de la Electrónica

Maestría en Ciencias de la Electrónica
Opción en Automatización

Control Lineal y Servomecanismos

Profesor:
Dr. Fernando Reyes Cortés

Tarea 16: Filtro de Kalman (aspectos prácticos)

Alumno:

Abel Alejandro Rubín Alvarado

Puebla, Pue., 14 de noviembre de 2018
RV
Tarea 16: Filtro de Kalman (aspectos prácticos)
Facultad de Ciencias
de la Electrónica

Resumen
Las señales de ruido pueden afectar en forma muy negativa a los sistemas. La aplicación de los ltros
de Kalman en estos casos constituye una alternativa capaz de producir notables mejoras en su desempeño,
ademas de ofrecer una ventaja muy poderosa debido a que combina los benecios de un ltro sin afectar
la dinámica del sistema, ademas de la posibilidad de estimar las variables de estado. Estas singularidades
que nos ofrece el ltro de Kalman son presentados en este trabajo, analizaremos un sistema el cual esta
afectado por un ruido de distribución normal, donde realizaremos la implementación del ltro de Kalman
para eliminar el ruido que afecta a dicho sistema, y estimaremos el vector de estados del mismo para
poder apreciar los benecios de emplear esta poderosa herramienta, estas simulaciones la desarrollamos
en MATLAB con ayuda de un algoritmo para la solución numérica de ecuaciones diferenciales de primer
orden (ODE45) y vericaremos de manera gráca la comparación del sistema original con la estimación
resultado de aplicar el Filtro de Kalman logrando obtener un error de estimación del orden de 10−04 .

1. Introducción

1.1. Ruido
Las perturbaciones a que está sujeto un sistema de control automático provocan que su salida se aleje
del comportamiento ideal. Por ello, un buen diseño debe contemplar medidas que le permitan mantener un
desempeño satisfactorio, incluso en la presencia de este tipo de señales, las cuales pueden considerarse de
naturaleza determinística o aleatoria.

El estudio sistemático de las señales aleatorias y de los medios que podrían utilizarse para minimizar sus
efectos ya había originado en la década de los años cuarenta los ltros de error medio cuadrático (ltro de
Wiener). Sin embargo, su aplicación en aquella época enfrentaba limitaciones, tales como su utilización con
señales discretas y en problemas complejos de múltiples entradas y salidas.

En marzo de 1960, el Dr. Rudolph Kalman publica su artículo .A New Approach to Linear Filtering and
Prediction Problems", con lo que realiza un aporte monumental a la solución del problema de diseño de
ltros óptimos, utilizando la técnica de espacios de estado. Kalman consideraba la recursividad, así como
casos de múltiples entradas y salidas, además de que permitía el uso de métodos digitales modernos para su
implementación.

Figura 1: Afectación del ruido en señal

Para poder contrarrestar estos efectos en nuestras señales, un método bastante efectivo es la aplicación
de ltros, sin embargo existe uno en particular que no afecta la dinámica de nuestro sistema y ademas de
lograr estimar las variables de estado es el ltro de Kalman.

Av. San Claudio y 18 Sur, Facultad Ciencias de la Electrónica Edif. FCE6, Col. San Manuel, Ciudad
Universitaria, Puebla, Pue.
2
Tarea 16: Filtro de Kalman (aspectos prácticos)
Facultad de Ciencias
de la Electrónica

1.2. Filtros de Kalman

El ltro de Kalman es un observador compuesto por un conjunto de ecuaciones que permiten la estima-
ción de estados en presencia de ruido de sistemas dinámicos.

El ltro de Kalman es un método que permite estimar variables de estado no observables a partir de
variables observables que pueden contener algún error de medición.

Es un algoritmo que requiere dos tipos de ecuaciones: las que relacionan las variables de estado con las
variables observables (ecuaciones principales) y las que determinan la estructura temporal de las variables
de estado (ecuaciones de estado).

Las estimaciones de las variables de estado se realizan en base a la dinámica de estas variables (dimensión
temporal) así como de las mediciones de las variables observables que se van obteniendo en cada instante
del tiempo (dimensión transversal). Es decir, la dinámica se resumen en dos pasos:

• Estimar las variables de estado utilizando su propia dinámica (etapa de predicción).

• Mejorar esa primera estimación utilizando la información de las variables observables (etapa de correc-
ción).

Una característica muy atractiva de la metododología que nos ocupa es que tiene carácter recursivo. Una
vez que el algoritmo pronostica el nuevo estado en el momento t , añade un término de corrección y el nuevo
estado “corregido” sirve como condición inicial en la siguiente etapa, t + 1.

De esta forma, la estimación de las variables de estado utiliza toda la información disponible hasta ese
momento y no sólo la información hasta la etapa anterior al momento en el cual se realiza la estimación.

Sea un sistema de una sola entrada de control u(t) y una sola salida y(t), cuya descripción de estado es:

ẋ = Ax + Bu + F v
y = cT x + w

Considere la siguiente estructura de observador:

x̂˙ = Ax̂ + Bu + L(y − cT x̂)

x̂˙ = Ax̂ + Bu + L(cT x − cT x̂) (1)
x̂˙ = Ax̂ + Bu + Lc x̃
ˆT

El sistema dinámico con error de estimación queda de la siguiente forma:

Av. San Claudio y 18 Sur, Facultad Ciencias de la Electrónica Edif. FCE6, Col. San Manuel, Ciudad
Universitaria, Puebla, Pue.
3
Tarea 16: Filtro de Kalman (aspectos prácticos)
Facultad de Ciencias
de la Electrónica

ẋ − x̂˙ = Ax̂ + Bu + L(y − cT x̂) + F v − Ax̂ − Bu − LcT x̃

ˆ)
(2)
ˆ˙ = A − LcT x̃
x̃ ˆ + Fv
 

La problemática consiste en encontrar una adecuada estructura para L = L(t) ∈ <n×1 , tal que, en
ausencia de ruido se mantenga la convergencia asintótica del error de estimación de estados x̃(t)
ˆ ∈ <n ; y, en
presencia de ruido, el error de estimación ||x̃(t)||
ˆ < ρ << 1, con ρ ∈ <+

Objetivos

General
Analizar un sistema lineal estocástico, el cual se ve afectado por ruido con una distribución normal, para
la aplicación de un un ltro de Kalman, realizando la metodología para comprobar la reducción del ruido
que la afecta y estimar el vector de estados, y vericar que el error de la estimación es al menos del orden
de 10−04

Especícos
• El alumno aplicara los conceptos antes aprendidos de trabajos anteriores correspondientes a la imple-
mentación de ode45.
• Comprenderá, analizara la manera de implementar y dar solución a la estructura del ltro de Kalman.
• Revisar mediante MATLAB que el ltro cumpla su función.
• Vericar mediante la norma euclidiana el orden del error

Av. San Claudio y 18 Sur, Facultad Ciencias de la Electrónica Edif. FCE6, Col. San Manuel, Ciudad
Universitaria, Puebla, Pue.
4
Tarea 16: Filtro de Kalman (aspectos prácticos)
Facultad de Ciencias
de la Electrónica

Planteamiento del Problema

Considere el siguiente sistema lineal estocástico:

     −04  
0 1 0 10 0 v1
ẋ = x+ u+
−ωn2 −2ρω ωn2 0 10−04 v2 (3)
y = [1 0]x + w
√
donde la frecuencia natural de resonancia es ωn = 2, y el factor de amortiguamiento ρ = 2ωn
3
; la señal
de entrada u corresponde a un escalón unitario, las señales de ruido v1, v2, ω ∈ < son señales aleatorias
(ruido con distribución normal), y rw1
= 1004 .

1) Implementar en MATLAB el ltro de Kalman:

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

ˆ
1
Ṗ (t) = AP (t) + P (t)AT − P (t)ccT P (t) + F Rv F T
rw
Donde :
(4)
10−04
 
1 0
L= P (t)c; F = Rv =
rw 0 10−04

tal que, proporcione el vector de estados estimados x̂ = [x̂1 x̂2 ]T del correspondiente sistema dinámico
lineal (5), considere un periodo de muestreo h = 0 : 001s.
2) En presencia de ruido a través de las señales v1 , v2 , w muestre que su simulación obtiene la norma eucli-
diana del error de estimación ||x̃(t)||
ˆ ≈ 10−04 << 1

Av. San Claudio y 18 Sur, Facultad Ciencias de la Electrónica Edif. FCE6, Col. San Manuel, Ciudad
Universitaria, Puebla, Pue.
5
Tarea 16: Filtro de Kalman (aspectos prácticos)
Facultad de Ciencias
de la Electrónica

Desarrollo

1.3. Solución 1)
Considere el siguiente sistema lineal estocástico:

     −04  
0 1 0 10 0 v1
ẋ = x+ u +
−ωn2 −2ρω ωn2 0 10−04 v2 (5)
y = [1 0]x + w
√
donde la frecuencia natural de resonancia es ωn = 2, y el factor de amortiguamiento ρ = 2ωn
3
; la señal
de entrada u corresponde a un escalón unitario, las señales de ruido v1, v2, ω ∈ < son señales aleatorias
(ruido con distribución normal), y rw1
= 1004 .

1) Implementar en MATLAB el ltro de Kalman:

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

ˆ
1
Ṗ (t) = AP (t) + P (t)AT − P (t)ccT P (t) + F Rv F T
rw
Donde :
(6)
10−04
 
1 0
L= P (t)c; F = Rv =
rw 0 10−04

tal que, proporcione el vector de estados estimados x̂ = [x̂1 x̂2 ]T del correspondiente sistema dinámico
lineal (5), considere un periodo de muestreo h = 0 : 001s.
Para llevar a cabo esta implementación realizaremos un código en la herramienta de MATLAB que nos
permita desarrollar una ecuación diferencial ordinaria (ODE), estamos hablando de la función ODE45, si
bien este método nos permite resolver de manera numérica nuestro sistema en espacio de estados de la (5)
al que agregaremos el ruido.
Podemos generar nuestra función que sera llamada por un código principal que resolverá de forma nu-
mérica el sistema, dicha función es la siguiente:
function sOrig=SistOrig(t,x)
u(t==0)=0;
u(t>0)=1;
Wn=sqrt(2);
Rho=3/(2*Wn);
A=[0 1;-Wn^2 -2*Rho*Wn];
B=[0;Wn^2];
c=[1 0];
F=[10^(-04) 0; 0 10^(-04)];
rd=[normrnd(0,.2);normrnd(0,.2)];
sOrig=A*x+B*u+F*rd;
end

Av. San Claudio y 18 Sur, Facultad Ciencias de la Electrónica Edif. FCE6, Col. San Manuel, Ciudad
Universitaria, Puebla, Pue.
6
Tarea 16: Filtro de Kalman (aspectos prácticos)
Facultad de Ciencias
de la Electrónica

Obteniendo así la solución del sistema (5) con ruido como lo muestra la siguiente gura 2 y aplicando un
zoom para visualizar el ruido que se encuentra presente en el sistema.

(a) Sistema original (b) zoom

Figura 2: Respuesta del sistema dinámico, mediante solución analítica Runge-Kuta

Ahora vamos a implementar el ltro de Kalman de acuerdo a la ecuación (1) implementaremos una ode45
que resuelva la ODE correspondiente a Ṗ (t) de la ecuación (6) el codigo es el siguiente:

function fK=FilKalman(t,x)
u(t==0)=0;
u(t>0)=1;
wn=sqrt(2);
rho=3/(2*wn);
rw=10^(-04);
A=[0 1;-wn^2 -2*rho*wn];
c=[1;0];
F=[10^(-04), 0;0, 10^(-04)];
P=[x(1,1) x(2,1); x(3,1) x(4,1)];
Rv=F;
xp1=A*P+P*A'-rw*P*c*c'*P+F*Rv*F';
fK=[xp1(1,1);xp1(1,2);xp1(2,1);xp1(2,2)];

La solución o el vector solución nos ayudara para poder obtener los valores de P (t) y mediante una
iteración construir nuestro vector L, como se muestra a continuación:

[t, fK]=ode45( 'FilKalman',ts, x0,opciones);%fK valores de P(t)

P(1,1,:)=fK(:,1);P(1,2,:)=fK(:,2);P(2,1,:)=fK(:,3);P(2,2,:)=fK(:,4);
c=[1 0];
rw=10^(-4);
for i=1:((tf-ti)/h+1)
L(:,i)=rw*P(:,:,i)*c';
end

Una vez obtenidos obtenidos los valores de L ya es posible implementar el ltro de Kalman al sistema y
original para poder estimar el vector de estados estimado x̂ = [x̂1 x̂2 ]T , esto es implementado en el codigo

Av. San Claudio y 18 Sur, Facultad Ciencias de la Electrónica Edif. FCE6, Col. San Manuel, Ciudad
Universitaria, Puebla, Pue.
7
Tarea 16: Filtro de Kalman (aspectos prácticos)
Facultad de Ciencias
de la Electrónica

principal que hace el llamado a las funciones antes mencionadas y gráca el vector de estados, el código
principal es el siguiente:

clc;
clear all;
close all;
format short
global L
ti=0; h=0.001; tf = 10; ts=(ti:h:tf);
confg=odeset('RelTol',1e-06,'AbsTol'1e-06,'InitialStep',h,'MaxStep',h);
x0=[0;0];
[t, sOrig]=ode45( 'SistOrig',ts, x0,confg);
x02=[0;0;0;0];
[t, fK]=ode45( 'FilKalman',ts, x02,confg);
P(1,1,:)=fK(:,1);P(1,2,:)=fK(:,2);P(2,1,:)=fK(:,3);P(2,2,:)=fK(:,4);
c=[1 0];
rw=10^(-4);
for i=1:((tf-ti)/h+1)
L(:,i)=rw*P(:,:,i)*c';
end
[t, K]=ode45( 'Kalman',ts, cond_iniciales,opciones);
rd=normrnd(0,.2,[size(t,1),1])*10^(-4);
Y=c*sOrig'+rd';
figure
plot(t,Y);xlabel('Segundos');title('Sistema original con Ruido de distribución normal');
figure
plot(t,sOrig(:,1),t,K(:,2));xlabel('Segundos');title('Comparación sistema original Vs estimación kalma
figure
plot(t,sOrig(:,1),t,K(:,1),'g')

Obteniendo las siguientes gracas donde realizaremos la comparación del sistema original Vs la estimación
realizada por el ltro de Kalman en la siguiente gura:

(a) Sistema original (b) zoom

Figura 3: Comparación de posición:sistema dinámico original Vs solución analítica mediante el ltro de Kalman

Av. San Claudio y 18 Sur, Facultad Ciencias de la Electrónica Edif. FCE6, Col. San Manuel, Ciudad
Universitaria, Puebla, Pue.
8
Tarea 16: Filtro de Kalman (aspectos prácticos)
Facultad de Ciencias
de la Electrónica

Ahora veamos el resultado de estimar el vector de estados, de la solución de aplicar el ltro de kalman,
como podemos apreciar en la gura 3 donde el ruido ha sido reducido en su gran mayoria y nos otorga una
señal con la cual se pude trabajar de una manera mas eciente.

Figura 4: Estimación del vector de estados del sistema original

Av. San Claudio y 18 Sur, Facultad Ciencias de la Electrónica Edif. FCE6, Col. San Manuel, Ciudad
Universitaria, Puebla, Pue.
9
Tarea 16: Filtro de Kalman (aspectos prácticos)
Facultad de Ciencias
de la Electrónica

1.4. Solución 2)
2) En presencia de ruido a través de las señales v1 , v2 , w muestre que su simulación obtiene la norma eucli-
diana del error de estimación ||x̃(t)||
ˆ ≈ 10−04 << 1

Para poder dar solución a este inciso, es necesario obtener el error entre el sistema en presencia del ruido
y la estimación del vector de estados que se obtuvo en el inciso anterior, esto lo gracaremos y obtendremos
la norma euclidiana de esa diferencia, esto con apoyo de la funcion norm que nos provee el entorno de Matlab.

La graca 5 nos muestra la posición del sistema original afectado por el ruido y la estimación obtenida
por el ltro de kalman:

(a) Sistema original (b) zoom

Figura 5: Comparación de posición:sistema dinámico original Vs solución analítica mediante el ltro de Kalman

Obteniendo un error de 2 × 10−03 , ahora si comparamos la posición del sistema original con la obtenida
por la estimación del ltro de kalman como lo muestra la gráca 6 podemos observar el error que se obtiene
es del orden de 4.6 × 10−05

(a) Sistema original (b) zoom

Figura 6: Comparación de posición:sistema dinámico original Vs solución analítica mediante el ltro de Kalman

Av. San Claudio y 18 Sur, Facultad Ciencias de la Electrónica Edif. FCE6, Col. San Manuel, Ciudad
Universitaria, Puebla, Pue.
10
Tarea 16: Filtro de Kalman (aspectos prácticos)
Facultad de Ciencias
de la Electrónica

2. Conclusiones

Se logro vericar las características principales del ltro de Kalman como lo son el reducir de manera
signicativa el ruido que se encontraba perturbando a nuestro sistema, ademas de lograr estimar el vector de
estados, para su implementación se presentaron ciertos inconvenientes, como lo fue el comprender que P (t)
se obtiene a partir de la solución numérica y la posibilidad de generar el vector L, sin embargo considero
que es posible obtener un algoritmo mas ecaz para dar solución con menos funciones ode45.

Para la obtención del error, entre el sistema original en presencia del ruido con distribución normal al
estimar el vector de estados solo hicimos uso de la función que ofrece matlab, aunque los valores en la
presencia de ruido y el estimado no son los esperados, si lo es el calculado entre el sistema original sin el
ruido y el estimado por el ltro de Kalman, considero que el que es representativo es el segundo debido a
que se compara el estimado con la posición original.

Referencias

Reyes Cortés, Fernando (2018a), Capítulo 2, Clase 8: Capítulo 2: Sistemas lineales con enfoque en variables
de estado, Material del curso de Control Lineal y Servomecanismos, CU,Puebla,Mexico.
*ftp://ece.buap.mx/pub/profesor/FernandoReyes/CLSM/
Reyes Cortés, Fernando (2018b), Capítulo 5: Filtrado y Observadores, Material del curso de Control Lineal
y Servomecanismos, Puebla,Mexico.
*ftp://ece.buap.mx/pub/profesor/FernandoReyes/CLSM/

Av. San Claudio y 18 Sur, Facultad Ciencias de la Electrónica Edif. FCE6, Col. San Manuel, Ciudad
Universitaria, Puebla, Pue.
11