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