Snapdragon: Poor altitude estimation with range sensor

[ndepal]

After following the advice given in #111, we have come to the conclusion that altitude estimation performance of the complementary filter is still not satisfactory.

This can be seen in the following image:

EST0.s8 tracks the range data (the sign of which is inverted in the shown plot) well, while LPOS.Z does not. LPOS.Z often overshoots in tracking changes in altitude. At 55s, LPOS.Z does something completely different than EST0.s8. Between 20s and 40s, the quad is placed on the ground (the range sensor saturates at its minimum value) and is disarmed (thus, IMU values are not noisy). Still, LPOS.Z overshoots significantly and appears to saturate at an offset value.

The depicted log is here: http://logs.uaventure.com/view/CYBGUXRipknC9Rr5Ug9kN4

Here is another replay log of my current setup: http://logs.uaventure.com/view/KphLtTVUAttaQhPtUf5XBn

The firmware is on ffb0d37 with all submodules updated.

On top of that I have added a low pass filter for the acceleration measurements fed into the control_status with a cutoff frequency of 30Hz, as suggested in the above-mentioned issue. I have also added the control_status acc measurements to the log file. However, it appears as though we have an estimation problem and not (or not only) a control problem.

@priseborough can you help us with this?

[priseborough]

Yes i can - I will do some analysis on the replay log.

[priseborough]

I'm away on travel for the rest of the week, so will not be able to resume work on this until Sat

[priseborough]

I have some requests for further tests:

1. Can you please provide a short replay log where the copter is placed upright and held still in a level orientation for 5 seconds and then rotated upside down and held still and level in an inverted orientation for another 5 seconds. It is possible that the accel calibration may not be correctly calibrating the delta velocity outputs.

2. Can you please generate a replay log where the copter is held at 50cm, 100cm and 150cm above the floor with the heights determined using a tape measure or other mechanical means. it is possible the range finder has a scale factor error.

3. Can you please test the following PR and report back: EKF: Improve output observer position and velocity tracking #129. This should improve the performance with inconsistent sensor data, but the root cause of the data inconsistency should still be determined.

Here is a plot of the comparison in height between the range finder, EKF and output predictor geenrated by replaying http://logs.uaventure.com/view/KphLtTVUAttaQhPtUf5XBn through the PR

Note that the EKF overshoot following large step changes in range finder is partly due to bias state estimation (similar effect to the use of integrator feedback in a controller).

[ndepal]

1. Log here: http://logs.uaventure.com/view/XkZMTFNZGbgwxXSDMpdSSD

2. Log here: http://logs.uaventure.com/view/SgLGKSantZPuGRGyaokEn9
   I tried to do this as accurately as possible, with the range sensor perpendicular to the ground, but of course slight tilt will also cause a scale factor.

3. I flew with ee33f21, which already has EKF: Improve output observer position and velocity tracking #129 merged.
   Log here: http://logs.uaventure.com/view/abRQ9KEnGNMY5Z2mBKarEP
   I still see significant oscillations.
   Another thing that I find strange is the drift at the beginning, where the quad is sitting on the floor, disarmed (I arm at 90s). Could this also be due to bias estimation? Or is it that range measurements are not fused if current_distance == min_distance?
   

Regarding the overshoot and bias estimation. Is this something you observe on all platforms? Could this be tuned away?

[priseborough]

To a lesser extent, some initial drift and overshoot is evident on other platforms, but in-flight the tracking performance is good. The default filter tuning can tolerate large switch-on bias errors for the IMU and rapid changes in bias during flight - ie it is tuned for robustness, not performance. As a result there will be more drift and overshoot in the first 30 seconds after switch on. This can be tuned out, although the initial bias uncertainties are currently hard coded, so I would need to add parameters to enable you to tune them.

I have attached plots of the range, EKF height and range finder in-flight variation for my pixhawk optical flow setup. This test was in manual mode so i could jump the hight up and down with the throttle. The offset is due to the Z offset parameter settings for the sensors.

Switch-On

In-Flight

I will now have a look at your test data and see if there is anything that shows up.

[priseborough]

I have worked through some potential causes:

1. Range finder errors. Your range finder does not return a measurement when the copter is on the ground with a default value of 20cm. This is contributing to overshoot during takeoff and landing. There is no evidence of a significant range finder scale factor error. The vertical position innovations during height changes indicate that the range finder and IMU data are not correctly time aligned. This will be further investigated.

2. IMU errors. IMU accelerometer calibration looks OK. IMU data quality is poor overall as the complementary filter is requiring a lot of correction to track the EKF states.

3. IMU accel bias learning. The effect of accel bias estimation on start-up and in-flight can be eliminated by setting EKF2_AID_MASK = 6 which turns off the accel bias states. The following is a replay of your range finder test log with accel bias on (EKF2_AID_MASK = 2)

and accel bias estimation off (EKF2_AID_MASK = 6)

Other than the change in height offset, there is not much difference

I will have another look tomorrow

[ndepal]

1. I can't confirm that the range finder does not return a measurement when on the ground. If the range is below 20 cm or above 14 m the device simply saturates it's measurements at these limits. I still publish the measurements on the ORB topic, however. I also don't see anything in EKF2 that actually checks if the range measurement is within the limits like the ekf_att_pos_estimator does, for example. So as far as I can see, the 20 cm measurements are constantly fused while the copter is on the ground.

[priseborough]

Yes the measurements are fused, but the saturation of the sensor is causing an inconsistency when the copter takes-off or touches down. This makes the solution overshoot.

[priseborough]

@ndepal let me know if there is anything more you think needs to be done inside the ecl library. If not and you now believe your altitude variations are caused by the 'spikes' in the delta velocity , can you please close this issue and raise one on the PX4/Firmware.