I am using this (sort of) Extended Kalman filter to estimate the position of a robot moving in a 2D space, equipped with GPS, magnetometer and wheel odometry.
class EKF(object):
'''
State: [x, y, vx, vy, h]
'''
def __init__(self, x, y, vx, vy, h):
self.wheel_distance = 0.34
# State variables
self.X = np.array([x, y, vx, vy, h])
# Initial covariance
self.P = np.eye(5)
# Process noise
self.Q = np.eye(5)*0.1
# Measurement noise
self.R_gps = np.eye(2) * 5
self.R_mag = np.eye(1) * 0.1
self.R_enc = np.eye(2) * 0.1
# Measurement matrices
self.H_gps = np.zeros((2, 5))
self.H_gps[0, 0] = 1.0
self.H_gps[1, 1] = 1.0
self.H_mag = np.zeros((1, 5))
self.H_mag[0, 4] = 1.0
self.H_enc = np.zeros((2, 5))
def update(self, vl, vr, gx, gy, mx, my, dt):
# Prediction
A = np.eye(5)
# x = x0 + vx
A[0, 2] = dt
# x = xy + vy
A[1, 3] = dt
self.X = np.dot(A, self.X)
self.P = np.dot(np.dot(A, self.P), A.T) + self.Q
# Magnetometer update
K_mag = np.dot(np.dot(self.P, self.H_mag.T),
np.linalg.inv(np.dot(np.dot(self.H_mag, self.P),
self.H_mag.T) + self.R_mag))
tmp = np.arctan2(mx, my) # measured h
# convert to my reference system
if mx >= 0 and my >= 0:
tmp = 2*np.pi - tmp
elif mx >= 0 and my < 0:
tmp = -tmp
elif mx < 0 and my < 0:
tmp = -tmp
elif mx < 0 and my >= 0:
tmp = 2*np.pi - tmp
tmp += np.pi/2
if tmp < 0:
tmp += 2*np.pi
elif tmp > 2*np.pi:
tmp -= 2*np.pi
Z_mag = np.array([tmp])
innovation_mag = Z_mag - np.dot(self.H_mag, self.X)
self.X = self.X + np.dot(K_mag, innovation_mag)
self.P = np.dot((np.eye(5) - np.dot(K_mag, self.H_mag)), self.P)
# Encoder update
dleft = vl*dt
dright = vr*dt
dcenter = (dleft + dright)/2
w = (dleft - dright) / (2*self.wheel_distance)
vcenter = (vl + vr)/2
theta = self.X[4] + w # TODO: w/2??
x = self.X[0] + dcenter*np.sin(theta)
y = self.X[1] + dcenter*np.cos(theta)
vx = vcenter*np.sin(theta)
vy = vcenter*np.cos(theta)
sin = np.sin(theta)
cos = np.cos(theta)
self.H_enc[0, 2] = 2/sin
self.H_enc[0, 3] = -2/cos
self.H_enc[1, 2] = 2/sin
self.H_enc[1, 3] = 2/cos
K_enc = np.dot(np.dot(self.P, self.H_enc.T),
np.linalg.inv(np.dot(np.dot(self.H_enc, self.P),
self.H_enc.T) + self.R_enc))
Z_enc = np.array([vl, vr])
innovation_enc = Z_enc - np.dot(self.H_enc, self.X)
self.X = self.X + np.dot(K_enc, innovation_enc)
self.P = np.dot((np.eye(5) - np.dot(K_enc, self.H_enc)), self.P)
# GPS update
# the GPS measurements have a lower sampling frequency
if gx is not None:
K_gps = np.dot(np.dot(self.P, self.H_gps.T),
np.linalg.inv(np.dot(np.dot(self.H_gps, self.P),
self.H_gps.T) + self.R_gps))
Z_gps = np.array([gx, gy])
innovation_gps = Z_gps - np.dot(self.H_gps, self.X)
self.X = self.X + np.dot(K_gps, innovation_gps)
self.P = np.dot((np.eye(5) - np.dot(K_gps, self.H_gps)), self.P)
# Wrap angle
if self.X[4] < 0:
self.X[4] += 2*np.pi
if self.X[4] > 2*np.pi:
self.X[4] -= 2*np.pi
return self.X
In my reference system, the north is at 0° and the increase is clockwise. The magnetometer has the X and Y axes switched. For these reasons I'm applying the conversions included in the code. This has already been tested.
The filter should perform an interpolation of the GPS samples and improve the GPS uncertainty with proper tuning of the measurement noise matrices. However, as showed in the zoomed plots, the heading computation is not correct.
Is there some error in my filter definition? How can I fix it?
