How to place the LPS anchors?

Discussions about all things Bitcraze
Post Reply
doctorlu
Beginner
Posts: 23
Joined: Mon Dec 28, 2015 8:41 am

How to place the LPS anchors?

Post by doctorlu »

Dear all,
I saw CF with locodeck could communicate with 6 anchors (as shown in the code). Could anyone tell me how to place these anchors? IMHO, only 3 anchors could position the CF, why do we need 6 anchors? How can we get the coordinates using 6 distances values? If we really need more than 3 anchors, how do we place them all? Thanks!
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: How to place the LPS anchors?

Post by arnaud »

Hi,

The is some documenation about the anchor placement there: https://wiki.bitcraze.io/doc:lps:earlya ... hor_set-up. Basically you need to have them spread around your flight area. I try to have 3 above and 3 bellow to increase the Z accuracy.

As for the number of anchor you need 4, not in the same plan, in order to get a 3D position. With 3 anchors there is 2 possible position for the measurements so a 4th anchor is required to find the actual position. We use 6 anchor to make the system more robust and precise by adding redundancy.

The kalman filter is using every single ranging update to refine its position estimate, so it does not really matter how many anchor there is in the system as long as the kalman filter has enough information to lock on a 3D estimate.
doctorlu
Beginner
Posts: 23
Joined: Mon Dec 28, 2015 8:41 am

Re: How to place the LPS anchors?

Post by doctorlu »

Thanks for the clarification. I understand it now.
Another question: what is the diffference between stateEstimatorHasDistanceMeasurement and stateEstimatorHasPositionMeasurement in kalman estimation? As I recall, the distance is used to calculate position. The distanceMeasurement_t and positionMeasurement_t have nearly the same stucture. Why do we need them both?
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: How to place the LPS anchors?

Post by arnaud »

This is two different kind of input to the kalman filter.

In positionMeasurement_t, the position is the measured position of the Crazyflie. This comes for example from a camera-based system (either mocap, or simple webcam, I have done a couple of experiment with webcam and opencv lately).

In distanceMeasurement_t, the position is the position of the Anchor, and there is a field for the distance to the anchor. This is used for the Loco positioning system in two way ranging more.

Another update structure implemented is the tdoaMeasurement_t, this one contains the position of two anchors and the distance difference from these two anchors. It is used for the LPS in TDoA mode.

Any of these structure are used to update the kalman filter and allows the filter to refine the Crazyflie position. In theory any combinason of measurement can be used, thought the current implementation of the kalman filter does not behave so well with mix of measurement.
doctorlu
Beginner
Posts: 23
Joined: Mon Dec 28, 2015 8:41 am

Re: How to place the LPS anchors?

Post by doctorlu »

OK. I get that. Glad to hear you are working on the mocap thing. It is amazing!
So from what you have just said, I think the distances got from LPS TWR could only be used to feed kalman estimation, am I right? If I do not want to use kalman estimation, I then have no way to obtain the position of crazyflie? Because in the following code, the distance is calculated but if ESTIMATOR_TYPE_kalman is not defined the distance is not enqued. Is there any way that I can directly calculate the position but does not resort to the existing kalman estimation.

tprop = tprop_ctn / LOCODECK_TS_FREQ;
options->distance[current_anchor] = SPEED_OF_LIGHT * tprop;
options->pressures[current_anchor] = report->asl;

#ifdef ESTIMATOR_TYPE_kalman
// Outliers rejection
rangingStats[current_anchor].ptr = (rangingStats[current_anchor].ptr + 1) % RANGING_HISTORY_LENGTH;
float32_t mean;
float32_t stddev;

arm_std_f32(rangingStats[current_anchor].history, RANGING_HISTORY_LENGTH, &stddev);
arm_mean_f32(rangingStats[current_anchor].history, RANGING_HISTORY_LENGTH, &mean);
float32_t diff = fabsf(mean - options->distance[current_anchor]);

rangingStats[current_anchor].history[rangingStats[current_anchor].ptr] = options->distance[current_anchor];

if ((options->combinedAnchorPositionOk || options->anchorPosition[current_anchor].timestamp) &&
(diff < (OUTLIER_TH*stddev))) {
distanceMeasurement_t dist;
dist.distance = options->distance[current_anchor];
dist.x = options->anchorPosition[current_anchor].x;
dist.y = options->anchorPosition[current_anchor].y;
dist.z = options->anchorPosition[current_anchor].z;
dist.stdDev = 0.25;
stateEstimatorEnqueueDistance(&dist);
}
#endif

ranging_complete = true;
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: How to place the LPS anchors?

Post by arnaud »

When using Kalman estimation you can get the position estimate using the log subsystem.

Without the kalman estimation you can use the log subsystem to calculate a position out of the ranges. The ranges are available in log variable.

This has been done very early for the loco positioning system and we used a particle fitler (https://github.com/bitcraze/lps-ros/blo ... /lps_pf.py). Other filter could be used but it was much less precise, the resulting position had a lot of noise.
doctorlu
Beginner
Posts: 23
Joined: Mon Dec 28, 2015 8:41 am

Re: How to place the LPS anchors?

Post by doctorlu »

Thanks Arnaud, and sorry for my so many questions...
Is there existing algorithm to calculate the position coordinates, based on the ranged distances? I do not find such stuff in CF firmware.
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: How to place the LPS anchors?

Post by arnaud »

The kalman filter is the algorithm responsible for calculating the position from ranges in the Crazyflie firmware.
doctorlu
Beginner
Posts: 23
Joined: Mon Dec 28, 2015 8:41 am

Re: How to place the LPS anchors?

Post by doctorlu »

Oh that is not what I mean. I want to ask if there is any algorithm that uses ranged distances to ''directly'' calculate the exact position, rather than using kalman filtering to ''estimate'' the position. As I recall, ROS may have provided such algorithm or implemented by you guys, I am not sure of that.
Thanks.
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: How to place the LPS anchors?

Post by arnaud »

There is no such things as 'exact' position since the measurement will always have some error.

If you want to calculate a position only from the ranges there is two algorithm implemented in ROS: a particle filter and a least mean square filter. Both will produce slightly different position estimate: the LMS will tend to be smoother but can get stuck in local minima. The particule filter is much more robust but more noisy and heavier computation-wise. None of these algorithm are implemented in the Crazyflie.

The kalman filter position output is of much better quality than the two filter implemented in ROS since the inertial sensors are used in the position estimate.
Post Reply