Add outlier detection and removal to the TDoA code

[krichardsson]

With the sequence numbering that has been added to TDoA 2 it is possible to detect and remove most of the corrupt measurements. There are still some outliers, probably due to reflections or interference. The goal is to reduce the risk of faulty position estimations that make the Crazyflie take off in a random direction.

When tested in our lab it seems as the outliers comes in 3 flavours:

1. about 50 cm - these are hard to detect and separate from normal noise and motion
2. about 3 - 5 m - should be possible to detect
3. more than 20 m - should be fairly easy to handle

The algorithm should not make assumptions of the size of the system (at least up to 50x50x50 m) or the order of messages from anchors.

It might be possible to use the estimated position to determine if a sample seems reasonable or not.

Add functionality to identify outliers and stop them from being fed into the Kalman filter.