EKF over-correcting?

Question

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.

Answer 1

I think you have a number of errors:

Your R matrix is the covariance of the measurement. You are using a diagonal matrix which is fine. The elements along the diagonal should be the variance of each input variable. It looks like you are passing a deviation instead. You would need to square those values to turn them into variances.

Your Jacobians look wrong. The matrix terms should be the derivatives of the measurement variables with respect to the state variables. Your measurement variables are range and bearing, not x and y. You should end up with some trig functions in your Jacobian matrix due to the rotation of the robot. The bearings are measured relative to the heading of the robot so you should have a rotation in the measurement function to cancel the rotation of the robot. The Jacobian for the landmark variables is also more complex as q is not static with changes in self.X.

Edit:

I looked some more at your Jacobians. I actually think they are pretty close. The J[1][2] term should just be -1.0 I think which it sounds like you have already fixed. I think you might have a sign error in JH[0][0] and JH[0][1]. I was coming up with the negative of the expression you have. I find calculating the derivatives for the Jacobians to be error prone as I have trouble seeing my own errors for any mistakes I make. I always try to test my Jacobians with a small test function to make sure I did it right. This is very easy to do by comparing the analytical Jacobian versus numerical derivatives. You can trivially calculate the numerical derivatives via (f(x+dx) - f(x))/dx for an appropriate delta dx doing each input variable separately. Appropriate here means small enough to be a local value and large enough to not run into numerical error.