I have multiple copters running 3.6.9 (Cube Black) that are running into a strange problem with altitude control. Basically, the EKF altitude estimate is correct, but it somehow disagrees with the altitude logged by the position controller in CTUN and POS messages. Unfortunately, those are the values which are ultimately used for control, so the drone does not maintain altitude correctly. Here is an example:
Here, the EKF vertical position PD (I inverted it) agrees with the barometer and the rangefinder. However, the Z controller is using the altitude reported in CTUN.Alt or POS.Alt, which diverge severely from the EKF altitude. The rangefinder can be used as a source of truth in this case.
This is in contrast to a typical flight, which looks like this.
Here, the EKF, CTUN, and POS altitudes are all right on top of each other. This is normal and makes sense, because the controller is supposed to be using the EKF estimate, and these values are all being logged from the same source (see below). Worth noting that this flight is from the same drone a few days earlier with the same parameters.
Ultimately, altitude is derived from climb rate, so just as a demonstration, here is the first flight again with climbrates and vertical velocities graphed:
The CTUN climb rate quickly diverges from the EKF vertical velocity. The GPS vertical velocity was not being used by the EKF at the time (
EK2_GPS_TYPE = 1
), so the GPS can be used as a third source in this case, and it agrees with the EKF.
So, how did the Z controller diverge from the EKF? I can’t figure it out. According to the source code, the logged values come from the same place:
NKF2.PD just logs the value returned by that function directly: https://github.com/ArduPilot/ardupilot/blob/Copter-3.6/libraries/DataFlash/LogFile.cpp#L909
For CTUN.Alt, that same value goes through a frame and units conversion here: https://github.com/ArduPilot/ardupilot/blob/Copter-3.6/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp#L36-L37
then gets passed unmodified by https://github.com/ArduPilot/ardupilot/blob/Copter-3.6/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp#L115 for logging.
I can’t find any opportunity for EKF and CTUN logged values to disagree - it’s supposed to be the same number! Looking for some technical help because this is driving me (and my drones) nuts. @rmackay9 I hope you don’t mind the tag, I’m not sure who would know best what is going on here.
Here is the log for the bad flight graphed above.
log62 inconsistent ekf ctun alt.bin (632 KB)