Monodimensional UKF – Unscented Kalman Filter in Python

El método “Unscented Kalman Filter” es un importante método de estimación recursivo gaussiano. Este tipo de técnicas son útiles para estimar el estado de un robot integrando la información proveniente de las medidas de los sensores con el conocimiento previo que tenemos sobre el sistema (modelo teórico).

En la robótica probabilística, el estado no es conocido con precisión. Por ejemplo, la posición de un robot (x,y,z) (que podría ser descrito por una tupla de tres reales) es descrita por una función probabilística tridimensional. Esta función que describe el estado se denomina en la literatura creencia bel(x). De esta forma la posibilidad de que un robot se encuentre en un estado determinado queda definida estocásticamente. Este enfoque puede describir de una forma mas realista el conocimiento que tenemos sobre el estado. Además permite otras ventajas como mantener múltiples hipótesis sobre el estado del robot. Por ejemplo, un robot se encuentra en la mezquita de córdoba y detecta con sus sensores que se encuentra delante de una columna, sin embargo si el estado en que se encontraba anteriormente no describía una mejor hipótesis clara, esta columna podría ser cualquiera de las cientos de columnas existentes en la mezquita.

La dos gráficas ilustran el comportamiento típico de los métodos de estimación recursivos gaussianos. En concreto este  ejemplo ha sido generado a partir del método UKF. Supongamos que el estado es monodimensional (por ejemplo la posición en el eje x):

La primera figura muestra en azul la creencia en el instante anterior del estado del robot. bel[t-1](x) Se observa como se cree que su valor es aproximadamente uno. Sin embargo el valor real podría ser 1.1 (representado mediante la línea vertical roja).

Supongamos que pasa un instante de tiempo. Haciendo uso del conocimiento teórico (modelo matemático) que tenemos del sistema y dadas las entradas conocidas, predecimos que el estado en el instante actual será alrededor de 3 (función amarilla). Esta función de probabilidad se denomina creencia anterior (predicción) ^bel[t](x). Dado que nuestro modelo es teórico  es evidente que tiene un error. Este error puede ser típicamente acotado teóricamente o mediante métodos empíricos en forma de matriz de covarianza. Por lo tanto podemos estimar que la creencia anterior ^bel[t](x) tendrá una mayor desviación típica que la creencia del instante anterior bel[t-1](x). Es decir, tras la predicción, tenemos una mayor incertidumbre del estado del robot. La línea vertical roja representa la posible posición real del robot en el estado actual.

A continuación recibimos las medidas del sensor. Estas medidas nos indican el estado de forma total o parcial (de forma directa o tras algún tipo de de procesamiento). Pero también, los sensores presentan un error (típicamente indicado por el fabricante o también se podría obtener mediante métodos empíricos).  En la segunda figura, la función de probabilidad roja representa tal medida y su error. En la misma también se muestra en azul la creencia anterior ^bel[t](x) predicha por el modelo teórico.

El método UKF (al igual que KF, EKF y otros) nos permite estimar a partir de los errores del modelo teórico y de la medida, en cual de ambos debemos confiar más, obteniendo un factor de ganancia K para ponderar ambas funciones de probabilidad. Una vez obtenido este factor de ganancia K se puede calcular la creencia posterior (final) del instante actual bel[t](x). La función discontinua verde representa tal creencia.

Para una descripción matemática mas detallada del funcionamiento del método UKF:
http://www.cslu.ogi.edu/nsel/ukf/node6.html

A continuación muestro una implementaicón personal en python del método UKF acompañada en la simulación utilizada en este ejemplo:

# This code is subject under the terms of the BSD License
# 2011 - Pablo Iñigo Blasco
# http://techbase.kde.org/Policies/Licensing_Policy#BSD_License</p>
import scipy.stats
from numpy import *
from matplotlib.pyplot import *

def getSigmaPoints(mu, sigma, n=1., alpha=1., kappa=1., betta=2.):
 #space for sigma points
 X = zeros(2 * n + 1)
 wm = zeros(2 * n + 1)
 wc = zeros(2 * n + 1)
 lambd = alpha * alpha * (n + kappa) - n

 #calculate sigma points
 X[0] = mu
 X[1] = mu + (math.sqrt((n + lambd) * sigma))
 X[2] = mu - (math.sqrt((n + lambd) * sigma))

 #calculate weights
 #first weight
 wm[0] = lambd / (n + lambd)
 wc[0] = lambd / (n + lambd) + (1 - alpha * alpha + betta)

 #rest of weights
 wm[1] = wc[1] = 1 / (2. * (n + lambd))
 wm[2] = wc[2] = 1 / (2. * (n + lambd))

 return [X, wm, wc]

def process_state_transition(x, u):
 return u +  x*x

def measurement(x):
 return x;

def measurement_noise(measurement_cov):
 return scipy.stats.norm.rvs(loc=0, scale=measurement_cov, size=1)

def process_noise(process_cov):
 return scipy.stats.norm.rvs(loc=0, scale=process_cov, size=1)

def h(x):
 return x;

#UKF parameters
alpha = 1.;
kappa = 0.;
betta = 2.;
n = 1;

#model parameters
#process noise covariance matrix
R = 0.45
Q = 0.3

#suppose the control input u = 1
u = 2;

#previous state believe
mu_prev = 1;
sigma_prev = 0.1;

real_prior_state = scipy.stats.norm.rvs(loc=mu_prev, scale=sigma_prev, size=1)
print "real previous state %f"%(real_prior_state)
print "posterior belief of the previous state (mu=%f,sigma=%f)"%(mu_prev,sigma_prev)

f = scipy.stats.norm(mu_prev, sigma_prev)
#getting sigma points from previous state belief
[X_prior_state, wm, wc] = getSigmaPoints(mu_prev, sigma_prev, n, alpha, kappa, betta)

#plot this previous state belief
#http://matplotlib.sourceforge.net/users/pyplot_tutorial.html
abcises = linspace(-1, 5, 400)
pX = f.pdf(X_prior_state)
figure(1)
subplot(211)
#plot gaussian function
plot(abcises, f.pdf(abcises),"b")
#plot sigma points
plot(X_prior_state, pX, "r+")
show();

#plot real previous state
axvline(real_prior_state,color="r")

print "...process happening..."
real_current_state = process_state_transition(real_prior_state,u) +  process_noise(R);
print "real current state %f"%(real_current_state)

#applying the process_state_transition to all sigma points to later build the prior belief
X_current_state = [process_state_transition(x, u) for x in X_prior_state]
plot(X_current_state, pX, "b+")

#rebuild the gaussian function after applying the process state transition to the sigma points samples
prior_bel_mu = sum(wm*X_current_state);
prior_bel_sigma_without_noise = sum (wc * (X_current_state - prior_bel_mu)* (X_current_state - prior_bel_mu))
prior_bel_sigma = prior_bel_sigma_without_noise + R

prior_bel_f_without_noise = scipy.stats.norm(prior_bel_mu, prior_bel_sigma_without_noise)
prior_bel_f = scipy.stats.norm(prior_bel_mu, prior_bel_sigma)
print "prior belief (mu=%f,sigma=%f)"%(prior_bel_mu,prior_bel_sigma)

#plot the prior belief after the process state transition without noise
#plot(abcises, prior_bel_f_without_noise.pdf(abcises),"g")
#plot the prior belief (with noise)
plot(abcises, prior_bel_f.pdf(abcises),"y")
#plot real current state
axvline(real_current_state,color="r")
subplot(212)
#also plot it in the bottom frame
axvline(real_current_state,color="r")

#so now our best guess is the prior belief. Then we get its sigma points
[X_prior_bel, wm, wc] = getSigmaPoints(prior_bel_mu, prior_bel_sigma, n, alpha, kappa, betta)

#so given the sigma points hypotesys, we get the expected measurements given our measurement model and noise
#in some way this is a substate of the prior_belief
Z_expected = [h(x) for x in X_prior_bel]
z_mu_expected = sum(wm*Z_expected);
z_sigma_expected_without_noise = sum (wc * (Z_expected - z_mu_expected)* (Z_expected - z_mu_expected))
z_sigma_expected = z_sigma_expected_without_noise + Q;
print "expected measurement (mu=%f,sigma=%f)"%(z_mu_expected,z_sigma_expected)

#plot the expected measurement given our best state guess (prior_belief)
#observe how the quality in this expectation depends on the prior belief convariance which
#at the same time depends on process covariance R
#In any case the expected measurement can be "more precise" or "less precise" that the prior belief
plot(abcises, scipy.stats.norm(z_mu_expected,z_sigma_expected).pdf(abcises),"b")

# calculate the gain, who we trust: process or measurement
K = sum(wc*(X_prior_bel - prior_bel_mu)*(Z_expected - z_mu_expected))/z_sigma_expected
print "Gain: %f"%K

# if K gets 1 trust the measurement
# if K gets 0 trust the process state transition model

#simulate observation
z = measurement(real_current_state) + measurement_noise(Q);
print "measurement %f"%(z)
plot(abcises, scipy.stats.norm(z,Q).pdf(abcises),"r")

#calculate the posterior gain
post_bel_mu = prior_bel_mu + K * h(z - z_mu_expected)
post_bel_sigma = prior_bel_sigma - K * z_sigma_expected *K

print "posterior belef (mu=%f,sigma=%f)"%(post_bel_mu,post_bel_sigma)
plot(abcises, scipy.stats.norm(post_bel_mu,post_bel_sigma).pdf(abcises),"g--")

Responder

Introduce tus datos o haz clic en un icono para iniciar sesión:

Logo de WordPress.com

Estás comentando usando tu cuenta de WordPress.com. Cerrar sesión / Cambiar )

Imagen de Twitter

Estás comentando usando tu cuenta de Twitter. Cerrar sesión / Cambiar )

Foto de Facebook

Estás comentando usando tu cuenta de Facebook. Cerrar sesión / Cambiar )

Google+ photo

Estás comentando usando tu cuenta de Google+. Cerrar sesión / Cambiar )

Conectando a %s

A %d blogueros les gusta esto: