Loco position estimation incorrect

[easy98]

Hi,

I use cfclient to setup LPS system. I notice that the position of crazyflie estimated by LPS seems incorrect. I feel its x and y position is almost correct, but the z position is definitely wrong. Also, I find the positions in different modes (TWR, TDOA2, TDOA3) are totally different. If it's normal or just something wrong with my setting?

Thanks in advance.

[knmcguire]

Hi!

It does not seem that there is anything wrong in the screenshots you send. Although probably a plot of the position estimate would be a bit more handier.

It can also be due to the setup. The nodes should be away from any walls or obstacles as in the tutorial instructions. Also the placement is important, as the distance tracking works the best on the side of the antenna on the lps node

Other then that, there are a lot of reasons why this happens that we do not have a good answer for.

Just to be clear, is the z axis position completely wrong or are you seeing a bias?

[easy98]

Hi, I think the z axis is totally wrong. Also, in TWR mode, I log the raw data (the distance between each anchor and the tag) to see if there is anything wrong. I find the distance of some anchors seem incorrect in some positions, since it has a very large fluctuation(like varying from 2m to 5m) and not stable. I try to re-prepare those anchors but it doesn't work. Now I just have some assumptions about the problem: 1) The manual position measurements of some anchor might be inaccurate. But I don't think just a few errors in measurement can cause such a wrong result. 2) Based on this page, it said that "we have placed 4 anchors above and 4 anchors bellow the flight area". So if the lower four anchors have to be placed strictly below the quadrotor, or it would cause some problems? Now I place the quadrotor in the center of the area, and its z position is below the lower anchors' position. So it would have some problems? 3) As shown in the pictures, the anchor is fixed in these two ways. I think the node should have been placed at least 15cm from any obstacles. But I am not sure if it's fine and might have some interference.

[knmcguire]

Those on tripods should be okay I think.

However, the nodes on the floor, are those usually also on the floor during ranging? The floor is also an obstacle as well so the node is too close to it. See this page as a reference.

If this doesn't work either you can try to increase the TX power of the nodes by the low level configuration

[easy98]

Hi,

The nodes are not always on the floor, and they are vertical to the floor. I try to reset up the reference and increase the TX power. But it still doesn't make sense. The z-axis is still wrong. Now I am so confused about this problem.

[krichardsson]

Hi!

In your initial images it looks like TWR and TDoA2 are fairly similar and sort of what I would expect, even though the Y-diff is on the large side (assuming the CF was at [3.0, 2.0, 0.0]). TDoA3 on the other hand is far off from this position which is unexpected. Usually TDoA2 and TDoA3 are similar while TWR gives a slightly different result (we don't know why).

The manual position measurements of some anchor might be inaccurate. But I don't think just a few errors in measurement can cause such a wrong result.

As long as the measurements are +-5 cm is should be fine

Based on this page, it said that "we have placed 4 anchors above and 4 anchors bellow the flight area". So if the lower four anchors have to be placed strictly below the quadrotor, or it would cause some problems? Now I place the quadrotor in the center of the area, and its z position is below the lower anchors' position. So it would have some problems?

In TDoA it works best inside the convex hull (inside the space formed by the anchors), but it will not stop working abruptly when you pass the edge, only degrade when you move away. In TWR the system is not very sensitive to where the anchors are and it usually works pretty well outside the convex hull as well.

As shown in the pictures, the anchor is fixed in these two ways. I think the node should have been placed at least 15cm from any obstacles. But I am not sure if it's fine and might have some interference.

The one on the floor might be problematic, but I don't think it should be too bad. You could try to leave the battery on the floor and put the anchor on top of a small box (or similar) to see what happens.

Do you get the same type of result if you put the CF a bit higher up, on a box for instance? Sometimes there are problems close to the floor.

We have also seen that in some environments there are more problems, we don't know why but assume it is related to radio interference. you could try to move the system somewhere else (another room or maybe different house) just to see if there is a difference.

Another experiment could be to make the system a bit smaller (maybe 3 meters) to see if it makes a difference.

Did you try to increase the TX power as suggested by @knmcguire ? Did it make a difference?

By the way, what version of the firmware are you running? Is it fairly recent? (also in the anchors)

[easy98]

Hi,

Now I reset up the system, increase the TX power and do some tests. I think the x and y position is accurate but z position is still incorrect.

First, I put the CF in the original point on the ground and see the position estimation of three different modes (TWR, Tdoa and Tdoa3). The result of TWR seems more accurate while the results of Tdoa and Tdoa is very similiar.

Then, I put the CF on different height to see its position. When the real height is about 0.02m (I put the CF on the ground), I test 4 different positions and the x y position is correct: 1) x = -0.88, y = -1.50, z = 0.45; 2) x = -1.01, y = 0.48, z = 0.33; 3) x =0.73, y = 0.64, z = 0.41; 4) x = 0.02, y = 0.01, z = 0.44.

When the real height is about 0.45m, I test 3 different positions and the x y position is correct: 1) x = 0.03, y = -0.06, z = 0.62; 2) x = 0.80, y = 0.51, z = 0.62; 3) x =-0.02, y = 1.80, z = 0.56.

When the real height is about 0.70m, I test 4 different positions and the x y position is correct: 1) x = 1.12, y = 0.48, z = 0.74; 2) x = -0.89, y = 0.31, z = 0.70; 3) x =-0.69, y = 1.90, z = 0.75; 4) x =-0.06, y = -0.08, z = 0.56.

About the firmware, I update all the anchors which should be fine. As for the CF, I am a little bit confused if the firmware of crayflie and crazyswarm both works on LPS? Since my purpose is to build the LPS for crazyswarm, so initially I updated the firmware based on crazyswarm. Then, I suspect something wrong with the firmware and I updated it on the python client which should be the latest version. But actually the results do not have any difference.

Finally, I add a flow-deck and now the z position seems correct. But without a flow deck, the system is incorrect. I guess if there is something wrong with the kalman estimator itself. My plan is to log the ranging.distance0....., and to recheck it. If the distance between the anchor and tag is correct, which means the raw input data of the kalman estimator should be fine, so it most likely that something wrong with the kalman estimator. Actually, I don't have other ideas to solve this problem now, or do you have any other suggestions?

Thanks in advance,

[easy98]

Hi,

I do some tests to see the distance between CF and anchors. I use Vicon to get the real position and calculate the distance between CF and anchors, comparing it with the distance obtained by LPS. I feel that the distance obtained by LPS is not so correct and that might be the reason for the wrong estimation of Z-axis.

In my LPS, the position of anchors is:

I put the CF in different positions and to compare the true distance and the distance obtained by LPS. I found the distance measured by LPS is always shorter than the true distance. Here are some results:

Also, I find in some cases, the distance measurement of LPS between the CF and some anchors is unstable and has a large variance. For example, in the following case, the distance measurement of CF and anchor 0 is unstable (highlight in red color).

Actually, I am still so confused about why those problems happen. Do you have any ideas or solutions?

Thanks in advance,

[krichardsson]

Finally, I add a flow-deck and now the z position seems correct.

The flow deck has a distance sensor pointing downwards that is very precise, it provides "good" information for Z which improves the Z-estimate greatly.

It is interesting that the distances seems to be too short in all measurements, not sure why though. There is a dependency on the orientation of the antennas as well, but I don't know if it is related to what you see.

The large variance you see seems to be quantized in 3 groups: 4.1, 4.7 and 5.2 m. It is possible that this is related to reflections.

[easy98]

Hi,

Thank you for your reply! All the anchors are vertical to the ground. So it seems that I need to tilt those anchors a little bit. Since my purpose is to get the TWR or TDOA data of anchors and test our own UWB localization algorithm. So it's important to make those data accurate in my experiments. Also, when I measure the position of anchors, does it mean to measure the coordinates of this part?

Does the shorter distance relate to AntenaDelay? I think I should reduce the ANTENNNA_OFFSET and how can I change it?

Thanks!

[krichardsson]

Hi!

We use the Decawave DWM1000 module in the LPS system, with pretty basic settings. One way to see the system is that we add two layers of logic/algorithms on top of the "raw" measurements from the UWB chip:

1. calculations of distance or TDoA
2. position estimation in the kalman filter

Each layer is of course a potential source of errors and I think they roughly can be categorised as:

1. Calculations of distances mainly includes adding and subtracting time stamps, handling of outliers and wrapping of counters. If something goes wrong here, the result usually is very wrong, like hundreds of meters. This is also where the antenna delay is used and this is an important parameter that is related to the hardware design.
2. The kalman estimator is harder to verify and can errors can be pretty "fuzzy"

The purpose of this small explanation is to indicate where to look for more information and problems. Anything related to measurement errors on the UWB radio level, antenna radiation patterns, receive power, timers and so on is documented by Decawave (https://www.decawave.com/). I know they have some papers about measurement errors and other topics that may be of interest to your research.

Also, when I measure the position of anchors, does it mean to measure the coordinates of this part?

Yes, that is the UWB antenna.

• Does the shorter distance relate to AntenaDelay? I think I should reduce the ANTENNNA_OFFSET and how can I change it?

The antenna delay compensates for the time it takes for the radio packet to travel from the transmitter through the hardware into space (it is around 150m when converted to distance). We simply used a couple of anchors to measure it. It is possible that the antenna delay is slightly different between anchors/decks but it is not a problem we have seen. On the other hand, we have not investigated it very thoroughly so it is possible that it actually is a problem. There is also a dependency between receive power and the measured receive time, but that should be compensated for in the code.

Anyway if you want to change the antenna delay you must recompile the firmware. It is defined here https://github.com/bitcraze/crazyflie-firmware/blob/master/src/deck/drivers/src/lpsTwrTag.c#L49 for the Crazyflie firmware and here https://github.com/bitcraze/lps-node-firmware/blob/11b7eeae7370c5de5a1234e583fcf164e5211310/src/uwb_twr_anchor.c#L93 and https://github.com/bitcraze/lps-node-firmware/blob/11b7eeae7370c5de5a1234e583fcf164e5211310/src/uwb_tdoa_anchor3.c#L96 in the anchor firmware

[easy98]

Thanks very much for your reply! It is very helpful and gives me a lot of useful information. Although I don't know the real reason for this problem now. I would give an update once the problem is solved!

[krichardsson]

Hi! Have you managed to solve this problem?

[easy98]

Hi, sorry for the delay. Actually, I am not able to solve this issue yet. I still feel the measured distance of TWR is not so correct. I see this issue might cause this problem, and what might be the proper value? Thanks!

[dominiknatter]

Hi Kristoffer,

I have lately been looking into UWB biases as well. First of all, from my tests I can confirm that there is quite some rotation-dependence, as you've suggested in this issue.

However, here I wonder about this quote of yours:

There is also a dependency between receive power and the measured receive time, but that should be compensated for in the code.

For me this sounds like changing the power of the UWB module (as described in this post) will not change the bias, because the dependency between time and power is already being compensated for. Is that correct? If that is what you're saying, then I have to disagree based on my tests. Before changing the power the UWB measurements with the default LOCODECK_ANTENNA_OFFSET (154.6m) were already too short in most cases. After changing the power of both the receiving and the sending module to full power, this effect increased (i.e. the bias seems even more pronounced).

[knmcguire]

hi @dominiknatter thanks for sharing this! @krichardsson is currently out of office until the end of August, so just letting you know.

[knmcguire]

So, this has been a bit dorment now... how has the situation been so far for you @easy98 and @dominiknatter ?

[krichardsson]

I completely missed this, sorry! I will add some information for anyone interested in the topic.

The dependency between receive power and the measured receive time is documented in the Decawave application notes (APS011) for the DW1000 chip "Sources of Error in Two Way Ranging", section 3.4 Friis' path loss formula and range bias correction value

There is also some information in DW1000 user manual, section 4.7 "Assessing the quality of reception and the RX timestamp"

As far as I understand, this is implemented in https://github.com/bitcraze/libdw1000/blob/master/src/libdw1000.c#L702-L754

@dominiknatter it is interesting if you get different results depending on TX-power, it is not impossible that there are bugs or that the implementation is flawed.

[dominiknatter]

Hey, @knmcguire we didn't investigate this much further, we just kept on using the system at a specific power rating and a specific LOCODECK_ANTENNA_OFFSET. In the future we might have a closer / more quantitative look at the UWB modules again. @krichardsson thanks for the confirmation. Now that I read your new comments it might also be related to Friis' path loss formula, given that there the transmitted power affects the received signal level and that in turn affects the range bias.

[krichardsson]

I'm closing this due to inactivity and also because it is not really a client problem. If this is still a problem that should be addressed, please open a new issue in the https://github.com/bitcraze/crazyflie-firmware repo (and refer to this issue)