Problem 673854 · hard · Robotics · Level 06 Estimation and Autonomy

The Museum Guide Finds Its Way Between the Pillars

extended Kalman filter · EKF localization · data association · Mahalanobis gate · angle wrapping · numpy

A museum guide robot localises itself among round pillars whose positions are known: landmarks[j] = (lx, ly) (metres, x to the right, y up). Its belief is a mean mu = (x, y, theta) (heading in radians, counter-clockwise from +x) and a 3 by 3 covariance P. Each entry of steps is a tuple (v, w, readings): a commanded speed v and turn rate w held for dt seconds, then a list of readings (r, b), each a range and a bearing (from the heading, counter-clockwise positive, in (-\pi, \pi]) to some pillar; the sensor does not say which one, and some readings are reflections that match none.

For each step:

  1. Predict. Euler step along the old heading: x' = x + v\,dt\cos\theta, y' = y + v\,dt\sin\theta, \theta' = \theta + w\,dt; covariance P' = G P G^T + Q, with G the Jacobian of this step with respect to (x, y, \theta) at the old mean.
  2. Each reading, in the given order, using the mean and covariance as they are at that moment (after the previous readings of the step):
    • for every pillar j: the expected range \hat{r} = \sqrt{\Delta x^2 + \Delta y^2} and bearing \hat{b} = \operatorname{atan2}(\Delta y, \Delta x) - \theta with \Delta x = l_x - x, \Delta y = l_y - y; the innovation \nu = (r - \hat{r},\ b - \hat{b}) with the bearing part wrapped to (-\pi, \pi]; the 2 by 3 Jacobian H of (\hat{r}, \hat{b}) with respect to (x, y, \theta); S = H P H^T + R; and the squared Mahalanobis distance \nu^T S^{-1} \nu;
    • the reading belongs to the pillar with the smallest such distance among those at most gate (ties: the lowest index). If there is none, skip the reading;
    • otherwise apply the EKF update with that pillar: K = P H^T S^{-1}, \mu \leftarrow \mu + K\nu, P \leftarrow (I - K H)P.
  3. Record the mean.

Keep every heading wrapped to (-\pi, \pi]. Write ekf_localize(mu, P, landmarks, steps, dt, Q, R, gate) that returns a tuple (means, matches): the recorded means, one list [x, y, theta] per step, and for each step the list of matched pillar indices, one per reading (-1 for a skipped reading). numpy is available.

The setup provides museum_run(T, seed), which returns the arguments of a simulated T-step tour among six pillars (with a reflection now and then), so ekf_localize(*museum_run(40, 7)) runs your filter on it. Press Run with plot(xs, ys) of your means to see the tour.

Examples

Input:  mu = (0, 0, 0), P = [[0.01, 0, 0], [0, 0.01, 0], [0, 0, 0.01]], landmarks = [(5, 0), (0, 5), (5, 5)],
        steps = [(1.0, 0.0, [(3.955, 0.025), (6.325, 0.922)]),
                 (1.0, 0.5, [(5.41, 1.443), (2.0, -2.5)]),
                 (1.0, 0.5, [(2.151, -1.293), (4.898, 0.127)])],
        dt = 1.0, Q = [[0.002, 0, 0], [0, 0.002, 0], [0, 0, 0.001]], R = [[0.01, 0], [0, 0.0025]], gate = 9.21
Output: ([[1.039637388581427, 0.013459062760948642, -0.020270454841982755],
          [2.035219843542808, -0.008717509477236065, 0.4947812423815255],
          [2.928953332283443, 0.5325428297976338, 1.01808779168252]],
         [[0, 2], [1, -1], [0, 2]])
Explanation: after the first prediction the robot expects pillar 0 at range 4, bearing 0; the reading
(3.955, 0.025) is 0.12 from it in squared Mahalanobis distance and over 200 from the others.
The reading (2.0, -2.5) in the second step fits no pillar and is skipped.

Input:  mu = (0, 0, 3.1), P = [[0.02, 0, 0], [0, 0.02, 0], [0, 0, 0.02]], landmarks = [(-4, 0.2), (3, 0)],
        steps = [(0.5, 0.0, [(3.52, 3.14), (3.47, 0.05)])], dt = 1.0, the same Q and R, gate = 9.21
Output: ([[-0.5191334941702317, 0.12444447122557009, 3.086925641508269]], [[1, 0]])
Explanation: pillar 1 is behind the robot: expected bearing -3.106, measured 3.14. The
difference wraps to -0.037, a close match.

Constraints

  • P, Q and R are symmetric covariances (R positive definite); dt > 0; gate > 0
  • no pillar is ever within 0.1 m of the robot's mean position
  • answers are compared with a tolerance of 1e-6; numpy arrays or tuples are accepted in place of lists

Goals

  • Run an EKF localizer end to end: predict with the odometry, then update with every reading
  • Decide which pillar each unlabeled reading belongs to by Mahalanobis distance in measurement space
  • Ignore readings that fit no pillar, and keep the updates in order within a step
Starting Python…