Automating position finding for anchors

All discussions related to the Loco Positioning system
Post Reply
Analogy
Beginner
Posts: 19
Joined: Fri Oct 24, 2014 6:55 am

Automating position finding for anchors

Post by Analogy »

I'm looking at using the Crazyflie+Loco system for an automated drone swarm application. Part of the requirements of the application is the minimization of setup and calibration time necessary to set up the system in an arbitrary space before the system is ready to execute the sequence. I'm looking for a way for the system to use its own rangefinding capabilities to locate its own anchors in space rather than needing me to measure and enter their positions manually.

Ideally, the setup workflow looks something like this: Place 8 anchors wherever they will fit in the space. Define two of the anchors as being co-linear in either the X, Y, or Z axis. Define a third anchor as being coplanar with the first two in either the XY, XZ, or YZ plane. Define which side of that plane the remaining anchors are on. Place one crazyflie at the desired origin of the coordinate system.

My understanding of geometry is that if each of the anchors is able to determine its range to any other anchor, this is enough information to find the position of every anchor, all that remains is to push those positions back out to the anchors via the crazyflie radio.

Is there any existing code out there that does something like this, or lacking that, an easy way for me to whip up some code of my own?
kristoffer
Bitcraze
Posts: 630
Joined: Tue Jun 30, 2015 7:47 am

Re: Automating position finding for anchors

Post by kristoffer »

Hi!

This should be possible to do, but we never put enough time into a working solution. I did write some code a few years ago but I did not think it was good enough and threw it away. You need different approaches depending on which mode you are using. In TWR you get the distance from the CF to each anchor and you will have to collect data for a few positions of the CF to be able to get a solution. In TDoA you can get the distances between anchors (as you suggest) instead.

Mike Hamer has an algorithm in his paper https://ieeexplore.ieee.org/stamp/stamp ... er=8344407 that automatically locates the anchors (for TDoA). This algorithm is distributed and was running in his anchors (our nodes unfortunately do not have enough CPU umpf) but you could use it in your PC if you want as well. I think I tried it our with good results at some point.
Analogy
Beginner
Posts: 19
Joined: Fri Oct 24, 2014 6:55 am

Re: Automating position finding for anchors

Post by Analogy »

kristoffer wrote: Tue Jun 15, 2021 6:27 am In TWR you get the distance from the CF to each anchor and you will have to collect data for a few positions of the CF to be able to get a solution.
This was also one of my ideas - Make a "calibration triangle" with three places for a CF to mount with known spatial relationships to each other, place it in the position and orientation you want for the origin of your coordinate system, take TOF measurements with the CF in each of the three positions, do some algebra to figure out where the anchors are in space, push those locations back out to the anchors through the CF.
Mike Hamer has an algorithm in his paper https://ieeexplore.ieee.org/stamp/stamp ... er=8344407 that automatically locates the anchors (for TDoA).
Looks like he's using the same basic concept I thought of earlier - define the origin by the location of one of the anchors, and orientation by the relative placement of three other anchors. Now that I'm thinking of it, though, that would probably constrain the location of the anchors too much for my application (where ideally I can just throw up anchors anywhere they'll fit in whatever arbitrary space I'm in). Also, my math education only went up to Calculus 2, so for me this paper commits the problem I have with pretty much every math-heavy paper where the actual algorithm is simultaneously beyond my understanding and also so trivial to actually-math-educated people that they see no need to make a straightforward implementation available as a C or Python library. At least the calibration triangle only requires Algebra.
kristoffer
Bitcraze
Posts: 630
Joined: Tue Jun 30, 2015 7:47 am

Re: Automating position finding for anchors

Post by kristoffer »

Now that I'm thinking of it, though, that would probably constrain the location of the anchors too much for my application
When I played with this I think I used 3 positions of the CF to define the coordinate system:
The first defined the origin
The second was somewhere along the positive x-axis to defined the x-axis
The third was somewhere in the positive xy-plane, where y is positive to define the Y and Z axis.
Also, my math education only went up to Calculus 2, so for me this paper commits the problem I have with pretty much every math-heavy paper where the actual algorithm is simultaneously beyond my understanding and also so trivial to actually-math-educated people that they see no need to make a straightforward implementation available as a C or Python library.
Yeah, the language takes a while to get used to. In this case I think it should not be too hard to use equations 22 - 27 to get an estimate of the positions, they only contain distances between anchors. These positions will contain errors though since the measurements have errors and the rest of that paragraph is about how to handle that problem. If you don't need a theoretically perfect solution you could may be just try to get a bunch of measurements and average the results, that will probably be good enough to fly.
Post Reply