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


4 comentarios:

  1. Buen trabajo!, has notado mejora de precisión?

    ResponderEliminar
  2. Aún tengo que probar, aunque no estimo que me mejore precisión en posición, si en el yaw.
    Saludos

    ResponderEliminar
  3. que trabajo más bueno estás haciendo, la posibilidad de poder incluir otros sensores o antenas adicionales es tremenda. plugins plugins plugins jejeje

    ResponderEliminar
  4. Ola cesar, me gusta ter mais detalhes do filtro de Kalman que você implementou.

    ResponderEliminar