Added code for position estimation.

Take mean of all points, slap an estimate dot there.