EKF estimates Yaw incorrectly

[whoenig]

When flying without external yaw-compensation, the Crazyflie will quickly (about 30seconds) become unstable due to incorrect yaw estimates. Until now, it seems to have assumed to be normal, as there are workaround in the code (e.g., push "virtual" yaw measurement in lighthouse:

crazyflie-firmware/src/modules/src/lighthouse/lighthouse_position_est.c

Line 445 in 0787950

estimateYaw(state, angles, baseStation);

).

From preliminary Python bindings, I found that the yaw is indeed estimated very badly (off by 20 degrees after 30s), even in cases where the gyro.z data is excellent.

When setting quadIsFlying to False (or alternatively deleting

crazyflie-firmware/src/modules/src/kalman_core/kalman_core.c

Lines 480 to 508 in 0787950

	float zacc;
	if (quadIsFlying) // only acceleration in z direction
	{
	// Use accelerometer and not commanded thrust, as this has proper physical units
	zacc = acc->z;
	// position updates in the body frame (will be rotated to inertial frame)
	dx = this->S[KC_STATE_PX] * dt;
	dy = this->S[KC_STATE_PY] * dt;
	dz = this->S[KC_STATE_PZ] * dt + zacc * dt2 / 2.0f; // thrust can only be produced in the body's Z direction
	// position update
	this->S[KC_STATE_X] += this->R[0][0] * dx + this->R[0][1] * dy + this->R[0][2] * dz;
	this->S[KC_STATE_Y] += this->R[1][0] * dx + this->R[1][1] * dy + this->R[1][2] * dz;
	this->S[KC_STATE_Z] += this->R[2][0] * dx + this->R[2][1] * dy + this->R[2][2] * dz - GRAVITY_MAGNITUDE * dt2 / 2.0f;
	// keep previous time step's state for the update
	tmpSPX = this->S[KC_STATE_PX];
	tmpSPY = this->S[KC_STATE_PY];
	tmpSPZ = this->S[KC_STATE_PZ];
	// body-velocity update: accelerometers - gyros cross velocity - gravity in body frame
	this->S[KC_STATE_PX] += dt * (gyro->z * tmpSPY - gyro->y * tmpSPZ - GRAVITY_MAGNITUDE * this->R[2][0]);
	this->S[KC_STATE_PY] += dt * (-gyro->z * tmpSPX + gyro->x * tmpSPZ - GRAVITY_MAGNITUDE * this->R[2][1]);
	this->S[KC_STATE_PZ] += dt * (zacc + gyro->y * tmpSPX - gyro->x * tmpSPY - GRAVITY_MAGNITUDE * this->R[2][2]);
	}
	else // Acceleration can be in any direction, as measured by the accelerometer. This occurs, eg. in freefall or while being carried.
	{

and

crazyflie-firmware/src/modules/src/kalman_core/kalman_core.c

Lines 553 to 560 in 0787950

	if (! quadIsFlying) {
	float keep = 1.0f - ROLLPITCH_ZERO_REVERSION;
	tmpq0 = keep * tmpq0 + ROLLPITCH_ZERO_REVERSION * initialQuaternion[0];
	tmpq1 = keep * tmpq1 + ROLLPITCH_ZERO_REVERSION * initialQuaternion[1];
	tmpq2 = keep * tmpq2 + ROLLPITCH_ZERO_REVERSION * initialQuaternion[2];
	tmpq3 = keep * tmpq3 + ROLLPITCH_ZERO_REVERSION * initialQuaternion[3];
	}

) the estimation works as expected (i.e., within 2/3 degrees after 30s). This last part has been verified both with the Python bindings and in real flight tests. Specifically, when removing this code, it is possible to hover in-place until the battery runs out using single-marker tracking in the Crazyswarm.

The main difference between the two modes is the use of the accelerometer. In the quadIsFlying mode, acc.z is used to estimate the commanded thrust (as rpm measurements are not readily available as proposed in the paper). In the second, better-working, mode, the accelerometer is used for position and velocity update (using the IMU rather than concrete dynamics models knowledge). I haven't found out yet why this differences causes such dramatic problems with the yaw estimate (and not the other orientation components).

@jpreiss: Did we always use acc+gyro for the prediction step in KalmanUSC? Do you have some insights on why the quadIsFlying mode might cause issues with the yaw estimate?

My proposal is to delete the logic mentioned above and have one unified code path, independent of quadIsFlying. Alternatively, we could use a proper model for the commanded thrust (as we can compute with the system ID deck) and use that rather than acc.z.

[jpreiss]

In KalmanUSC we always used the full IMU data. We didn't have anything like quadIsFlying.

Re. the first quadIsFlying block: I do not have much intuition. If we were to treat accelerometer as a measurement, not an input, then it seems reasonable that ignoring the x and y components of the accelerometer would degrade the yaw estimate because those components provide information about the direction of the gravity vector. However, it's not clear to me whether or not the EKF can still benefit from the information in the x and y components in our setup, where we treat the accelerometer as an input.

If the attitude estimate is always perfect, and we pretend the body is perfectly rigid and blade flapping does not happen, then it's true that the x and y components of the acceleration in the body frame should always be zero. Therefore we could assume (as the current code appears to do) that they are only noise and should be ignored. However, it is not clear to me that this is still a good thing to do when the attitude estimate is not perfect.

Re. the second quadIsFlying block: I recall KalmanUSC drifting when not flying for a long time, like sitting in sysOff mode for 15 minutes. The drift was in the whole attitude, not just yaw. We noticed that the complementary filter does not drift in this condition because it essentially assumes that the accelerometer measures gravity plus noise. KalmanUSC did not make this assumption, so it could support constant nonzero acceleration, but this means it also integrated the accelerometer bias when stationary.

It looks like this block is trying to make the EKF behave more like the complementary filter when the quad is sitting on the ground. This is (IMO) a good goal. However, it is always reverting towards initialQuaternion which is determined by the param initialYaw only once at system startup. If this code were enabled during flight, I can imagine it might boost performance when hovering at initialYaw, but it could cause problems when hovering at any yaw other than initialYaw for a long time. @whoenig did you test that?

If we want to apply such a correction during flight, It might be more correct to rotate towards the nearest quaternion with roll/pitch of zero but allow the yaw to drift. Technically even this is wrong, since we might hold a fixed nonzero roll/pitch attitude for a long time if we were flying long distances outdoors.

[whoenig]

Thanks for the detailed comments! I commented out the correction (

crazyflie-firmware/src/modules/src/kalman_core/kalman_core.c

Lines 553 to 560 in 0787950

	if (! quadIsFlying) {
	float keep = 1.0f - ROLLPITCH_ZERO_REVERSION;
	tmpq0 = keep * tmpq0 + ROLLPITCH_ZERO_REVERSION * initialQuaternion[0];
	tmpq1 = keep * tmpq1 + ROLLPITCH_ZERO_REVERSION * initialQuaternion[1];
	tmpq2 = keep * tmpq2 + ROLLPITCH_ZERO_REVERSION * initialQuaternion[2];
	tmpq3 = keep * tmpq3 + ROLLPITCH_ZERO_REVERSION * initialQuaternion[3];
	}

) in all my tests. The difference in yaw-tracking behavior can be reproduced just by focusing on the the quadIsFlying blocks. However, I did reset the kalman filter always prior to takeoff, which might have prevented me from seeing strange drifting in roll/pitch. I also only did the hover test (where there are micro-movements in x/y). I didn't do any test where the Crazyflie is just sitting on the ground. The offline analysis with the Python bindings used a simple circle trajectory.

[jpreiss]

So your test was equivalent to setting quadIsFlying to false for the first block but true for the second block? Just want to make sure I understand.

[jpreiss]

FYI, the zacc "trick" goes back to the original EKF commit fc88c6a. I couldn't find a PR or other discussion for this commit. Perhaps it is discussed in the papers they cite.

[jonasdn]

So, @whoenig could you explain the state of this like I was a 38-year-old engineer with no control theory background?

[whoenig]

• The current EKF works terribly when providing position-only information, a use-case that is very common for the Crazyswarm.
• It remains unclear if that is caused by a wrong implementation or by wrong math (mostly because we couldn't find a justification for the current implementation in any of the cited papers.)
• A fix is described in the initial post, but it requires more testing with different modes (lighthouse, LPS, etc.) to ensure that it doesn't cause a regression somewhere else.
• We could either run these tests manually, or we all push very hard for the Python bindings, so that we can run regression tests in software simulation:-)

[matejkarasek]

Hi,
I am looking into the EKF myself these days and so tried to replicate the above.
I am testing this with a CF2.1 (master) and the LPS system (so I have no pose estimation, as with single marker tracking).
However, for me no (major) yaw error that would be causing instability is occurring even after 1-2 minutes... @whoenig What is your test setup when you observe this?

As for the implementation, I have tried both the original code as well as when commenting out the lines suggested by @whoenig. While both work fine for me with respect to yaw, the implemenation with all the accelerations (so not just zacc) makes the crazyflie subjectively more "nervous" in terms of position control (especially with TDoA):

crazyflie-firmware/src/modules/src/kalman_core/kalman_core.c

Lines 506 to 509 in 7a0ed44

	// position updates in the body frame (will be rotated to inertial frame)
	dx = this->S[KC_STATE_PX] * dt + acc->x * dt2 / 2.0f;
	dy = this->S[KC_STATE_PY] * dt + acc->y * dt2 / 2.0f;
	dz = this->S[KC_STATE_PZ] * dt + acc->z * dt2 / 2.0f; // thrust can only be produced in the body's Z direction

crazyflie-firmware/src/modules/src/kalman_core/kalman_core.c

Lines 521 to 524 in 7a0ed44

	// body-velocity update: accelerometers - gyros cross velocity - gravity in body frame
	this->S[KC_STATE_PX] += dt * (acc->x + gyro->z * tmpSPY - gyro->y * tmpSPZ - GRAVITY_MAGNITUDE * this->R[2][0]);
	this->S[KC_STATE_PY] += dt * (acc->y - gyro->z * tmpSPX + gyro->x * tmpSPZ - GRAVITY_MAGNITUDE * this->R[2][1]);
	this->S[KC_STATE_PZ] += dt * (acc->z + gyro->y * tmpSPX - gyro->x * tmpSPY - GRAVITY_MAGNITUDE * this->R[2][2]);

However, it seems to me this implementation is "more correct". The original (only zacc) uses the z-axis acceleration as an input representing thrust. But this means any other aerodynamic forces (lift and drag of the structure and props) is neglected. Including all the accelerations accounts for all the forces acting on the crazyflie (see equations 9 and 15 of Mueller 2015).

Interestingly, the "nervousness" can be to a greater deal removed (again subjectively) by using the "robust" implementation of TWR & TDoA, which seems to improve the performance in general.

I am personally interested in including some terms that can capture aerodynamic forces other than thrust, as otherwise the attitude of our Bolt-based flapping-wing drones does not get estimated correctly in forward flight... And so this could be the easiest and most generic way to go...

Will try to do some more testing with the Lighthouse and also with the flapping-wing...

[whoenig]

My setup is a motion capture system with a single marker on top (i.e., the EKF fuses position and IMU data). I fly a simple figure 8 and can observe the drift very quickly after a few seconds. The drifting gets worse over time.

Lighthouse has some "hacked" yaw-compensation when flying in the crossing-beam method. For the regular EKF fusion, the current filter works OK for me.

Tu fully implement Mueller 2015, we need the forward model of the quadrotor (i.e. PWM-> force; easy to obtain for CF2.1, but might be difficult for Bolt or flapping-wing).

[matejkarasek]

I see, so when simply hovering there is no drift, only with the figure 8 pattern?
My setup is slightly different (EKF fuses distance/TDoA and IMU), though for yaw it should be the same, as only the position state gets updated...

The implementation of the drag model would "just" need to measure/estimate motor rpm, and then measure the thrust curve and measure/estimate the drag coefficients... Not impossible, even for a flapping wing :) But the accelerometer is in principle measuring all that already, isn't it?

[whoenig]

Hovering also causes a drift, but this is "expected" (since yaw is not observable if x/y are not changing).

Yes, the accelerometer captures everything, although with lots of noise and time delays. It might be interesting to fuse the acc-data directly as measurement.

Slightly off-topic: the Python bindings are making progress and I have used prototypes of them in the past for tuning the EKF. If we have good datasets, this would be much easier than tuning + flight tests.

[matejkarasek]

Ok, I see... Well in my tests I was simply hovering and no problem observed after 2 minutes.... But it could be that with LPS the xy oscillations are enough to make yaw observable again... But that wouldn't explain why the figure 8 is problematic in your case...

FYI, with a CF2 in a Lighthouse keeping all the accelerometer terms in the position & velocity updates did not seem to have any adverse effect.

For a flapping wing in LPS, however, the high accelerometer "noise" (actually real vibrations) makes it perform much much worse than with only accz. So if we make some changes, I would opt for making them optional :) Maybe indeed treating the accelerometer as measurement could help to some extent...

[knmcguire]

Perhaps what is suggested in #981 can be a solution to this?

[whoenig]

I think #981 can certainly help for tuning, but is independent of this issue.

[knmcguire]

Seems like people are hitting this issue again. Let's check if this is still the case https://github.com/orgs/bitcraze/discussions/1321