Combining two sensors

Two cameras, two blind spots

Two cameras stand at the edge of the table. front_cam looks back along the x axis and side_cam along the y axis. A camera sees left-right and up-down sharply, but it judges distance along its line of sight badly:

	σ in x	σ in y	σ in z
front_cam	40 mm	3 mm	3 mm
side_cam	3 mm	40 mm	3 mm

Here σ is each camera's standard deviation of error on each axis (front_cam.sigma, in metres). Each camera took one picture, so front_cam.read("red") returns the same estimate every time you ask.

Weigh by trust

A plain average, $(z_1 + z_2)/2$, trusts both readings equally. On every axis one of them is 40 mm unsure, which drags the average about 2 cm off, and it misses a ball in most runs. Instead, weigh each reading by its precision, one over its variance:

$$\hat z = \frac{z_1/\sigma_1^2 + z_2/\sigma_2^2}{1/\sigma_1^2 + 1/\sigma_2^2}$$

$z_1$ and $z_2$ are the two readings of one coordinate, $\sigma_1$ and $\sigma_2$ their standard deviations, and $\hat z$ the fused estimate. A reading 13 times sharper gets $13^2 \approx 180$ times the weight, so the fused value follows the better sensor on each axis. This is inverse-variance weighting.

Uncertainty shrinks

Precisions add up:

$$\frac{1}{\hat\sigma^2} = \frac{1}{\sigma_1^2} + \frac{1}{\sigma_2^2}$$

so the fused standard deviation $\hat\sigma$ is smaller than both $\sigma_1$ and $\sigma_2$. Two equally good readings give $\hat\sigma = \sigma/\sqrt 2$. Even the 40 mm reading helps a little.

If z1, s1, z2 and s2 are numpy arrays, the same formulas work coordinate by coordinate, so one call fuses x, y and z at once.

This is exactly the Kalman filter's update step, in Noisy Sensors & Kalman Filters, step 2.

Your task

1. Write fuse(z1, s1, z2, s2), returning (z, s): the fused estimate and its standard deviation. It must work for single numbers and for arrays.
2. The starter fuses both cameras' readings of each ball, draws every estimate's 2σ ellipse on the table (blue: front, orange: side, pink: fused) and picks the ball at the fused estimate. Put the red and green balls in the cup.