Problem 639337 · medium · Robotics · Level 06 Estimation and Autonomy

Which Trunk Did the Lidar See?

data association · Mahalanobis distance · validation gate · covariance · frames · numpy

An orchard robot knows its pose pose = (x, y, theta) exactly (metres, x to the right, y up; heading in radians counter-clockwise from +x). Its map lists tree trunks: landmarks[j] = (mx, my) with a 2 by 2 covariance covs[j] saying how well the map knows that trunk's position (in the world frame). Its lidar reports observations, each a trunk position (f, l) in the robot's frame: f metres forward and l metres to the robot's left. The lidar's error has covariance R in the robot's frame (forward first, left second).

To decide which trunk an observation belongs to, the robot:

  1. converts the observation to a world point p;
  2. for every landmark j, forms the difference d = p - m_j and the combined covariance S_j = \Sigma_j + R_w, where R_w = \text{Rot}(\theta)\, R\, \text{Rot}(\theta)^T is the lidar's covariance turned into the world frame and \text{Rot}(\theta) = \begin{bmatrix}\cos\theta & -\sin\theta\\ \sin\theta & \cos\theta\end{bmatrix};
  3. computes the squared Mahalanobis distance d^T S_j^{-1} d: a distance measured in standard deviations, so a miss along a direction the map is unsure about counts for little;
  4. matches the observation to the landmark with the smallest squared distance among those at most gate; if several share that smallest value, the lowest index. If no landmark is within the gate, the observation is unmatched: -1.

Each observation is matched on its own (two observations may get the same landmark). Write associate(pose, observations, landmarks, covs, R, gate) that returns the list of matched indices, one per observation. numpy is available (np.linalg.solve(S, d) or np.linalg.inv(S)).

Examples

Input:  pose = (0, 0, 0), observations = [(5, 0.5), (8, 0)], landmarks = [(5, 0), (5, 1.2)],
        covs = [[[0.01, 0], [0, 0.01]], [[0.01, 0], [0, 1.0]]], R = [[0.04, 0], [0, 0.04]], gate = 5.99
Output: [1, -1]
Explanation: (5, 0.5) is nearer trunk 0 (0.5 m against 0.7 m), but the map knows trunk 1's
y only to within about 1 m: the squared distances are 0.25 / 0.05 = 5.0 and 0.49 / 1.04 = 0.47.
(8, 0) is 3 m from both trunks, far outside the gate.

Input:  pose = (0, 0, math.pi / 2), observations = [(6, 0)], landmarks = [(0, 5), (0.5, 6)],
        covs = [[[0.01, 0], [0, 0.01]], [[0.01, 0], [0, 0.01]]], R = [[1.0, 0], [0, 0.01]], gate = 5.99
Output: [0]
Explanation: the lidar is unsure along its beam (variance 1.0 forward). The robot faces +y,
so in the world that doubt lies along y: trunk 0 is 1 m off along y (0.99), trunk 1 is 0.5 m
off along x, where the lidar is sharp (12.5).

Constraints

  • covs has one entry per landmark; every covs[j] and R is symmetric, and every S_j is invertible
  • gate > 0; there may be no landmarks or no observations

Goals

  • Match each observation to a known landmark by Mahalanobis distance, not by plain distance
  • Combine the map's uncertainty with the sensor's, rotated into the world frame
  • Leave an observation unmatched when no landmark is inside the gate
Starting Python…