New TDoA2 algorithm merged into Master

The Loco Positioning System (LPS) default working mode is currently Two Way Ranging (TWR), it is a location mode that has the advantage of being pretty easy to implement and gives good positioning performance for most use cases and anchor setups. This was a very good reason for us to start with it. Though, TWR only supports positioning and flying of one or maybe a couple of Crazyflies, while it is not a solution to fly a swarm.

One solution to fly a swarm is an algorithm called Time Difference of Arrival (TDoA). We have had a prototype implementation for a while but we experienced problems with outliers, most of them where due to the fact that we where loosing a lot of packets and thus using bad data.

To solve these issues, TDoA2 makes two changes:

• Each packet has a sequence number and each timestamps is associated with the sequence number of the packet it has been created from
• The distances between anchors are calculated and transmitted by the anchors

A slightly simplified explanation follows to outline why this helps (a more detailed explanation of how TDoA works is available in the wiki).

We start by assuming that all timestamps are available to the tag, this is done by transmitting them in the packets from the anchors to the copter.

The end goal is to calculate the difference of time of arrival between two packets from two different anchors. Assuming we have the transmission time of the packets in the same clock, all we need to do is to subtract the time between the two transmissions with the time between the two receptions:

0 – anchor 0, 1 – anchor 1, T – Tag (that is the LPS deck on the Crazyflie)

To do so we need to have the time it took for the packet to travel between the two anchors, this will enable us to calculate the transmit time of P2 in anchor 1, this can be done by calculating the TWR time of flight between the two anchors, this would require the tag to receive 3 packets in sequence:

So now for the part where TDoA2 helps: previously we had to have the 3 packets in sequence in order to calculate a TDoA, if any one of these where missing the measurement would fail or worse, it could give the wrong result. Since we did not have sequence numbers, it was hard to detect packet loss. Now that we have sequence numbers, we can understand when a packet is missing and discard the faulty data. We also do not have to calculate the distance between anchors in the tag anymore, it is calculated by the anchors themselves. This means that we can calculate a TDoA with only two consecutive packets which increases the probability of a successful calculation substantially.

To reduce packet loss even more, we have also added functionality to automatically reduce the transmission power of the NRF radio (the one talking to the Crazyradio dongle) when the LPS deck is detected. It has turned out that the NRF radio transmissions are interfering with UWB radio reception, and since most indoor use cases does not require full output power we figured that this was a good trade-off.

The results we have seen with the new protocol is quite impressive: TDoA is usually very sensitive to the tag being inside the convex hull, so much so that with the first TDoA protocol we had to start the Crazyflie from about 30cm up to be well within the convex hull. This is not required anymore and the position is still good enough to fly even a bit outside of the convex hull. The outliers are also greatly reduced which makes this new TDoA mode behave very close to the current TWR mode, but with the capability to locate as many Crazyflies as you want:

Added to that, we have also implemented anchor position handling in the TDoA2 protocol and this means that it is now as easy to setup a system with TDoA2 as with TWR:

We are now working on finishing the last functionality, like switching between algorithms (TWR and TDoA) and on writing a “getting started guide”. When that is done TDoA will become an official mode for the LPS.

In the mean time, if you are adventurous, you can try it yourself. It has been pushed in the master branch of the Crazyflie firmware and the LPS node firmware. You should re-flash the Crazyflie firmware, both STM32 and nRF51, from master and the anchors from master too.