The Tank Gauge That Sometimes Hears the Wrong Echo

An ultrasonic gauge on top of a water tank measures the level (cm) every few seconds. Now and then it hears an echo from a pipe instead of the water surface and reports nonsense, such as 35 cm in a tank that is clearly about 120 cm full. A Kalman filter that believes such a reading is dragged far off and takes many steps to recover.

The filter can spot the liars itself. Before using a reading z it has a prediction x with variance p. The difference z - x is the innovation (what is new in the reading), and if the filter's model is right, its variance is S = p + r. A well-behaved innovation is a few times sqrt(S) at most, so the normalised innovation d = (z - x) / sqrt(S) is usually between -3 and 3. A reading with |d| > gate is rejected: the filter keeps its prediction and does not update.

Starting from x = x0, p = p0, for every reading in order (index k from 0):

p = p + q                                     predict (the level is a random walk)
S = p + r
if abs(z - x) / sqrt(S) > gate:  reject k     (x and p stay as predicted)
else:
    K = p / S
    x = x + K * (z - x)
    p = (1 - K) * p

Write gauge(readings, x0, p0, q, r, gate) that returns a pair: the list of estimates x after each reading, and the list of the indices of the rejected readings.

Examples

Input:  readings = [120.4, 119.8, 35.0, 120.9, 121.3], x0 = 120, p0 = 1, q = 0.1, r = 0.25, gate = 3
Output: ([120.32592592592593, 120.03745819397993, 120.03745819397993, 120.53272571916833, 120.91134835675332], [2])
Explanation: at index 2 the prediction is 120.04 with S = 0.49, so d = (35.0 - 120.04) / 0.698 = -122:
rejected. The estimate holds and the next readings carry on as normal.

Input:  the same readings with gate = 1000 (no gate in practice)
Output: ([120.32592592592593, 120.03745819397993, 78.64263645726055, 98.50348968378152, 109.10933616660245], [])
Explanation: the bad echo pulls the estimate down to 78.6 cm, and two good readings later it is still 11 cm low.

Constraints

• answers are compared with a tolerance of 1e-6
• no test has a reading with |d| within 1e-9 of gate

Goals

• Compute the innovation of a reading and its expected variance
• Normalise the innovation and reject readings that are too surprising to be true
• See how a single wild reading drags an ungated filter for many steps