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

A Range and a Bearing to the Lighthouse

extended Kalman filter · measurement update · range-bearing sensor · Kalman gain · angle wrapping · numpy

A survey boat runs an extended Kalman filter (EKF) for its pose: a mean mu = (x, y, theta) (metres, x to the right, y up; heading in radians counter-clockwise from +x) and a 3 by 3 covariance P. Its sensor sees a lighthouse at the known position landmark = (lx, ly) and reports z = (r, b): the range r (metres) and the bearing b, the angle from the boat's heading to the lighthouse, counter-clockwise positive, in (-\pi, \pi]. The sensor's noise covariance is the 2 by 2 matrix R.

From the mean, the boat expects to see

\hat{r} = \sqrt{\Delta x^2 + \Delta y^2}, \qquad \hat{b} = \operatorname{atan2}(\Delta y, \Delta x) - \theta, \qquad \Delta x = l_x - x,\ \Delta y = l_y - y .

The EKF update is:

  1. the innovation \nu = (r - \hat{r},\ b - \hat{b}), with the bearing difference wrapped to (-\pi, \pi] (two bearings either side of \pm\pi are close, not 2\pi apart);
  2. H, the 2 by 3 Jacobian of (\hat{r}, \hat{b}) with respect to (x, y, \theta), at the mean;
  3. S = H P H^T + R and the gain K = P H^T S^{-1};
  4. the new mean \mu + K\nu (its heading wrapped to (-\pi, \pi]) and the new covariance (I - K H)\,P.

Write ekf_update(mu, P, z, landmark, R) that returns a tuple (mu, P): the new mean as a list [x, y, theta] and the new covariance as a list of 3 lists. numpy is available (np.linalg.inv, @, .T, np.eye(3)).

Examples

Input:  mu = (1, 1, 0), P = [[0.1, 0, 0], [0, 0.1, 0], [0, 0, 0.05]], z = (4.2, 0.05),
        landmark = (5, 1), R = [[0.01, 0], [0, 0.0025]]
Output: ([0.818181818181818, 0.9787234042553191, -0.042553191489361986],
         [[0.009090909090909084, 0.0, 0.0], [0.0, 0.08936170212765958, -0.021276595744680854],
          [0.0, -0.021276595744680854, 0.007446808510638298]])
Explanation: the lighthouse is 0.2 m further than expected, so the boat moves back;
it appears 0.05 rad to the left, so the boat turns a little right and slides a little down.

Input:  mu = (0, 0, 0), the same P, z = (4.1, -3.13), landmark = (-4, 0.1), the same R
Output: ([0.09013387477989188, 0.013316814205740036, -0.03114032215047491],
         [[0.009141050709474175, 0.002005664742603116, 0.0005316179793200606],
          [0.002005664742603116, 0.08931749879503371, 0.021264719172802423],
          [0.0005316179793200606, 0.021264719172802423, 0.007443980755429153]])
Explanation: the expected bearing is 3.1166 and the measured one -3.13; the difference
-6.2466 wraps to 0.0366 rad, a small surprise, not a huge one.

Constraints

  • the landmark is at least 0.1 m from the mean position; P is symmetric with a non-negative diagonal; R is symmetric with a positive diagonal
  • answers are compared with a tolerance of 1e-6; numpy arrays or tuples are accepted in place of lists

Goals

  • Predict a range-bearing measurement of a known landmark from the pose estimate
  • Form the innovation with the bearing difference wrapped into $(-\pi, \pi]$
  • Compute the measurement Jacobian, the Kalman gain and the corrected mean and covariance
Starting Python…