Understanding `MC_YAW_WEIGHT`

[profxavr]

Hello! Do any of you guys have an understanding of how the parameter MC_YAW_WEIGHT impacts multicopter flight? I was reading through the code where it was implemented in AttitudeControl.cpp:

void AttitudeControl::setProportionalGain(const matrix::Vector3f &proportional_gain, const float yaw_weight)
{
    _proportional_gain = proportional_gain;
    _yaw_w = math::constrain(yaw_weight, 0.f, 1.f);

    // compensate for the effect of the yaw weight rescaling the output
    if (_yaw_w > 1e-4f) {
        _proportional_gain(2) /= _yaw_w;
    }
}

matrix::Vector3f AttitudeControl::update(const Quatf &q) const
{
    Quatf qd = _attitude_setpoint_q;

    // calculate reduced desired attitude neglecting vehicle's yaw to prioritize roll and pitch
    const Vector3f e_z = q.dcm_z();
    const Vector3f e_z_d = qd.dcm_z();
    Quatf qd_red(e_z, e_z_d);

    if (fabsf(qd_red(1)) > (1.f - 1e-5f) || fabsf(qd_red(2)) > (1.f - 1e-5f)) {
        // In the infinitesimal corner case where the vehicle and thrust have the completely opposite direction,
        // full attitude control anyways generates no yaw input and directly takes the combination of
        // roll and pitch leading to the correct desired yaw. Ignoring this case would still be totally safe and stable.
        qd_red = qd;

    } else {
        // transform rotation from current to desired thrust vector into a world frame reduced desired attitude
        qd_red *= q;
    }

    // mix full and reduced desired attitude
    Quatf q_mix = qd_red.inversed() * qd;
    q_mix.canonicalize();
    // catch numerical problems with the domain of acosf and asinf
    q_mix(0) = math::constrain(q_mix(0), -1.f, 1.f);
    q_mix(3) = math::constrain(q_mix(3), -1.f, 1.f);
    qd = qd_red * Quatf(cosf(_yaw_w * acosf(q_mix(0))), 0, 0, sinf(_yaw_w * asinf(q_mix(3))));

    // quaternion attitude control law, qe is rotation from q to qd
    const Quatf qe = q.inversed() * qd;

    // using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle)
    // also taking care of the antipodal unit quaternion ambiguity
    const Vector3f eq = 2.f * qe.canonical().imag();

    // calculate angular rates setpoint
    matrix::Vector3f rate_setpoint = eq.emult(_proportional_gain);

    // Feed forward the yaw setpoint rate.
    // yawspeed_setpoint is the feed forward commanded rotation around the world z-axis,
    // but we need to apply it in the body frame (because _rates_sp is expressed in the body frame).
    // Therefore we infer the world z-axis (expressed in the body frame) by taking the last column of R.transposed (== q.inversed)
    // and multiply it by the yaw setpoint rate (yawspeed_setpoint).
    // This yields a vector representing the commanded rotatation around the world z-axis expressed in the body frame
    // such that it can be added to the rates setpoint.
    if (is_finite(_yawspeed_setpoint)) {
        rate_setpoint += q.inversed().dcm_z() * _yawspeed_setpoint;
    }

    // limit rates
    for (int i = 0; i < 3; i++) {
        rate_setpoint(i) = math::constrain(rate_setpoint(i), -_rate_limit(i), _rate_limit(i));
    }

    return rate_setpoint;
}

I’m trying to understand how the yaw weight impacts things here. It looks like it just shortens the yaw setpoint demanded by _yaw_w, so if the yaw weight is .4 and we were going to ask for a yaw heading 10deg away, we are now using a yaw heading 4deg away instead. This would slow the rate at which yaw setpoints are achieved because it would ultimately decrease the error, and therefore decrease the yawrate commanded. To ffset this, the yaw proportional gain is scaled by 1 / _yaw_w so the rate setpoint would ultimately calculate the same setpoint.

IF this is the case, and I am understanding it correctly, this just cancels out the effect of the yaw weight. If so, what exactly is the MC_YAW_WEIGHT parameter supposed to do?

If I am misunderstanding, please explain! Ty

@mbresch

[JulianOes]

For reference: Parameter Reference | PX4 Guide (v1.15)

I assume you have already read that.

[profxavr]

Yes I have. I am mainly inquiring because we are having trouble with yaw tracking, and I was trying to see if we needed to increase the yaw priority. I realized then that I didn’t understand how the code was attempting to implement this.

[bresch]

I tried to clarify it a bit here: MC att: clarify prioritization algorithm by bresch · Pull Request #24231 · PX4/PX4-Autopilot · GitHub

We’re actually not just limiting the yaw rate. The problem is that if we take the full attitude error, the shortest trajectory can be a combined roll, pitch and yaw motion.
We know that the actuation on the yaw axis of the drone is weaker, so we don’t want to rely on yaw actuation to orient the thrust vector. By scaling down the angular error around the body Z axis before computing the attitude error, we “force” the controller to mainly drive roll and pitch to obtain the desired thrust vector.

[profxavr]

@bresch

By scaling down the angular error around the body Z axis before computing the attitude error, we “force” the controller to mainly drive roll and pitch to obtain the desired thrust vector.

This bit made sense to me… you scale down the yaw error signal that WOULD be multiplied by the gains to generate the yawspeed setpoint. So you get a smaller value than you would have otherwise, limiting the focus on yaw.

However… the gain used to multiply the yaw error is itself scaled by 1 / _yaw_w in AttitudeControl::setProportionalGain(...). Does that not just cancel out the effect?

What I am seeing is:

yawspeed_setpoint = (yaw_p_gain / yaw_w) * (yaw_error * yaw_w) = yaw_p_gain * yaw_error

Am I misunderstanding?

[bresch]

It’s not only about the gain. What we try to modify here is the trajectory between two attitudes.
I’ll use this simulation to illustrate a bit.
Here is the trajectory of a multirotor flying from right to left starting with a yaw of 0 a desired final yaw of 90degrees.

Case 1:
yaw weight = 1, perfect roll/pitch/yaw actuation
The drone flies in a straight line and ends up with the correct rotation

Case 2:
yaw weight = 1, yaw actuation reduced by 80% compared to roll/pitch
The drone ends up with the correct rotation but does not fly a straight line because the controller assumes that it can follow the shortest rotation by applying any combined 3D motion.

Case 3:
yaw weight = 0.1, yaw actuation reduced by 80% compared to roll/pitch
The controller knows that the yaw is less actuated, so the thrust is mainly controlled by applying roll/pitch rotations and yaw is slowly reached. The drone then flies a straight line.

I hope that helped!

[profxavr]

@bresch

hmm… this makes sense as the intent of what is happening, but I think I am still confused about how the code reflects this action?

Working through the quaternion math, I see how we are “shortening” the expected rotation around just the yaw axis. This would make the yaw get reached more slowly to allow for full authority on the other axes. But it still seems as though that is undone?

---

I will walk through my understanding of the code:

void AttitudeControl::setProportionalGain(const matrix::Vector3f &proportional_gain, const float yaw_weight)
{
	_proportional_gain = proportional_gain;
	_yaw_w = math::constrain(yaw_weight, 0.f, 1.f);

	// compensate for the effect of the yaw weight rescaling the output
	if (_yaw_w > 1e-4f) {
		_proportional_gain(2) /= _yaw_w;
	}
}

This sets the proportional gains. Notably, it modifies the yaw gain by MC_YAW_WEIGHT. This makes _proportional_gain(2) equal to MC_YAW_P / MC_YAW_WEIGHT.

Now inside

matrix::Vector3f AttitudeControl::update(const Quatf &q) const
{
  Quatf qd = _attitude_setpoint_q;
  
  // calculate reduced desired attitude neglecting vehicle's yaw to prioritize roll and pitch
  const Vector3f e_z = q.dcm_z();
  const Vector3f e_z_d = qd.dcm_z();
  Quatf qd_red(e_z, e_z_d);

During this part we get the quaternion representation of the current craft attitude, q, and the desired attitude, qd. It builds qd_red as the attitude difference between the two attitudes, neglecting the yaw component. It does this by finding the attitude difference between the z-axes of the current and desired orientations.

  if (fabsf(qd_red(1)) > (1.f - 1e-5f) || fabsf(qd_red(2)) > (1.f - 1e-5f)) {
    // In the infinitesimal corner case where the vehicle and thrust have the completely opposite direction,
    // full attitude control anyways generates no yaw input and directly takes the combination of
    // roll and pitch leading to the correct desired yaw. Ignoring this case would still be totally safe and stable.
    qd_red = qd;

  } else {
    // transform rotation from current to desired thrust vector into a world frame reduced desired attitude
    qd_red *= q;
  }

This bit just checks a weird edge case. In the normal case this sets qd_red = qd_red * q which makes qd_red the orientation between qd and q that represents the yawless attitude setpoint.

  // mix full and reduced desired attitude
  Quatf q_mix = qd_red.inversed() * qd;
  q_mix.canonicalize();
  // catch numerical problems with the domain of acosf and asinf
  q_mix(0) = math::constrain(q_mix(0), -1.f, 1.f);
  q_mix(3) = math::constrain(q_mix(3), -1.f, 1.f);

This creates q_mix (which i agree with you is oddly named). q_mix represents the rotation that is the “yaw delta” to go to get from qd_red to qd. Note: it is calculated as a local rotation difference instead of a global rotation difference, so it must right multiply qd_red as its only transformation option. I like your recommendation that q_mix should be like q_yaw_delta or something.

  qd = qd_red * Quatf(cosf(_yaw_w * acosf(q_mix(0))), 0, 0, sinf(_yaw_w * asinf(q_mix(3))));

Here, we reconstruct qd by right multiplying qd_red with a weighted version of the demanded yaw. This is what rotates us “part of the way.”

If MC_YAW_WEIGHT is 1, then this value just reapplies all of the yaw in q_mix getting back to the original qd.

If MC_YAW_WEIGHT is 0, then this value applies the identity matrix to qd_red and the yawless value is used as the setpoint.

  // quaternion attitude control law, qe is rotation from q to qd
  const Quatf qe = q.inversed() * qd;

This simply takes the quaternion delta again… kind of. I would expect this to be
const Quatf qe = qd * q.inversed(); so you get the tangent vector in the global frame. This one does it in the local frame. Given this restriction, qe can only ever be used to right multiply q to retrieve qd. But that doesn’t matter, we won’t be using it like that. The main point is that it represents the attitude difference between the new, yaw-weighted qd and q. We would have started here were we not doing all of the yaw-weighting.

  // using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle)
  // also taking care of the antipodal unit quaternion ambiguity
  const Vector3f eq = 2.f * qe.canonical().imag();
  
  // calculate angular rates setpoint
  matrix::Vector3f rate_setpoint = eq.emult(_proportional_gain);

This bit creates a vector, eq from the qe’s imaginary parts and scales it by 2. This is a small angle approximation of the Logmap of q. I wonder if the full calculation was too costly?

The intent here is to be to convert eq into its euler angle equivalents via this equation:

It then constructs the rate_setpoints by multiplying the euler angle delta by the rate gains.

  // Feed forward the yaw setpoint rate.
  // yawspeed_setpoint is the feed forward commanded rotation around the world z-axis,
  // but we need to apply it in the body frame (because _rates_sp is expressed in the body frame).
  // Therefore we infer the world z-axis (expressed in the body frame) by taking the last column of R.transposed (== q.inversed)
  // and multiply it by the yaw setpoint rate (yawspeed_setpoint).
  // This yields a vector representing the commanded rotatation around the world z-axis expressed in the body frame
  // such that it can be added to the rates setpoint.
  if (is_finite(_yawspeed_setpoint)) {
    rate_setpoint += q.inversed().dcm_z() * _yawspeed_setpoint;
  }

  // limit rates
  for (int i = 0; i < 3; i++) {
    rate_setpoint(i) = math::constrain(rate_setpoint(i), -_rate_limit(i), _rate_limit(i));
  }

  return rate_setpoint;
}

This last bit just adds in the feedforward setpoint for the yawspeed.

---

In this case I am still thinking that the work of MC_YAW_WEIGHT is undone by upscaling MC_YAW_P by the same amount. The error signal, the yaw delta between qd and q is scaled down by MC_YAW_WEIGHT through the mechanisms outlined above. Then 1 / MC_YAW_WEIGHT is reapplied as a gain when calculating the rate setpoint. I believe this will have undone that work.

A simple example:

• q: RPY[0, 0, 0]
• qd: RPY[5, 5, 5]
• qd_red: RPY[5, 5, 0]
• q_mix: Yaw command of 5deg
• MC_YAW_WEIGHT: .5
• qd_new: RPY[5, 5, 2.5]
• qe: RPY[5, 5, 2.5]
• rates_setpoint: RPY[2 * MC_ROLL_P * 5, 2 * MC_PITCH_P * 5, 2 * MC_YAW_P / MC_YAW_WEIGHT * 2.5]
  • Let’s say all of the P gains are .1 for simplicity. this gets: rates_setpoint: RPY[1.0, 1.0, 1.0]

Which is exactly what we would have gotten if we had done nothing. This also makes it seem to me that this entire method of weighting yaw is functionally equivalent to reducing MC_YAW_P? If I think about the trajectory the craft carves through SO(3), it doesn’t take the shortest distance to get places, it takes whatever path is deduced from the gains. Like if MC_YAW_P is just very small, then yaw setpoints will be reached very slowly because the generated yawrate setpoint is small.

Given how confused I have been about other stuff, I may still be confused, so pardon the long-winded question!

---

edit marathon explanation: I learned a lot about quaternions during this process. it took me a bit to understand all that was being done here. i’m leaving all of the pictures and whatnot in case others are wanting to understand how it works.

[bresch]

@profxavr

Note: it is calculated as a local rotation difference instead of a global rotation difference

Just for clarity, it’s computing the “global rotation difference” (both z vectors are expressed in the global frame), this is why it needs to be right-multiplied by the attitude.

@profxavr

 // quaternion attitude control law, qe is rotation from q to qd
  const Quatf qe = q.inversed() * qd;

Here is the important part: the “tilt” part of this “quaternion error” depends on MC_YAW_WEIGHT; you don’t get the same desired roll and pitch rates with different yaw weights.
There are many trajectories you can follow to move from an attitude to another one: the more direct one is obtained when yaw weight is 1. The other extreme case would be to set the yaw weight to 0 and add a separate controller for the yaw axis. Here, we have a blend of the two cases.

@profxavr

I would expect this to be
const Quatf qe = qd * q.inversed(); so you get the tangent vector in the global frame

We need a body rate setpoint for the rate controller, this is why we take the “local error”. This tangent 3D vector is really just the delta rotations around the x, y and z body axes.

@profxavr

This is a small angle approximation of the Logmap of q. I wonder if the full calculation was too costly?

It’s a bit more costly to do, yes, and it doesn’t affect stability.

@profxavr

It then constructs the rate_setpoints by multiplying the euler angle delta by the rate gains.

Not delta euler angles but delta angles around each axis of the current attitude.

[profxavr]

@bresch

Just for clarity, it’s computing the “global rotation difference” (both z vectors are expressed in the global frame), this is why it needs to be right-multiplied by the attitude.

I think here I was referring to q_mix being calculated as a local difference. I shouldn’t have said “instead” it is just correct to do it this way.

@bresch

We need a body rate setpoint for the rate controller, this is why we take the “local error”. This tangent 3D vector is really just the delta rotations around the x, y and z body axes.

Oh right. Duh. It is motivated to use the body rate vector. Makes sense.

@bresch

Not delta euler angles but delta angles around each axis of the current attitude.

Okay, yes. Local attitude rotations, not euler angles.

@bresch

Here is the important part: the “tilt” part of this “quaternion error” depends on MC_YAW_WEIGHT; you don’t get the same desired roll and pitch rates with different yaw weights.

Ah. I understand now. It is because the affected rate setpoints are local setpoints. I was thinking of them as tho they were global. Okay this makes sense now. I will mark it as resolved.