Servers by jDrones

EKF yaw estimation drifts, while complementary filter is fine

We are developing a custom UAV using the ArduPlane 3.4 firmware.
Currently I’m having some issues concerning the yaw estimation. Sometimes, when I power on the UAV, the Yaw estimation starts drifting with a speed of few degrees per second, without converging to any value.

Analyzing the log files, I’ve noticed that all the sensors readings seem to be consistent but the EKF-based yaw estimation drifts. Here is a log file about the issue:

On the attached log file I see that all the sensors readings seem to be fine. I don’t see the magnetometer readings going crazy. Furthermore I’ve noticed that, while the DCM estimation provides a stable value for the heading (packet named AHR2, that should contain the secondary attitude solution), the EKF-based yaw estimation drifts (packet ATT).

In order to fix the problem I’ve done all the followings, but the problem still persists:

  1. Disabling internal compass
  2. Placing external compass away from the batteries (around 20 cm)
  3. Re-calibrating the compass making sure that the coefficients values are all below 150, as reported on the ardupilot wiki.

Note: the log file attached refers to a test carried out after the corrective actions listed above.

One thing worth to note is that this error seems to happen after performing the “Calibrate level” function. This function estimates the trim values that will be applied to correct the attitude estimation.

In the file AP_NavEKF.cpp, the only two parts where the trim values are used are:

  • void NavEKF::getRotationBodyToNED(Matrix3f &mat) const
  • void NavEKF::getEulerAngles(Vector3f &euler) const

The first function performs a transformation matrix from XYZ (body) to NED axes that also uses the trim values estimated with the “calibrate level” functions. These values, even if are related to the roll and pitch axes, affect also the yaw. Therefore, if not estimated properly, they could lead to the drift I’m seeing.

Could this be the problem? Do you have any other idea?

Thank you.

Servers by jDrones