I've been implementing an extended kalman filter, following Thrun's Probabilistic Robotics implementation. I believe my correct step may be wrong, as the state appears to be corrected far too much.
Here's a screen capture showing the issue https://youtu.be/gkSpFK27yvg
Note, the bottom status reading is the 'corrected' pose coordinates.
This is my correct step:
def correct(self, reobservedLandmarks):
for landmark in reobservedLandmarks:
storedLandmark = self.getLandmark(landmark.id)
z = Point(landmark.dist, math.radians(landmark.angle))
h, q = self.sensorModel(storedLandmark)
inv = np.array([[z.x-h.x], [wrap_radians(z.y-h.y)]])
JH = np.zeros([2, 3 + (self.landmarkCount*2)])
JH[1][2] = -1.0/q
JH[0][0] = -((self.X[0] - storedLandmark.x) / math.sqrt(q))
JH[0][1] = -((self.X[1] - storedLandmark.y) / math.sqrt(q))
JH[1][0] = (storedLandmark.y - self.X[1]) / q
JH[1][1] = -((storedLandmark.x - self.X[0]) / q)
JH[0][3+(landmark.id*2)] = -JH[0][0]
JH[0][4+(landmark.id*2)] = -JH[0][1]
JH[1][3+(landmark.id*2)] = -JH[1][0]
JH[1][4+(landmark.id*2)] = -JH[1][1]
R = np.array([[landmark.dist*self.sensorDistError, 0],[0, self.sensorAngleError]])
Z = matmult(JH, self.P, JH.T) + R
K = matmult(self.P, JH.T, np.linalg.inv(Z))
self.X = self.X + matmult(K, inv)
self.P = matmult((np.identity(self.X.shape[0]) - matmult(K, JH)), self.P)
h = The range and bearing of state landmark.
q = (landmark.x - self.X[0])^2 + (landmark.y - self.X[1])^2
My sensor covariance errors are 1cm per meter, and pi/180 for the bearing. My assumption was that the correction should be relative to the size of the robot's pose error. Which is very small in this example, as it only moved forward less than 30cm.
Is the kalman gain applied correctly here, and if yes, what other factors would result in this 'over-correcting'?
Thanks.