Copter unexpectedly descended in guided mode

jomiham · September 23, 2022, 5:11pm

(sorry for the long post, I kept finding things as I was writing it… )

I am trying to evaluate relative position tracking using Optical Flow in Guided mode and had a strange issue in one of the flights.

The setup is quite basic:

CubeOrange with Ardupilot v4.2.2, Auto-tuned
HereFlow and separate RangeFinder is configured and fairly tuned (works well in Loiter etc.)
Using separate EK3_SRC2 with OF+Barometer+Compass with RC switch
Using MavProxy to basically do takeoff to e.g. 3m move forward 5m and then land:

arm throttle
takeoff 3.0
message SET_POSITION_TARGET_LOCAL_NED 0 0 0 9 3576  5.0 0.0 0.0  0 0 0  0 0 0  0 0
long NAV_LAND 0 0 (or switch to Loiter and land manually)

This works fairly well and typically I get this kind of altitude curves

Video

But in one flight, the drone unexpectedly descended while moving forward.

Video

The rangefinder, barometer(s) and gps all indicates the drop in altitude, but for some reason that is not reflected in CTUN.Alt.

I have tried getting my head around the various XKF log messages and one thing that stood out is that EKF3 core 0 (blue) is the one that is off (and likely used), and it also has a very interesting “flatline” in the XKF3[0].IPD at the very time it starts to descend and a brief activity right when it stops…

For the “good” flight, there were no such flatlines. There were some high innovations for core 1 in both cases though…

So then I looked at the IMUs and sure enough IMU[2] is way off compared to 0 and 1.
It looks ok:ish before takeoff though, so I’m guessing it is due to vibrations.
Interestingly, all IMUs seem to report as healthy (IMU[].AH=1)?

My questions:

I’m guessing I should do an IMU calibration and look into notch filtering for the vibrations? Or could it be something else?
What could have caused the flatline / freezing effect for EKF core 0? I.e. what caused the drone to disregard the barometer etc?
Apart from the “freezing” issue, it seems that EKF core 1 (orange) is the most “off” compared to the others. Could it be that XKF[1] uses IMU[2] in this case, or is it always that XKF[n] uses IMU[n]?

Note
For some reason I cannot upload the log file (4MB) as I get “maximum size is 4.4MB” even when zipped to 1.9MB so I’ll try and upload it in a comment instead.

jomiham · September 23, 2022, 5:16pm

Still couldn’t upload a 1.9MB zip file so I have put the log here for now:
https://www.dropbox.com/s/r4tkdth3f5szc64/log_102_2022-9-22-13-41-46.bin?dl=1

It contains three flights, and looking at it now, it seems that there are some “flatline” issues on the first one as well, but I didn’t react to it at the time.

The second flight is the “good one” and the third is the descending one.

amilcarlucas · September 23, 2022, 5:31pm

Take a look at:

jomiham · September 24, 2022, 12:03am

Thank you @amilcarlucas for responding.

I looked at the video and the posts, and maybe I’m missing something but that is for managing incorrect altitude changes due to errors in the baro readings?

In this case, the baro readings are in fact pretty accurate but for some reason the CTUN.Alt doesn’t follow the CTUN.BAlt (for one of the EKF cores).

jomiham · September 28, 2022, 8:15am

Sorry if this is an obvious case of bad IMU, but I looked some more into this and just want to add that the “flatline” effect for the IPD values come from the “Ground Effect Compensation”.

github.com

ArduPilot/ardupilot/blob/Copter-4.2.2/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp#L892-L907


      
          innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - velPosObs[obsIndex];
          const ftype gndMaxBaroErr = MAX(frontend->_baroGndEffectDeadZone, 0.0);
          const ftype gndBaroInnovFloor = -0.5;
          
          
if ((dal.get_touchdown_expected() || dal.get_takeoff_expected()) && activeHgtSource == AP_NavEKF_Source::SourceZ::BARO) {
              // when baro positive pressure error due to ground effect is expected,
              // floor the barometer innovation at gndBaroInnovFloor
              // constrain the correction between 0 and gndBaroInnovFloor+gndMaxBaroErr
              // this function looks like this:
              //         |/
              //---------|---------
              //    ____/|
              //   /     |
              //  /      |
              innovVelPos[5] += constrain_ftype(-innovVelPos[5]+gndBaroInnovFloor, 0.0f, gndBaroInnovFloor+gndMaxBaroErr);
          }

As expected, the takeoff_expected is flagged while on the ground and and cleared shortly after takeoff.

The drone continues to ascend and then oscillates a bit around the target altitude. This causes the touchdown_expected flag to get set on each small “descent”.

I’m still not sure if/how this could cause the actual descent, but at least it explains the IPD flatlines.

Could it be that the ground effect compensation basically ignores the barometer in favour of the IMU so if the IMU is off, the drone will basically be flying blind?