search

bitcraze / lps-node-firmware

GNU Lesser General Public License v3.0
83 stars 79 forks source link

Remove outliers in distances #52

Open guglie opened 4 years ago

guglie commented 4 years ago

https://github.com/bitcraze/lps-node-firmware/blob/6f5eb462272e828dd4e0f1576fa9612cc348710e/src/uwb_tdoa_anchor3.c#L475-L479

Hi @krichardsson, I've seen this TODO, how can I contribute to remove those outliers?

krichardsson commented 4 years ago

Hi, I'm happy you are interested in contributing to the project!

At this point in the code the anchor has received a packet from another anchor and we have calculated the distance to the other anchor. The idea would be to check if the distance is resonable or not, and to discard samples that seems to be "wrong". As you can see in the code there is currently only a very basic check that verifies that the distance is larger than MIN_TOF (essentially > 0).

First some information: The distance is really not a distance, it is actually the Time Of Flight for the radio waves between the two anchors and it is measured in ticks of the UWB radio clock (see definition of MIN_TOF). The clock is running at 499.2 * 128 = 63897.6 MHz and using the speed of light (299792458 m/s) we can calculate that the radio waves travel around 5 mm for one tick. The distance measurement also contains the ANTENNA_DELAY which is the time it takes for a radio packet to propagate through the antenna before it is transmitted, so this is a constant offset.

As the distance value is passed on to the Crazyflie in the UWB packets, the easiest way to examine it is probably to log it from the crazyflie. It is available in the tdoaEngine.tof log variable. To use it you first have to set the anchor pair you are interested in through the parameters tdoaEngine.logId and tdoaEngine.logOthrId. Note that the logging system samples the value at regular intervals and you will not get all distance values that the anchor sees.

Now to the actual outlier problem. I'm not sure what the best strategy would be, but know some things

  1. anchors are not supposed to move around, so the distance should be fairly static.
  2. New anchors may appear and old may disappear, new anchors will initially have distance 0.
  3. People or objects may move in the space, changing the measure distance. There are different views on if this should be filtered out or not.
  4. The value is noisy and different systems may have different noise levels.
  5. The anchors have very limited RAM and CPU power

One simple approach would be to low pass filter the distance. Another is to remove distances that are "too far away" from the current value. This type of solution usually requires a mechanism that "unlocks" the outlier filter to avoid getting stuck in the wrong value and discarding correct values.

Since it is a bit cumbersome to work on the anchor firmware (flashing all anchors take some time) you can probably make some experiments in the Crazyflie instead. A good spot would be here https://github.com/bitcraze/crazyflie-firmware/blob/master/src/deck/drivers/src/lpsTdoa3Tag.c#L180-L194 or here https://github.com/bitcraze/crazyflie-firmware/blob/master/src/utils/src/tdoa/tdoaEngine.c#L224-L235.

Good luck!

guglie commented 4 years ago

Thanks for all the details, I was thinking exactly about a low pass filter. By the way the anchors have their positions, so the distance between positions can also be used at least as a check, right?

krichardsson commented 4 years ago

Yes, the positions are stored in the anchors, but they currently only know about their own position. The remote anchor position is included in all packets but it is currently not decoded. From a protocol point of view the position is also not guaranteed to be included, so it would be nice to have a solution that does not rely on that.