LPS with drones as anchors

All discussions related to the Loco Positioning system
Post Reply
giggi
Beginner
Posts: 9
Joined: Thu Dec 03, 2020 9:56 am

LPS with drones as anchors

Post by giggi »

Hi everyone,

I'm developing a system, where a drone can also act as an anchor in a TWR-based system, when it is placed on the ground.

At the moment I use the normal anchors as anchors :-)
I'm using the loco-deck, so I disabled the locodeck-driver and wrote a new one.
It works, but the problem is, that the accuracy is quite bad.

I'm not sure if I am feeding the kalman correctly:

Code: Select all

    
    distanceMeasurement_t dist;
    dist.distance = distance;
    dist.x = x;
    dist.y = y;
    dist.z = z;
    dist.stdDev = 0.3;
    estimatorEnqueueDistance(&dist);
I'm feeding the kalman without any filtering, every distance-measurement gets fed directly.
Should I may do some outlier-filtering?

I do 30 measurements per second, but the drone can barely fly in this system.
I can program some trajectories, and it tries to fly them, but it look very wobbly and instable.

Does anybody may know, what I'm doing wrong?

Cheers,
Michael
kristoffer
Bitcraze
Posts: 630
Joined: Tue Jun 30, 2015 7:47 am

Re: LPS with drones as anchors

Post by kristoffer »

Hi!

The code looks good to me. The x/y/z should contain the position of the "anchor" and the distance should contain the measured distance to the point.
It is possible that an outlier filter would help, but be careful. A kalman filter is essentially designed to use all samples but an outlier filter can be useful to remove samples that are obviously wrong. The outlier filters we have in the firmware are mainly trying to remove the samples that are corrupt due to poor reception, reflections or such. Most often outliers are very wrong, on the order of hundreds of meters off, these should be easy to catch at least.

Outliers usually "pulls" the estimated position away pretty far, and sudden, and the result is that the Crazyflie "jumps" or flips.

Some related information. There is a master thesis that might be interesting to you https://findit.dtu.dk/en/catalog/2439894045

There is also work ongoing for similar functionality using TDoA (we usually call it TDoA3+ or TDoA4). There are currently some pull requests in the https://github.com/bitcraze/crazyflie-firmware repository that I have not had the time to review yet. Even though the solution is based on TDoA, the ranging mechanism between anchors is essentially TWR so one way to look at it is that any Crazyflie that is acting as an anchor is actually doing TWR, while other Crazydlies can benefit from the same data through TDoA.
giggi
Beginner
Posts: 9
Joined: Thu Dec 03, 2020 9:56 am

Re: LPS with drones as anchors

Post by giggi »

Thanks Kristoffer!

Well, then I probably haven't seen any outlier, when the "treshold" is in the order of hundreds of meters.
Sometimes I have outliers which are like one meter off.

This thesis sounds interesting!
Thanks for the link.

I'm still having some problems with the stock loco-positioning-system.
I've placed the nodes more ore less in the shape of the "getting-started guide".
I'm using a Vicon-Tracker, in order to compare the estimate against ground truth.

The crazyflie should do a square with a length of 2m.
The crazyflies's estimate is actually a square, but the ground truth is way off:
loco_rect_02.pdf
(22 KiB) Downloaded 142 times
This plot is made with the flow-deck attached.
Without the flow-deck it is nearly impossible to fly...

Is this to be expected?
I've read that the accuracy should be withing 10cm.
kimberly
Bitcraze
Posts: 1050
Joined: Fri Jul 06, 2018 11:13 am

Re: LPS with drones as anchors

Post by kimberly »

Hi!

Kristoffer is on a holiday now so I will try to answer.

The 10 cm accuracy is if everything is setup right and the antenna are all pointing directly to the crazyflie like this:
Screenshot from 2020-12-21 11-32-44.png
Screenshot from 2020-12-21 11-32-44.png (5.87 KiB) Viewed 2212 times
So in practice I wouldn't be super surprised of an accuracy that is 15-20 cm off, However, if this is with the flowdeck attached, I would agree that it should be much better. Have you tried with the flowdeck alone? It has a tendencies to under or overestimate the flow, especially when there is no or little texture on the ground.

The unflyable part should be addressed too... since that is bad by itself. You are talking about the regular TDOA 2, 3 or TWR right, not the one you are developing yourselves? You should be able to fly without the flowdeck with those. Maybe your crazyflie is unbalanced so you can check the propellers with the prop test (there is a button for that in the console tab of the cfclient). And did you measure the lps node locations by hand and updated the locations?
giggi
Beginner
Posts: 9
Joined: Thu Dec 03, 2020 9:56 am

Re: LPS with drones as anchors

Post by giggi »

Thanks Kimberly!

It actually was one prop that was causing the error!

In your TWR algorithm, are you overlapping the measurement or are the anchors sequentially ranged?
kimberly
Bitcraze
Posts: 1050
Joined: Fri Jul 06, 2018 11:13 am

Re: LPS with drones as anchors

Post by kimberly »

I think you can find this information in the LPS node documentation, probably under TWR protocol? Let us know if the explanation is unclear
Post Reply