Yaw bug in MC attitude controller

[bkueng]

Observed behavior: when doing a fast roll/pitch change at high tilt angle (60+ degrees), the yaw goes off (estimation & actual observed vehicle yaw) by an arbitrary large amount (I've seen 90 and 180 degrees). Generally the faster the stick motion, the faster the yaw goes off. I've seen this now in HIL and on 2 different vehicles.

When centering the roll/pitch stick again, the yaw quickly recovers to the correct angle. If the stick stays at large tilt angle without moving it, the yaw also recovers, but only very slowly (in the order of seconds).

• https://logs.px4.io/plot_app?log=5b613a54-e4f4-481c-ae78-1bed66e9fd24 time 2:55: the yaw angle setpoint stays 0 (which is correct, as the RC yaw is 0) but the estimated angle goes off by a large amount. Looking at the yaw rates: the setpoint increases, and the vehicle tracks it correctly, resulting in the observed yaw change.
• https://logs.px4.io/plot_app?log=2e647094-96e6-4a36-9b6a-1e93b57ac147 time 2:28-2:32: same as above: yaw setpoint stays 0, and rate is high and tracked correctly. The vehicle does a full 360 degrees yaw rotation.

The estimated yaw angle seems correct (since I see the vehicle actually yawing), and I assume the reported rates are correct as well. This makes me conclude that there is a bug in the attitude controller, outputting wrong rate setpoints (and since the rate is tracked correctly, it's not a mixer limit issue).

Steps to reproduce:

• use HIL or a real vehicle (should be reproducible in SITL as well though)
• increase MC_PITCHRATE_MAX and MC_ROLLRATE_MAX (I set them to 1000)
• increase MPC_MAN_TILT_MAX to 75 (or higher). The higher, the stronger the effect will be
• fly at max/very high tilt in any direction, then change roll/pitch quickly. Eg. move the stick to the right, then quickly move it circular towards each corner. Throttle does not matter for this, it can stay in the center, yaw stick stays 0.

@MaEtUgR @julianoes fyi

[tops4u]

@bkueng could this be related to #7941 ?

[bkueng]

I spent some effort analyzing this and found several bugs. 2 of them (the slow yaw convergence at high tilt is one) are fixed in #7997.

There's one more thing remaining: large attitude oscillations at high tilt angles and fast roll-pitch motions. I traced it down to the roll-pitch correction in case there is a yaw error present (https://github.com/PX4/Firmware/blob/master/src/modules/mc_pos_control/mc_pos_control_main.cpp#L2872:L2908). This change got introduced in https://github.com/PX4/Firmware/pull/4564.

Disabling this makes the oscillations go away, the behavior is much better, as the following log shows: https://logs.px4.io/plot_app?log=b290c1d5-8989-402e-86ec-121535b7b4f8 It's a test in HIL with 70 deg max tilt angle, and 4 times the same stick motion (not yawing): move roll/pitch to the left upper corner, let the vehicle stabilize, then move it very fast to the right upper corner and wait until stabilized. The first 2 times: roll-pitch correction enabled, then disabled. Observation: severe oscillations when enabled, much better when disabled. These are the relevant plots:

I'm not entirely clear why it happens but here are some likely explanations:

• there's a feedback loop: roll & pitch are changed based on the yaw error (this is the correction), but changing the roll & pitch can lead to necessary yaw corrections as well (the setpoint is the same but the yaw rate goes up, at high tilt angles). Or in other words: the yaw error can increase. Which then in turn requires a different correction for roll and pitch, which closes the loop. This makes the whole design very complex, and hard to reason about. And I think especially the manual mode needs to be very simple.
• The correction itself helps, but it's not perfect: I tested in HIL with a roll angle of 80 deg, 0 pitch, and started to yaw. The yaw error increased to ~90 degrees (which is ok), and the pitch was not 0 anymore, but about 10-20 degrees off (should stay 0).

In light of these points (especially the first), I suggest disabling this correction, at least for MC, and we can keep if for VTOL's. However it should be re-evaluated if it's needed since yaw tracking is better now. Ideally this should be solved inside the attitude controller, not built around it.

Here are some ideas that could help improve the existing algorithm (but they don't break the feedback loop):

• lowpass filter the yaw error used for the correction
• only apply the correction if the yaw error becomes very large (e.g. >30deg), and probably use a weight by how much the correction is applied to make a smooth transition.

@AndreasAntener @tumbili @MaEtUgR @Stifael @bresch @sanderux @LorenzMeier @julianoes what do you think?

[MaEtUgR]

I want to port the existing implementation of quaternion based attitude control into mc_att_control as soon as the quaternion convention https://github.com/PX4/Firmware/pull/7908 is finally merged. I would suggest to compare your observations of the remaining yaw issue with that solution afterwards. Chances are that there is some not easy to see rotation order error in there right now because there are multiple error prone euler calculations like https://github.com/PX4/Firmware/blob/master/src/modules/mc_pos_control/mc_pos_control_main.cpp#L2886 in there now.

[MaEtUgR]

After a discussion with @bkueng I agree that the original problems with VTOL yaw tracking and hence invisible attitude setpoint heading should have a different solution than the apparently "hacky", retrospective

roll-pitch correction in case there is a yaw error present

[RomanBapst]

@bkueng Sorry for the late reply. I had a chance to quickly glance into this. What I noticed is that the yaw setpoint in the logs is not continuous anymore but jumps between two samples. I assume this behaviour was introduced with the roll/pitch input mapping https://github.com/PX4/Firmware/commit/55bd35cba687d9ac085056c3aa00e75386862eb0

I don't think that the attitude modifications due to large yaw errors implemented in #4564 is supposed to work with this new modification. The yaw error will not be continuous just like the yaw setpoint. At the moment I don't quite see through this any more, since it has changed a lot.

Regarding the oscillations in yaw: Could it be that the step in the yaw setpoint make the oscillations visible?

[bkueng]

What I noticed is that the yaw setpoint in the logs is not continuous anymore but jumps between two samples

Can you point me to a log? I think what you see is just a jump, introduced by a fast roll/pitch input change, but I'm only guessing here. It's correct that the yaw setpoint changes now due to roll/pitch input change - the discussion is here: https://github.com/PX4/Firmware/pull/7940.

I don't think that the attitude modifications due to large yaw errors implemented in #4564 is supposed to work with this new modification.

I did this analysis before the roll/pitch input modification, so it definitely existed before as well. It's just not visible if you flew only with low tilt angles & small maximum roll/pitch rates. The problem I see is that roll/pitch changes will result in changes of the yaw error as well. This also happened even before https://github.com/PX4/Firmware/pull/7940, without changing the yaw setpoint: the vehicle does its corrections in body frame, but the yaw is in world frame. For example, imagine the vehicile is at a 45deg roll angle, and it does a change in pitch angle: without yaw correction, this leads to an increase of yaw error, thus the vehicle needs to correct the (local) yaw as well, to keep the global yaw setpoint constant. This leads to a change in yaw error. Does this make sense?

[RomanBapst]

The problem I see is that roll/pitch changes will result in changes of the yaw error as well. This also happened even before #7940, without changing the yaw setpoint: the vehicle does its corrections in body frame, but the yaw is in world frame. For example, imagine the vehicile is at a 45deg roll angle, and it does a change in pitch angle: without yaw correction, this leads to an increase of yaw error, thus the vehicle needs to correct the (local) yaw as well, to keep the global yaw setpoint constant. This leads to a change in yaw error. Does this make sense?

I think we probably mean the same thing, let me formulate it in my words. Since we are using a 3-2-1 sequence of the Tait Bryant angles any change in roll / pitch should not affect the yaw setpoint of the vehicle. This is because the rotation is created by first rotating to the desired heading, followed by pitching the nose to the desired pitch and then rolling (which does not affect the heading). So if you have the vehicle rolled at 45 degrees and you apply a pitch stick input, in theory I would expect the vehicle to bring the nose up without changing yaw. This motion however, can only be accomplished by a combination of body pitch- and body yaw rate. This is where there might be a problem.

[bkueng]

Since we are using a 3-2-1 sequence of the Tait Bryant angles any change in roll / pitch should not affect the yaw setpoint of the vehicle. This is because the rotation is created by first rotating to the desired heading, followed by pitching the nose to the desired pitch and then rolling (which does not affect the heading).

Exactly (well, except that you first apply roll, then pitch, then heading. But it's correct that roll & pitch will not affect the heading). After #7940 it's a bit more complicated, because the roll/pitch stick input is not directly mapped to roll & pitch angles, but is interpreted as the desired thrust direction (or tilt angle & direction of maximum tilt), and this affects the yaw setpoint as well.

So if you have the vehicle rolled at 45 degrees and you apply a pitch stick input, in theory I would expect the vehicle to bring the nose up without changing yaw. This motion however, can only be accomplished by a combination of body pitch- and body yaw rate. This is where there might be a problem.

Yes, this is precisely what I mean.

[RomanBapst]

@bkueng Ok, thanks for getting back on this! In the end I think it's important to have what feels best for the user and obviously you have been doing lots of testing. Having taken a closer look at the new method for attitude setpoint generation the code which used to handle the yaw error does not make any sense any more. I think it should be removed and decided if compensation for large yaw errors is still needed (e.g. for large VTOLS) or not.

[bkueng]

Ok, cool. I already have a PR here: https://github.com/PX4/Firmware/pull/8725. Since I have no experience with VTOL's I left them unchanged.