INFORME CONCEPTOS TEORIA Periodo 2018-1

DEPARTAMENTO DE INGENIERÍA MECATRÓNICA

Filtro de Kalman y Filtro de Partículas

Fabio S. Guerra Duarte
{1802716}@unimilitar.edu.co

Resumen— Este documento pretende dar a conocer los conceptos funcionamiento del filtro, éste se desempeña bien en la mayoría
básicos de la estimación y sus diferentes métodos por estimadores de situaciones.
Bayesianos, comprendiendio propiedades y características por medio
de la simulación en Matlab A. El filtro de Kalman Discreto

Palabras clave— Transformada Z, Convolución de señales,

Ecuaciones en diferencias, Análisis frecuencial, Serie de Fourier,,B. En esta sección se describe el filtro en su formulación original
Simetría, Función de transferencia, Matlab (Kalman 1960) donde las mediciones ocurren y el estado es
estimado en puntos discretos en el tiempo
I. INTRODUCCIÓN

El filtro de Kalman apunta al problema general de tratar de

Filtro de Kalman estimar el estado xe de un proceso controlado en tiempo
discreto que es gobernado por una ecuación en diferencias
lineal estocástica.
El filtro de Kalman es un algoritmo que se basa en el modelo
de espacio de estados de un sistema para estimar el estado
futuro y la salida futura realizando un filtrado optimo a la señal
de salida, y dependiendo del retraso de las muestras que se leX (1)
ingresan puede cumplir la función de estimador de parámetros
o únicamente de filtro. Pero en ambos casos elimina ruido, estas
ecuaciones son ampliamente utilizadas ya que incluyen Con una medición z R^m que es:
probabilidades estadísticas puesto que toma en cuenta la
aleatoriedad tanto de la señal como del ruido. [1].A diferencia
de otros tipos de filtros este no requiere de una frecuencia de
corte específica debido a que se basa en la característica del (2)Las variables aleatorias Wk y Vk representan
ruido permitiendo de esta manera filtrar en todo el espectro de respectivamente el ruido del proceso y de la medición. Se
frecuencias. Además sus ecuaciones solo dependen de una asumen que son independientes una de la otra, blancas y con
muestra anterior y la muestra presente lo que permite un ahorro distribución normal de probabilidad.
considerable de memoria a la hora de ser implementado en un
sistema digital y su fácil programación lo hacen muy atractivo
ya que se basa en un método recursivo [2] p(W)~N( 0, Q) (3)

Entre varias de sus aplicaciones se encuentran la estimación p(V) ~N(0,R) (4)

demográfica, procesamiento de señales biológicas, sistemas de
navegación, predecir el comportamiento de variables
económicas, procesamiento de imágenes, entre otras. Debido a
su gran campo de acción se hace muy importante conocer su
funcionamiento para así tener las herramientas básicas que
permitan la solución de diversos problemas prácticos de forma
sencilla y óptima[3]
El filtro de Kalman es esencialmente una serie de ecuaciones
matemáticas que implementan un estimador tipo predictor -
corrector que es óptimo en el sentido que minimiza el error
estimado de la covarianza, cuando algunas condiciones son
dadas. Desde el momento de su introducción, el filtro de
Kalman ha sido sujeto de investigación autónoma o asistida.
Esto es debido a que en gran parte de los avances en la
computación digital se ha trabajado se ha trabajo para hacer el
filtro práctico, pero relativamente simple y robusto. Aunque no
siempre se presentan todas las condiciones óptimas para el
En la práctica la covarianza del ruido del proceso Q y la
covarianza del ruido de la medición R son matrices que pueden
cambiar con cada paso en el tiempo o medición, sin embargo se
asumen que son constantes.
La matriz A de nxn en la ecuación en diferentes relaciona el
estado en el paso anterior k-1 con el estado actual, en ausencia
de función de conducción o ruido del proceso. Nótese que en la
práctica A puede cambiar con cada paso del tiempo, pero aquí
se asume constante. La matriz B de nx1 se relaciona con la
entrada opcional de control n e !" al estado x. La matriz H de
mxm en la ecuación en diferencia de la medición, relaciona el
estado con la medición. En la práctica la matriz H puede
cambiar con cada paso del tiempo, pero aquí se asume
constante.
Página 1 de 8
 INFORME CONCEPTOS TEORIA
DEPARTAMENTO DE INGENIERÍA MECATRÓNICA
C. Algoritmo del filtro de Kalman discreto
El filtro de Kalman estima un proceso usando una forma de
realimentación y control: el filtro estima el estado del proceso
en un tiempo y después obtiene la realimentación en forma de
mediciones (ruidosas). Las ecuaciones del filtro de Kalman
caen en dos grupos: ecuaciones de actualización de tiempo y
ecuaciones de actualización de mediciones. Las ecuaciones de
medición el tiempo son responsables de proyectar hacia
adelante (en el tiempo) el estado actual y estimaciones de error
y covarianza para obtener los estimados a priori del siguiente
paso en el tiempo. Las ecuaciones de actualización de medida
pueden ser vistas como ecuaciones correctoras. De hecho el
algoritmo final de estimación reúne los algoritmos de
predicción-corrección para la resolución de problemas
numéricos
Las ecuaciones específicas para actualización de tiempo y
medida son
De nuevo nótese como la actualización de tiempo en la tabla 1
proyecta el estado y la covarianza estimados adelante en el
tiempo del paso k-1 al paso k. A y B son matrices del modelo
de estados mientras que Q es la varianza del ruido del proceso
Y por último para la siguiente iteración debemos hacer una
reasignación de las variables
La primera tarea durante la actualización de medidas es el
cálculo de la ganancia de Kalman, Kk. El siguiente paso es
medir el proceso para obtener zk, y después generar un
estimado del estado a posteriori incorporado a la medición. El
paso final es la obtención de una covarianza de error estimada
a posteriori.
Periodo 2018-1
Después de cada par de actualizaciones de tiempo y
mediciones, el proceso es repetido con el estimado previo a
posteriori usado para proyectar o predecir el nuevo estimado a
priori. Esta naturaleza recursiva es una de las buenas cualidades
del filtro de Kalman, lo que hace prácticas muchas
implementaciones. El filtro de Kalman a pesar de sus
condiciones recursivas basa su estimado actual en todas las
mediciones pasadas. La figura 2 ofrece un esquema completo
de la operación del filtro Siendo Pk el estimado de la covarianza
del error a priori y Rk la covarianza del error medio.
Simulación
En esta sección se presentan simulaciones del filtro de Kalman,
utilizado como estimador sobre un sistema hipotético de
segundo orden, este sistema es definido mediante el modelo de
espacio de estados, además se inserta ruido aleatorio a la señal
de entrada del estimador, utilizándolo también como filtro.
%% define nuestras meta-variables (es
decir, cuánto tiempo ya menudo
muestrearemos)
duracion = 10% cuánto tiempo vuela Quail
dt = .1; % El Ninja busca continuamente
el pajaro,
% pero supondremos que está muestreando
repetidamente a lo largo del tiempo en
un intervalo fijo
%% Definir ecuaciones de actualización
(matrices de coeficientes): un modelo
basado en la física para donde esperamos
que la codorniz sea [transición de
estado (estado + velocidad)] + [control
de entrada (aceleración)]
Página 2 de 8
 INFORME CONCEPTOS TEORIA Periodo 2018-1
DEPARTAMENTO DE INGENIERÍA MECATRÓNICA

A = [1 dt; 0 1]; % de matriz de

transición de estado: vuelo esperado de
la codorniz (predicción de estado)
B = [dt ^ 2/2; dt]; % de matriz de
control de entrada: efecto esperado de
la aceleración de entrada en el estado.
C = [1 0]; % de matriz de medición: la
medición esperada dado el estado
predicho (probabilidad)
% ya que solo estamos midiendo la
posición (demasiado difícil para que el
ninja calcule la velocidad),
establecemos la variable de velocidad en
cero.

%% definir variables principales

u = 1.5; % define la magnitud de
aceleración
Q = [0; 0]; % estado inicializado -
tiene dos componentes: [posición;
velocidad] de la codorniz
Q_estimado = Q; % x_estimate de la
estimación de ubicación inicial de donde
está Quail (lo que estamos actualizando)
QuailAccel_ruido_mag = 0.05; % de ruido
de proceso: la variabilidad en la
velocidad de aceleración de la codorniz
(stdv de aceleración: metros / seg ^ 2)
OjoNinja_ruido_mag = 10; % de ruido de
medición: qué tan oculto está el Ninja
(stdv de ubicación, en metros)
Ez = OjoNinja_ruido_mag ^ 2;% Ez
convierte el ruido de medición (stdv) en
una matriz de covarianza
Ex = QuailAccel_ruido_mag ^ 2 * [dt ^
4/4 dt ^ 3/2; dt ^ 3/2 dt ^ 2]; % Ex
convierte el ruido de proceso (stdv) en
matriz de covarianza
P = Ex; % estimación de la varianza
inicial de la posición de las codornices
(matriz de covarianza)

%% iniciar las variables de resultados %% simula lo que el Ninja ve en el

% Inicializar para la velocidad
Q_ubi = []; % ACTUAL Ruta de vuelo de
codorniz
vel = []; % De velocidad ACTUAL de
codorniz
Q_Medir_ubi = []; %La ruta de lacodorniz
que el Ninja ve
tiempo
figure(2); clf
figure(1); clf
for t = 0 : dt: duracion
% Generar el vuelo de codorniz
QuailAccel_ruido =
QuailAccel_ruido_mag * [(dt^2/2)*randn;
dt*randn];
Q= A * Q+ B * u + QuailAccel_ruido;
% Genera lo que el Ninja ve
OjoNinja_ruido = OjoNinja_ruido_mag
* randn;
y = C * Q+ OjoNinja_ruido;
Q_ubi = [Q_ubi; Q(1)];
Q_Medir_ubi = [Q_Medir_ubi; y];
vel = [vel; Q(2)];

Página 3 de 8
 INFORME CONCEPTOS TEORIA Periodo 2018-1
DEPARTAMENTO DE INGENIERÍA MECATRÓNICA

% iterativamente traza lo que el

ninja ve
plot(0:dt:t, Q_ubi, '-r.')
plot(0:dt:t, Q_Medir_ubi, '-k.')
axis([0 10 -30 80])
hold on
pause()
end

% grafica de la ruta teórica de ninja

que no usa kalman
plot(0:dt:t, smooth(Q_Medir_ubi), '-g.')

% gráfica de la velocidad, solo para

mostrar que está aumentando
constantemente, debido al
% de aceleración constante
figure(2);
plot(0:dt:t, vel, '-b.')

Evolucion de estados Final

inicial

Implementación en el laboratorio

Para el laboratorio se tomarían las ecuaciones de

movimiento lineal uniforme como modelo estimando
la posición, velocidad y aceleración y llenando las
matrices del observador con los valores otorgados
por el fabricante de cada sensor

Página 4 de 8
 INFORME CONCEPTOS TEORIA Periodo 2018-1
DEPARTAMENTO DE INGENIERÍA MECATRÓNICA

Donde xt denota el estado del sistema en el instante

t, y zt denota la medida como una función de un
FILTRO DE PARTICULAS estado del sistema desconocido en el instante t.

los filtros de partículas. El filtro de partículas es una

técnica basada en estimación Bayesiana recursiva de
un estado del sistema desconocido. El interés que han
despertado estas técnicas radica en su habilidad para
trabajar con múltiples hipótesis, y en la posibilidad
de desarrollar implementaciones simples y
eficientes.

En la aproximación Bayesiana a la estimación de Las relaciones de recurrencia de estas ecuaciones

estados dinámicos, el objetivo es construir la f.d.p. a constituyen la base de la solución Bayesiana óptima.
posteriori del estado basándose en toda la La propagación recursiva de la densidad a posteriori
información disponible, incluyendo el conjunto de es sólo una solución conceptual que, en general, no
medidas recibidas. Dado que esta f.d.p. agrupa toda puede ser determinada analíticamente. Existen
la información estadística disponible, se trata de una soluciones en un conjunto restrictivo de casos, entre
solución completa al problema de estimación, y se las que se incluye el Filtro de Kalman, descrito en la
puede obtener una estimación óptima del estado siguiente sección. También describiremos como,
(respecto a cualquier criterio). cuando no existe solución analítica, el filtro de
Kalman extendido (EKF) y el filtro de partículas
Para muchos problemas, se requiere una estimación aproximan la solución Bayesiana óptima.
cada vez que se recibe una nueva medida. En este
caso, un filtro recursivo es una solución conveniente.
Una aproximación mediante filtrado recursivo
permite que los datos recibidos sean procesados
secuencialmente en vez de en bloque, por lo que no
es necesario almacenar todo el conjunto de datos o
reprocesar datos existentes cuando una nueva
medida está disponible. Un filtro de este tipo consta
de dos etapas básicas: predicción y actualización. La
etapa de predicción utiliza el modelo de movimiento
para predecir el estado de la f.d.p. de un instante al
siguiente. Dado que el estado está normalmente
sujeto a perturbaciones desconocidas (modeladas
como ruido Gaussiano), la predicción generalmente
traslada, deforma y expande la f.d.p.. La etapa de
actualización utiliza la última medida para modificar
la predicción de la f.d.p.. Para definir el problema del
seguimiento, considérese un sistema de ecuaciones
movimiento y verosimilitud dependientes del tiempo

clear all
close all
ecuación de observación clc

Página 5 de 8
 INFORME CONCEPTOS TEORIA
DEPARTAMENTO DE INGENIERÍA MECATRÓNICA
%% initialize the variables
set(0,'DefaultFigureWindowStyle','docked
') %dock the figures..just a personal
preference you don't need this.
x = 0.1; % initial actual state
x_N = 1; % Noise covariance in the
system (i.e. process noise in the state
update, here, we'll use a gaussian.)
x_R = 1; % Noise covariance in the
measurement (i.e. the Quail creates
complex illusions in its trail!)
T = 75; % duration the chase (i.e.
number of iterations).
N = 10; % The number of particles the
system generates. The larger this is,
the better your approximation, but the
more computation you need.
%initilize our initial, prior particle
distribution as a gaussian around
%the true initial value
V = 2; %define the variance of the
initial esimate
x_P = []; % define the vector of
particles
% make the randomly generated particles
from the initial prior gaussian
distribution
for i = 1:N
x_P(i) = x + sqrt(V) * randn;
end
%{
%show the distribution the particles
around this initial value of x.
figure(1)
clf
subplot(121)
plot(1,x_P,'.k','markersize',5)
xlabel('time step')
ylabel('flight position')
subplot(122)
hist(x_P,100)
xlabel('flight position')
ylabel('count')
pause
%}
%the functions used by the Quail are:
% x = 0.5*x + 25*x/(1 + x^2) +
8*cos(1.2*(t-1)) + PROCESS NOISE -->
sqrt(x_N)*randn
% z = x^2/20 + MEASUREMENT NOISE -->
sqrt(x_R)*randn;
%generate the observations from the
randomly selected particles, based upon
Periodo 2018-1
%the given function
z_out = [x^2 / 20 + sqrt(x_R) * randn];
%the actual output vector for
measurement values.
x_out = [x]; %the actual output vector
for measurement values.
x_est = [x]; % time by time output of
the particle filters estimate
x_est_out = [x_est]; % the vector of
particle filter estimates.
for t = 1:T
%from the previou time step, update
the flight position, and observed
%position (i.e. update the Quails
position with the non linear function
%and update from this position what
the chasing ninja's see confounded
%by the Quails illusions.
x = 0.5*x + 25*x/(1 + x^2) +
8*cos(1.2*(t-1)) + sqrt(x_N)*randn;
z = x^2/20 + sqrt(x_R)*randn;
%Here, we do the particle filter
for i = 1:N
%given the prior set of particle
(i.e. randomly generated locations
%the quail might be), run each
of these particles through the state
%update model to make a new set
of transitioned particles.
x_P_update(i) = 0.5*x_P(i) +
25*x_P(i)/(1 + x_P(i)^2) + 8*cos(1.2*(t-
1)) + sqrt(x_N)*randn;
%with these new updated particle
locations, update the observations
%for each of these particles.
z_update(i) =
x_P_update(i)^2/20;
%Generate the weights for each
of these particles.
%The weights are based upon the
probability of the given
%observation for a particle,
GIVEN the actual observation.
%That is, if we observe a
location z, and we know our observation
error is
%guassian with variance x_R,
then the probability of seeing a given
%z centered at that actual
measurement is (from the equation of a
%gaussian)
P_w(i) = (1/sqrt(2*pi*x_R)) *
exp(-(z - z_update(i))^2/(2*x_R));
end
% Normalize to form a probability
distribution (i.e. sum to 1).
Página 6 de 8
 INFORME CONCEPTOS TEORIA Periodo 2018-1
DEPARTAMENTO DE INGENIERÍA MECATRÓNICA

%store this new value to the new

P_w = P_w./sum(P_w); estimate which will go back into the
%next iteration
%{ for i = 1 : N
figure(1) x_P(i) = x_P_update(find(rand <=
clf cumsum(P_w),1));
subplot(121) end

plot(P_w,z_update,'.k','markersize',5) %The final estimate is some metric

hold on of these final resampling, such as
plot(0,z,'.r','markersize',50) %the mean value or variance
xlabel('weight magnitude') x_est = mean(x_P);
ylabel('observed values (z update)')
subplot(122)
%{
plot(P_w,x_P_update,'.k','markersize',5) %the after
hold on subplot(133)
plot(0,x,'.r','markersize',50)
xlabel('weight magnitude') plot(0,x_P_update,'.k','markersize',5)
ylabel('updated particle positions hold on
(x P update)') plot(0,x_P,'.r','markersize',5)
pause plot(0,x_est,'.g','markersize',40)
xlabel('fixed time point')
title('weight based resampling')
pause
%plot the before and after %}
figure(1) % Save data in arrays for later
clf plotting
subplot(131) x_out = [x_out x];
z_out = [z_out z];
plot(0,x_P_update,'.k','markersize',5) x_est_out = [x_est_out x_est];
title('raw estimates')
xlabel('fixed time point') end
ylabel('estimated particles for
flight position')
subplot(132) t = 0:T;
figure(1);
plot(P_w,x_P_update,'.k','markersize',5) clf
hold on plot(t, x_out, '.-b', t, x_est_out, '-
plot(0,x,'.r','markersize',40) .r','linewidth',3);
xlabel('weight magnitude') set(gca,'FontSize',12);
title('weighted estimates') set(gcf,'Color','White');
%} xlabel('time step'); ylabel('Quail
%% Resampling: From this new flight position');
distribution, now we randomly sample legend('True flight position', 'Particle
from it to generate our new estimate filter estimate');
particles

%what this code specifically does is

randomly, uniformally, sample from
%the cummulative distribution of the
probability distribution
%generated by the weighted vector
P_w. If you sample randomly over
%this distribution, you will select
values based upon there statistical
%probability, and thus, on average,
pick values with the higher weights
%(i.e. high probability of being
correct given the observation z).

Página 7 de 8
 INFORME CONCEPTOS TEORIA Periodo 2018-1
DEPARTAMENTO DE INGENIERÍA MECATRÓNICA

I. REFERENCIAS
[1] J. G. Díaz, A. M. Mejía y F. Arteaga, Aplicación de los
filtro de Kalman a sistemas de Control, 2001.

[2] Haykin S, Cognition is the Key to the Next Generation of

Radar Systems, 2009

[3] Qi-tu Zhang, Haykin S, Tracking characteristics of the

Kalman filter in a nonstationary environment for adaptive filter
applications, 1983.

[4] D. J. Jwo and S. C. Chang, Application of Optimization

Technique for GPS Navigation Kalman Filter Adaptation,
2004.

[5] P. Meller and R. Laban, Aplicación del filtro de Kalman a

la estimación de elasticidades variables en el mercado del
trabajo chileno, 1974-1985, 1987.

[6] L.D. Avendaño, L.E. Avendaño, J.M Ferrero, G.

Castellanos, Improvement of an extended Kalman filter power
line interference suppressor for ECG signal, 2007.

[7] P. M. Van Dooren, Numerical Linear Algebra for Signals

Systems and Control, 2003.

Página 8 de 8