martes, 1 de marzo de 2011

Filtro de Kalman.

Por fin, he implementado el filtro de Kalman al programa Cerea. Y esto, ¿Para qué sirve?.

El filtro, en general, sirve para fusionar datos de diferentes sensores, por ejemplo, de dos gps a la vez, de un gps y una brújula electrónica, etc.

En la aplicación particular del Cerea, fusiono los datos del gps con un modelo matemático cinemático de un vehículo, con la hipótesis de restricción no holónoma ( esto es, sin deslizamiento lateral, sabemos que en un tractor, trabajando en el barro, la no holonomía puede estar en entredicho).

En la fusión y dadas las características del algoritmo, doy poco peso a las estimaciones de posición que me da el modelo y doy mucho peso a las estimaciones del ángulo del vehículo.

Un error común es suponer que el gps mide el yaw o cabeceo del vehículo. Es incierto, el gps mide el ángulo del vector velocidad. Mediante el filtro de Kalman, un modelo de triciclo de ackerman y usando como variables de control el módulo de la velocidad y el ángulo de este vector se puede cálcular el ángulo de yaw del vehículo.

Beneficios:
  • En parado, no hay bailoteo del vehículo ( el gps en parado da ángulos falsos). El ángulo es filtrado y el vehículo permace parado.
  • Reproduce la cinemática de marcha atrás, esto no me lo esperaba, pero cuando das marcha atrás el modelo se mueve como el vehículo. (* Parece que si, pero no lo reproduce, tengo que imponerle la restricción de máximo ángulo de giro de las ruedas)
  • Me permite aumentar el frame rate del programa, es decir, puedo tener 1 dato de gps por segundo y estimar los estados intermedios, subiendo a 20Hz, por ejemplo
  • Sirve para integrar nuevos sensores, por ejemplo, una unidad de guiado inercial.

El programa:

def kf_predict(X, P, A, Q, U):
dt =1.0
v = float(U[0])
titagps = float(U[1])
alfa= titagps
tita =float(X[2])
ganma= alfa-tita
b = 3.0
xpunto = v *cos(alfa)
ypunto = v* sin(alfa)
titapunto =v/b*sin(ganma)
BU = array([[dt*xpunto],[dt*ypunto],[dt*titapunto]])
#X = dot(A, X) + dot(B, U)
X = dot(A,X) + BU

P = dot(A, dot(P, A.T)) + Q
return(X,P)

def kf_update(X, P, Y, H, R):
IM = dot(H, X)
IS = R + dot(H, dot(P, H.T))
K = dot(P, dot(H.T, inv(IS)))
X = X + dot(K, (Y-IM))
P = P - dot(K, dot(IS, K.T))
#LH = gauss_pdf(Y, IM, IS)
return (X,P,K,IM,IS)


for line in f_in:
campo = line.split(",")
x = float(campo[2])
y = float(campo[3])

aux = float(campo[5].split(";")[0])
titav = 450 -aux
titav = titav-floor(titav/360)*360

if i ==0:
x_ant = x
y_ant= y
i+=1
x=x-x_ant
y=y-y_ant
v= float(campo[4])*0.514

if i == 1:
# Initialization of state matrices
X = array([[x], [y],[radians(titav)]])
P = diag((0.01, 0.01, 0.01))
A = array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
Q =10* eye(X.shape[0])
U=array([[v],[radians(titav)]])
Z=array([[x],[y]])
H = array([[1, 0, 0], [0, 1,0]])

R=array([[.25],[.25]])
(X, P) = kf_predict(X, P, A, Q, U)
(X, P, K, IM, IS) = kf_update(X, P, Z, H, R)
i+=1










Sobre los resultados, en el gráfico del ángulo del vehículo se aprecia, en la parte final , en rojo una zona ruidosa, el vehículo pintado por el programa oscila rotando en parado. Al aplicar el filtro permance quieto. Se ve en azul que el ángulo salta entre 0 y 360º. ( estos saltos son artificiales, porque al pasar los radianes a grados y meterlos dentre de una única vuelta de circunferencia ocurre esto).