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'?



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.


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.

  • $\begingroup$ Thanks for the reply. With regards to the measurement covariance, are you saying I should be squaring my pre-defined error values, the input, or the product? I should've posted my sensor model, which acts on the cartesian coordinates. pastebin.com/qQwGU1Vp I assume the jacobian 'JH' is the correct derivative of that. Though I could be wrong, of course. And you're correct, the angle is offset by the current angle of the robot. Thanks again. $\endgroup$ – jub Apr 11 '15 at 19:15
  • $\begingroup$ Update: Thanks @DrRoboto squaring the deviation value appears to have helped greatly. The landmarks are no longer being flung about. I also changed part of my measurement jacobian to -1.0 instead of -1.0/q. $\endgroup$ – jub Apr 11 '15 at 21:16

Your Answer

By clicking "Post Your Answer", you acknowledge that you have read our updated terms of service, privacy policy and cookie policy, and that your continued use of the website is subject to these policies.

Not the answer you're looking for? Browse other questions tagged or ask your own question.