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

How the Doubt Grows While the Rover Drives

extended Kalman filter · Jacobian · covariance propagation · unicycle model · numpy

An extended Kalman filter (EKF) describes a robot's belief about its pose by 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 matrix P (variances on the diagonal, how errors go together off it).

A field rover moves like a unicycle: it is commanded a forward speed v (m/s) and a turn rate w (rad/s) for dt seconds. One step of the motion model is a single Euler step, with the position moved along the old heading first and the heading updated after:

x' = x + v\,dt\cos\theta, \qquad y' = y + v\,dt\sin\theta, \qquad \theta' = \theta + w\,dt.

The EKF's prediction moves the mean through this model and the covariance through its linearisation: with G the 3 by 3 Jacobian of (x', y', \theta') with respect to (x, y, \theta), evaluated at the mean before the step,

P' = G\,P\,G^T + Q,

where Q is the 3 by 3 covariance the motion adds in every step.

Write ekf_predict(mu, P, controls, dt, Q) that applies one prediction step for each (v, w) in controls, in order, and returns a tuple (mu, P): the final mean as a list [x, y, theta] with theta wrapped to (-\pi, \pi], and the final covariance as a list of 3 lists. numpy is available (import numpy as np; @ is the matrix product, .T the transpose, .tolist() turns an array into lists).

Examples

Input:  mu = (0, 0, 0), P = [[0.01, 0, 0], [0, 0.01, 0], [0, 0, 0.01]], controls = [(1.0, 0.0)],
        dt = 1.0, Q = [[0.001, 0, 0], [0, 0.001, 0], [0, 0, 0.002]]
Output: ([1.0, 0.0, 0.0], [[0.011, 0.0, 0.0], [0.0, 0.021, 0.01], [0.0, 0.01, 0.012]])
Explanation: driving 1 m along +x, a heading error of e radians becomes a sideways error of
about e metres, so the y variance gains the heading variance 0.01, and y and theta become correlated.

Input:  mu = (2, 1, math.pi / 2), P = [[0.04, 0, 0], [0, 0.04, 0], [0, 0, 0.1]],
        controls = [(0.5, 0.2), (0.5, 0.2)], dt = 0.5, Q = all zeros
Output: ([1.975041645838293, 1.4987510413195064, 1.7707963267948967],
         [[0.0648752601217292, 0.001244800512777242, -0.04987510413195065],
          [0.0012448005127772418, 0.04006229194424612, -0.002495835416170703],
          [-0.04987510413195065, -0.002495835416170703, 0.1]])
Explanation: the second step's Jacobian uses the heading pi/2 + 0.1 reached after the first step.

Constraints

  • P and Q are symmetric with non-negative diagonals; dt > 0; the start heading is in (-\pi, \pi]
  • answers are compared with a tolerance of 1e-6; numpy arrays or tuples are accepted in place of lists

Goals

  • Linearise a unicycle motion step with its Jacobian, evaluated at the current estimate
  • Propagate a 3 by 3 pose covariance with $G P G^T + Q$ in numpy
  • See how heading doubt turns into sideways position doubt
Starting Python…