I have a question about the fusion of range beacon data in ardupilot with EKF3 for positioning.
I am experimenting with range beacons using UWB beacons, and when I looked into the logs, I found that for periods of several seconds, the measurements of some beacons were not fused alongside the other beacon measurements. This happened consistently, and on average, at least one beacon’s data was not fused into the main filter (not always the same beacon).
I was wondering if someone has an idea why this is happening. I used the EKF3, without the GPS.
Maybe it is linked to the high rate of data (4 distance measurements at 20Hz, so 80 range measurements to fuse per second).
I also thought it might be linked to the logging of the data (so it would be fused but not recorded ? but it looks unlikely).
I do not believe the problem lies with the sensor, because the way our driver is designed, if one distance is received, all 4 are received (at the same time). Also, as I understood the code, if a distance is received, even if it is bad and doesn’t pass the innovation gate test, it is still logged at the end of the EK3 fuseRangeBeacon() method.
To do this test, I carried around a drone in a cross-shaped trajectory (indoors), between 4 UWB beacons. The 4 beacons were placed in a rectangle 7.8m long and 5.1m wide. This was done after the initialization with the fuseRangeBeaconStatic() method was finished.
Here is the plot with the ranges : (plotted with XKF0 from the log file)
A high data rate can cause data to be overwritten in the FIFO buffer before it falls behind the fusion tine horizon and is retrieved from the buffer within the EKF. Once the range beacon data starts to approach the EKF’s internal update rate of 100Hz, timing jitter can cause data to be overwritten.
However looking at your log and plotting of beacon range innovations and beacon ID’s from the XKF0 log message indicates that regular fusion of each beacon ID is occurring internally to the EKF.
which means that the rngBcnFusionReport[rngBcnDataDelayed.beacon_ID] array is not being updated for that value of rngBcnDataDelayed.beacon_ID. I’m digging through the code to see what could be causing this.
Exactly, and the ranges that we see with the XKF0.rng message, in the EKF log, match the ones in BCN.DX (if we split them between the relevant beacons), it’s just that they are not always updated.
I have found the source of my problem, so I’m posting it here in case it could help someone !
The time it takes to fuse 4 beacon’s ranges is 37ms, more or less. All my measurements arrive in batches of 4.
I had set EK3_BCN_DELAY to 30ms, so the EKF only had the time to fuse 3 measurements.
Here’s the way the data’s time is computed from the AP_NavEKF3_core.cpp file:
rngBcnDataNew.time_ms = lastTimeRngBcn_ms[index] - frontend->_rngBcnDelay_ms - localFilterTimeStep_ms/2;
As I see it, when it reached the 4th measurement, the fusion time horizon had already gone past it, and it was too old so it was erased from the ring buffer inside the EKF. Maybe it was unrealistic to try with a delay of 30ms ! With a delay of 40ms it works, but I set it at 50ms to be safe.
I’m wondering what kind of delay it takes to fuse 8 beacons at once, and if we have to raise the beacon delay even more to compensate it (although I can’t test it yet !). Then the set delay wouldn’t really represent what it’s meant to represent, the lag behind IMU data, but it would rather be the time it takes to fuse the other beacons.
Is there a simple way to see what the real beacon delay is with the log data, to see whether it’s significant or not?
We can make the buffer longer, however there will still be an underlying limitation that the EKF is currently limited to fusing a single range measurement per EKF major update cycle which is every 10msec. To get around that we could modify the code to repeat the buffer read and fusion operation until the buffer read returns no new data.
