The roadrunner seems very unreliable when the TDoA(v3) anchors are powered after the roadrunner

[AlexisTM]

Short description

The estimator has an issue when the TDoA (v3) anchors are started after initialisation. The master branch outputs becomes chaotic. My guess is that the position of the TDoA shouldn't be impacting orientation of the copter.

The following gif shows what happens when the 3rd anchor is powered (thus giving chaotic position because it is only 3 anchors)

More information

I hooked up MAVLink into the estimator abstraction to send the roadrunner state, sensors and TDoA in the following places: Sensors and Kalman state and TDoA Measurement

It seems to be working when the TDoA anchors are started when powering the roadrunner. Yet, if the anchors are not connected beforehand and then connected, the GYRO/ACC/POSE are no more received.

Rates of the following log:

• Pose/Gyro/Accelerometer: 21Hz (Rate divider of 45)
• TDoA Measurement: Full rate (only 3 anchors powered).

<---- Anchors are powered
100 of TDOA_MEASUREMENT 6.42449980792e-08Hz
200 of TDOA_MEASUREMENT 68.3389653768Hz
300 of TDOA_MEASUREMENT 72.5591472479Hz
100 of GYRO_ACC_TEMP 6.42449979278e-08Hz
100 of POSE 6.42449979278e-08Hz
400 of TDOA_MEASUREMENT 67.35444369Hz
500 of TDOA_MEASUREMENT 65.3145733908Hz
600 of TDOA_MEASUREMENT 73.8993382137Hz
200 of POSE 21.7295319045Hz
200 of GYRO_ACC_TEMP 21.5019781001Hz
700 of TDOA_MEASUREMENT 58.8140502948Hz
800 of TDOA_MEASUREMENT 73.8559019777Hz
900 of TDOA_MEASUREMENT 63.9884103023Hz
300 of POSE 21.6648247219Hz
1000 of TDOA_MEASUREMENT 90.4495907312Hz
300 of GYRO_ACC_TEMP 21.6609948824Hz
1100 of TDOA_MEASUREMENT 109.595409555Hz
1200 of TDOA_MEASUREMENT 92.252130509Hz
1300 of TDOA_MEASUREMENT 104.979034498Hz
1400 of TDOA_MEASUREMENT 94.6548485316Hz
400 of GYRO_ACC_TEMP 21.7170073109Hz
400 of POSE 21.4876226125Hz
1500 of TDOA_MEASUREMENT 97.2235801785Hz

When the anchors are powered afterwards:

100 of GYRO_ACC_TEMP 6.42449900007e-08Hz
100 of POSE 6.42449900006e-08Hz
200 of GYRO_ACC_TEMP 21.7721336058Hz
200 of POSE 21.7713515593Hz
300 of GYRO_ACC_TEMP 21.7794991255Hz
300 of POSE 21.7806753566Hz
400 of GYRO_ACC_TEMP 21.7723189545Hz
400 of POSE 21.7724896138Hz
500 of GYRO_ACC_TEMP 21.7782359495Hz
500 of POSE 21.7782178567Hz
600 of GYRO_ACC_TEMP 21.7729778722Hz
600 of POSE 21.7730151707Hz
700 of GYRO_ACC_TEMP 21.7725755095Hz
700 of POSE 21.772253404Hz
800 of GYRO_ACC_TEMP 21.7786057271Hz
800 of POSE 21.7782269031Hz
<---- Anchors are powered
100 of TDOA_MEASUREMENT 6.42449885102e-08Hz
200 of TDOA_MEASUREMENT 44.2626928108Hz
300 of TDOA_MEASUREMENT 57.075867163Hz
400 of TDOA_MEASUREMENT 52.434069624Hz
500 of TDOA_MEASUREMENT 46.8391504481Hz
600 of TDOA_MEASUREMENT 46.0489989954Hz
700 of TDOA_MEASUREMENT 54.5137959686Hz
800 of TDOA_MEASUREMENT 51.8660918035Hz
900 of TDOA_MEASUREMENT 55.2731347888Hz
1000 of TDOA_MEASUREMENT 49.475706405Hz

[krichardsson]

It is odd that you do not get any GYRO_ACC_TEMP and POSE data when you power the anchors. I have no good explanation for this. I don't see why? Can it be caused by an over flow somewhere? That the TDoA data fills some buffer and the other data is discarded? Do you see the same behaviour if you only power one anchor?

Have you checked what the estimated position is just before you power the anchors? If it is far away from the real position, the kalman filter can maybe go crazy when it starts to get TDoA data?

[AlexisTM]

It does go crazy once it gets the TDoA data, especially for the attitude. I do think that this is a bug as the attitude should not be impacted by the possibly bad TDoA data.

When we start the roadrunner in a properly calibrated system, it does not seems to have the issue for the small time we actually had a roadrunner connected to the computer for manual checks (Meaning I did not replicate the problem)

I don't understand why would the update function not be called anymore. It could also be on the way the data is published that fails under certain conditions and that the update function is actually still called.

[krichardsson]

@AlexisTM I can not reproduce this.
I have added counters and logging to stateEstimator() and estimatorEnqueueTDOA() to monitor when they are called, but have not been able to observe that stateEstimator() is no longer called when the LPS system is turned on. Not sure if I miss understood the issue?

[AlexisTM]

This could be another bug introduced by a wrong usage or an overlooked problem due to the MAVLink middleware. I am closing this for now.