Improved swarm support in Loco Positioning with TDoA 2.0

We have had an implementation of a Time Difference of Arrival algorithm (TDoA) in the Crazyflie 2.o and the Loco Positioning System for quite a long time. The coolest feature of the TDoA algorithm is that it can be used to position virtually unlimited number of Crazyflies concurrently as opposed to the standard Two Way Ranging algorithm that is limited to one (or very few Crazyflies). The original implementation is working pretty well but contains some flaws that we are not completely happy with, hence we have not released it officially and are still calling it experimental. Since support for tracking multiple objects is a requirement for flying swarms and we like swarms, we have started to iron out the problems. 

A small swarm using the old TDoA algorithm, from February 2017.
 

In the current implementation each anchor transmits the time of the transmission and the times of the latest reception of transmissions from all other anchors about every 16 ms. From this information it is possible to calculate the difference in time of flight for the radio waves from two anchors to the Crazyflie. When we know the difference in time of arrival, by multiplying with the speed of light we get the difference in distance and can calculate the position of the Crazyflie. This all sounds fine and dandy but the set up has some problems, the biggest one being error handling. If one or more packets are lost, either from anchor to Crazyflie or anchor to anchor, there is no deterministic way to detect it in some cases. The current algorithm relies on sanity checking the calculated result and discarding data that looks suspicious, which is usually easy as the distances quickly gets unrealistic (several thousands of meters!). We suspect that some erroneous values slip through the check though and we would like to be able to really understand when data is valid or not.

TDoA 2.0

What we are working on now is to add a sequence numbering scheme to enable the receiving party to understand when a packet has been lost. With this information it will be possible to discard bad data as well as use the available information better. While we are re-writing the code we are also moving a part of the algorithm from the Crazyflie to the anchors, after all there is a CPU in the anchors that is not fully utilized. The idea is to let each anchor continuously calculate the distance to all other anchors and add this information to the messages it transmits, which will reduce the work in the Crazyflie.

This is work in progress and we are not completely sure where we will end up, but we are aiming at making the TDoA mode part of the official release at some point.

Sensor fusion

Related to the Loco Positioning system is our line of other positioning sensors; the Z-ranger deck and the Flow deck. The Flow deck has really good precision at low altitudes but can not provide absolute positioning while the Loco Positioning system does not have the same precision but absolute position capabilities. So what if we fuse the information from the Flow deck with the Loco Positioning system? We have tried it out and it works pretty well, we can get the best of two worlds! The Z-ranger can also be used in the same way to improve the Z component of the estimated position when flying bellow ~1m.

Even though it works using multiple positioning sensors at the same time, there is room for improvements and some tweaking will be required to make it rock solid.

 

 

Leave a Reply

Your email address will not be published. Required fields are marked *