A lidar reports each point as a range r (metres) and a bearing bearing (radians, measured from the robot's heading, counter-clockwise). The robot's pose (x0, y0, phi) is known exactly (position in metres, heading phi in radians from +x), so the point in the world frame is
where \beta is the bearing. Neither reading is perfect: the range has a random error with standard deviation sigma_r and the bearing one with standard deviation sigma_b (radians), independent of each other and with mean 0.
To first order (for small errors), an error \delta r in the range and \delta\beta in the bearing move the point by
Write point_spread(pose, r, bearing, sigma_r, sigma_b) that returns the tuple (sigma_x, sigma_y, cov_xy): the standard deviations of \delta x and \delta y, and their covariance (the mean of \delta x \cdot \delta y), all to first order.
Examples
Input: pose = (0, 0, 0), r = 10, bearing = 0, sigma_r = 0.05, sigma_b = 0.01
Output: (0.05, 0.1, 0.0)
Explanation: straight ahead, the range error moves the point along x (0.05 m) and the
bearing error along y: 10 m times 0.01 rad is 0.1 m.
Input: pose = (2, 3, 0.5), r = 4, bearing = 0.3, sigma_r = 0.02, sigma_b = 0.01
Output: (0.03189858481783751, 0.031344541576153684, -0.0005997441618249031)
Input: pose = (0, 0, math.pi / 2), r = 20, bearing = -math.pi / 4, sigma_r = 0.1, sigma_b = 0.002
Output: (0.07615773105863909, 0.07615773105863909, 0.004200000000000001)
Explanation: the beam points at 45 degrees; along the beam the spread is 0.1 m, across it
20 * 0.002 = 0.04 m, so x and y spread equally and are positively correlated.
Constraints
- answers are compared with a tolerance of
1e-6; do not round
Goals
- Propagate the spreads of a range and a bearing to the spread of a point, to first order
- Combine independent errors: variances add, each scaled by the square of its coefficient
- See why the bearing's error grows with distance while the range's does not