Decoupling the TDoA ranging system.

[AlexisTM]

Context

One of the use-cases of the Roadrunner if a Robot localization system.
In such systems, we often have high computation capable systems (onboard computers) such as the Odroid XU4 or other embedded Linux devices. In such cases, every robot has a fined-tuned EKF running incorporating their kinematics. It this use-case, the roadrunner is fixed onto the robot and connected either in UART or USB.

Precedent steps taken for each issue for this use-case:

• When the USB connection is dropped, the Roadrunner freezes, then the watchdog is rebooting the board (STM32 issue? Crash when USB connection is terminated #393, Fail to connect via USB and get logs multiple times #394)
  • workaround: Disable the watchdog:

    crazyflie-firmware/src/drivers/src/watchdog.c

    Line 78 in e998ddc

    IWDG_Enable();

• The Kalman filter will not start fusing until the Gyroscope Bias is determined.
  • workaround: Pre-calibration of the Gyroscope and loading it if Bias calibration failed - https://github.com/umdlife/crazyflie-firmware/pull/2
• The Gyroscope calibration will fail if the Roadrunner is not flat (DWM1000 works best positioned vertically).
  • workaround: Pre-calibration of the Gyroscope and loading it if Bias calibration failed - https://github.com/umdlife/crazyflie-firmware/pull/2
• The TDoA system is coupled to the IMU in the Kalman filter and CF kinematics causing an overshoot of the position, also, we do not know if the TDoA system reports a position or not while the position of the EKF might be drifting to 100 or -100. (Decoupling the TDoA ranging system. #403)
  • Decoupling the TDoA: This issue might solve it.

Proposition

Therefore, we would like to decouple the position estimation of the TDoA off the Kalman filter to allow to fuse it in the onboard EKF filter (robot_localization in our case).

I salvaged a Python library to test the behaviour of localization: AlexisTM/Multilateration. It is a proof of concept of multilateration using LSE minimization method on a list of circles. It computes the first approximation of position which is then minimized.

In this fork, I added a "goal" which will force one or the other axis to be first approximated to a fixed value. That allows computing an XYZ position from anchors which are all on the same height for a robot which is at a known height. Indeed, the minimization process will then be forced to converge to the correct value and not the ambiguity.

Changing the "goal" to the last measured value will definitely make it capable of fast convergence but will have a localization issue if the robot height goes close to the plane of the anchors. Indeed, the convergence could be to the wrong height then.

If such an algorithm works well, we could create an alternative estimator that returns the "raw UWB measurements".

As a flaw, I see that this method does not provide the "estimated error" which is primordial for further fusing. I guess this can be estimated from the way the circles (anchor position + measurement) interacts to each other. There should be a paper providing an analytic solution.

[krichardsson]

A first step could be to generalize the functions (https://github.com/bitcraze/crazyflie-firmware/blob/2019.02/src/modules/interface/estimator_kalman.h#L68-L73) in the kalman filter that are used to push sensor data to the filter. A generic version of the functions should be implemented in estimator.c that passes the call on to the active estimator. This would enable you to add a new estimator implementation that do what ever you are interested in.

TDoA3 produces around 300 TDoA measurements/s, where each measurement contains the position of the two anchors + the difference in distance to the anchors. The total data rate of raw TDoA data should end up around 10-15 kilo bytes/s. Currently there is no functionality to stream the data off from the Roadrunner, but from a technical point of view it should be possible.

You could even implement a "virtual" estimator that streams the raw TDoA data + other sensors to an external system that estimates the position and then streams the result back to the estimator.

[AlexisTM]

Note that the use-case I am pointing out is not about improving the estimator, but outsourcing the estimator to a more powerful and generic estimator (robot_localization) that runs on a different machine.
The roadrunner is therefore converted from a robot to a sensor of a robot. In such a case, depending on the robot, you (user) might want the fused or raw data stream.

[krichardsson]

I hope the suggested solution would support this use case, as well as alternative estimators if someone would like to work on that. And it would break some dependencies in the code that will make it cleaner and more testable as well, which is nice.

[AlexisTM]

The solution that PX4 chose is to use a communication middleware between tasks instead of static queues (uOrb, now going to FastRTPS). That allows decoupling estimator and sensor tasks. Yet, such modification is huge.

[krichardsson]

We usually try to keep it simple and minimalistic :-)