Reading a Lift's Speed From a Rangefinder

A laser rangefinder at the top of a lift shaft measures how far the lift car is below it (m), every dt seconds, with error variance r (m²). The maintenance computer also wants the car's speed, which nothing measures. A Kalman filter can estimate both if its state is the pair x = [position, velocity].

The model says the car keeps its speed, apart from small unknown accelerations. In matrix form, with numpy arrays:

F = [[1, dt],            next position = position + dt * velocity
     [0, 1 ]]            next velocity = velocity
Q = q * [[dt**3 / 3, dt**2 / 2],
         [dt**2 / 2, dt       ]]       process noise: random accelerations of strength q
H = [1, 0]                             the rangefinder sees the position only

Start from the state x0 and its 2 by 2 covariance P0 (the variances of the two estimates on the diagonal). For each reading z:

x = F @ x                     predict
P = F @ P @ F.T + Q
S = P[0][0] + r               the innovation's variance (H P H^T + r)
K = [P[0][0] / S, P[1][0] / S]   the gain, one entry per state (P H^T / S)
x = x + K * (z - x[0])        update both states from the position innovation
P = (I - outer(K, H)) @ P     I is the 2 by 2 identity

Write track_lift(readings, dt, q, r, x0, P0) that returns a list with the estimate [position, velocity] after each reading. numpy is available (numpy.array, @, numpy.outer, numpy.eye); plain lists and loops work too.

Examples

Input:  readings = [0.52, 1.03, 1.49, 2.01, 2.48], dt = 0.5, q = 0.1, r = 0.0004,
        x0 = [0.0, 0.0], P0 = [[0.01, 0.0], [0.0, 4.0]]
Output: [[0.5197949863652791, 1.0314748496895227], [1.0301092366467088, 1.019925688396151],
         [1.4918681033991845, 0.911620510279506], [2.0076711993410354, 1.0463435361289302],
         [2.481900999781481, 0.9364269107746629]]
Explanation: the car starts at rest as far as the filter knows, but with a large speed variance (4).
The first reading, 0.52 m after half a second, makes it conclude a speed of about 1.03 m/s.

Press Run with plot([s[1] for s in track_lift(...)]) to watch the speed estimate.

Constraints

• P0 is symmetric with a non-negative diagonal; dt > 0, r > 0
• answers are compared with a tolerance of 1e-6; numpy arrays or tuples are accepted in place of lists

Goals

• Write a two-state Kalman filter with matrices: state transition, measurement row, process noise
• Estimate a quantity that is never measured (speed) from one that is (position)
• Use numpy for the matrix products and check them against the one-number filter