AC_AttitudeControl:There seems to be an error in the calculation of the desired_ang_vel_quat variable in the attitude_controller_run_quat function

[BreederBai]

As shown in the figure above, there seems to be a problem with the coordinate system mapping of the _attitude_target_ang_vel variable. According to my understanding， Quaternion desired_ang_vel_quat = to_to_from_quat.inverse() * attitude_target_ang_vel_quat * to_to_from_quat; should be written as Quaternion desired_ang_vel_quat = to_to_from_quat * attitude_target_ang_vel_quat * to_to_from_quat.inverse() ;

[rmackay9]

Thanks for the report. I'll leave this one to @lthall to respond to I think..

[Rajeta-Sarkar]

Hi I'm Rajeta and I am interested in working in this issue, how should I proceed?

[lthall]

There seems to be an error or there IS an error.

@Rajeta-Sarkar The first thing we need to do is work out if there really is an error or if @BreederBai doesn't understand what the algorithm is doing. This may be producing only a small error that doesn't cause a massive problem so we didn't notice it.

Unfortunately we often get little snippets of "this looks wrong" on the issues but with no indication as to why it is wrong or how they have established it is wrong. I suspect I spend more time checking this stuff to find out that it is right than the person spent thinking about it before saying it was wrong......

[esaldiran]

Hi @lthall,

I was happened to check the formulation of the mention lines recently. I arrived at two different conclusions.

At first, I will try to establish the way quaternion notation calculation performed in Ardupilot.

To rotate a vector from local(body) to inertial frame following rotation performed on the vector vb. vi = qvbq^-1

To perform two successive rotations with quaternion defined in the inertial frame. q = q2*q1 where both q1 and q2 defined in the inertial frame.

To perform rotation q3 in q1 frame(local)
q = q1*q3 where q1 defined in the inertial frame, but q3 defined in the local frame q1.

Now, this is out of the way. We can look at the formulation.

This line calculates rotation from the vehicle frame and the target frame and it is in vehicle frame, hence the left to right multiplication with attitude_vehicle_quat. to_to_from_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat

Using the inverse of to_to_from_quat to rotate a vector from target to vehicle frame seems right.

However, if we rotate the vector defined in the target frame to the inertial frame and then rotate the new vector to the vehicle frame we obtain a different result.

First rotate vector defined in target frame to the inertial frame, where vt is vector defined in target frame and vi is a vector defined in the inertial frame. vi = _attitude_target_quat vt _attitude_target_quat^-1 (1)

now rotate this vector back into the vehicle frame, where vv is the vector defined in the vehicle frame. vv = attitude_vehicle_quat^-1 vi attitude_vehicle_quat (2)

if you substitute (1) into (2), vv = attitude_vehicle_quat^-1 _attitude_target_quat vt _attitude_target_quat^-1 attitude_vehicle_quat vv = to_to_from_quat vt to_to_from_quat^-1 which contradicts with source code.

I also run a quick SITL test with Callisto frame, did not noticed any difference. At the left is the current formulation, at the right is new formulation.

[BreederBai]

Hi @esaldiran, Your analysis is really great！This is also the reason why I think this code is problematic.Because my English is so bad, I can only rely on Google Translate. I can't express my reasoning clearly.Thank you for your supplement to this question and learn from you!

[lthall]

@esaldiran Thank you very much for your work! This is very helpful.

@esaldiran @BreederBai @Rajeta-Sarkar @Hs293Go I would love to get a group of people together that are capable of reading and understanding this code and not only checking it like this but also improving it.

When I wrote this it was my introduction to Quaternions and it would not surprise me if there was a little mistake here. The tricky thing is these mistakes may not be noticeable in a log unless you excite things in just the right way.

I can obviously tell you exactly what the code is supposed to be trying to do and I do my best to make the variable names and the comments explain what is going on but that can always be improved with feedback to make it easier for the rest of you to understand.

@esaldiran so do you think that @BreederBai is correct?

[lthall]

Any suggestions on naming those variable better would also be great!

[lthall]

Ok, I have spent some time going over this again and I think there is a problem but in the calculation above here: Quaternion to_to_from_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat;

It looks to me like it should be this: Quaternion to_to_from_quat = attitude_vehicle_quat * _attitude_target_quat.inverse();

I read this as moving from the target back to the origin then from the origin to the current vehicle orientation.

This would mean that when we are dealing with large attitude errors the corrections would be rotated away from the body rather than towards it.

@BreederBai and @esaldiran what do you think?

[BreederBai]

@lthall This change should also be correct. I think which way to fix this problem depends on the definition of "to_to_from_quat". Your change method means that "to_to_from_quat" means the rotation of "_attitude_target_quat" -> "attitude_vehicle_quat".When you define the "to_to_from_quat" variable, what does it mean?

[lthall]

It is exactly what you describe here: I should probably change it to target_to_attitude_quat.

Could you take a little time to suggest things we can do to make this clearer for you?

[BreederBai]

@lthall In the current attitude controller, for the angular rate represented by the rotation vector, such as _attitude_target_ang_vel, _rate_target_ang_vel, (desired_ang_vel_quat.x, desired_ang_vel_quat.x, desired_ang_vel_quat.x), the actual reference coordinate system is different. My thought is Reflect this difference through comments or in variable names. Similar to what you said, change to_to_from_quat to target_to_attitude_quat. _rate_target_ang_vel was changed to _target_to_attitude_rate_ang_vel, _attitude_target_ang_vel was changed to _attitude_target_to_desired_ang_vel, and desired_ang_vel_quat was changed to _attitude_to_desired_ang_vel_quat. Among them, "desired" means the target posture calculated by the remote control input. But with this change, the name of the variable seems a bit too long. There is a trade-off between variable name length and clear meaning, which requires you to make the final decision.

[esaldiran]

@lthall I have done further analysis I found a way to create a scenario where the effect of this change can be seen.

I have changed the following parameters. The rest is the same as Callisto frame parameters. ATC_ACCEL_R_MAX, 0 ATC_ACCEL_P_MAX, 0 ATC_ACCEL_Y_MAX, 0 ATC_INPUT_TC, 0.01 - This is the minimum allowed in the code. ATC_RATE_R_MAX, 0 ATC_RATE_P_MAX, 0 ATC_RATE_Y_MAX, 0

I wrote a simple script to inject step input in guided mode. The step inputs were injected one axis at a time as roll, pitch, and yaw, respectively. I run two SITL with the first being the current equation and the second is the updated one. The updated equation is, Quaternion to_to_from_quat = attitude_vehicle_quat.inverse() _attitude_target_quat; Quaternion desired_ang_vel_quat = to_to_from_quat attitude_target_ang_vel_quat * to_to_from_quat.inverse();

I also logged both current and updated desired_ang_vel_quat in two SITL flights. In the first flight, the difference rises as high as 10-20% at the highest values of desired_ang_velquat. This can be seen by comparing upper left and upper right graphs. QUAT.CQ is the current formulation and QUAT.NQ_ is the updated one.

This is the logging function used in this flight.

If we zoom into one of the spikes, we can see roll rate and pitch desired have the same sign. This will become important later on.

In the second flight, the logging function and calculation changed as follows, QUAT.NQ_ is always (1,0,0,0) quaternion, so we are not interested in it anymore.

In the whole flight we can see descripices at off-axis rate desired signal compared to the previous flight.

If we zoom into to the same section as the first flight. We can see roll rate and pitch rate signal have different signs.

If we zoom into further in y axis of RATE logs. We can see sign error in off-axis term causes slight increase in off-axis rate response. I tried to aligned y axis scale as much as I can. First Flight.

Second Flight

There is slight difference in the offaxis response between two formulation, under these very unlikely parameters choise.

[esaldiran]

@lthall I think this will result in a wrong result as well. If you intended to take the inverse of the only right-hand side, attitude_vehicle_quat and _attitude_target_quat should also switch places.

It looks to me like it should be this: Quaternion to_to_from_quat = attitude_vehicle_quat * _attitude_target_quat.inverse();

When we switch the side in your equation, it become to_to_from_quat _attitude_target_quat = attitude_vehicle_quat (1) _attitude_target_quat = to_to_from_quat^-1 attitude_vehicle_quat (2)

This means to_to_from_quat^-1 is defined in the inertial frame due to to_to_from_quat^-1 being placed right to left, but we want the difference between two local frames.

Also, my earlier comment was wrong. to_to_from_quat defined between two local frames, but it needs to be defined between local to the inertial frame or reverse order for this statement to be true.

Using the inverse of to_to_from_quat to rotate a vector from target to vehicle frame seems right.

I think changing only to_to_from_quat variable name to target_to_vehicle_quat is enough. Other variable names are self-explanatory and I think they can stay the same. By only changing target_to_vehicle_quat(to_to_from_quat) calculation we can leave the rest the same. target_to_vehicle_quat(to_to_from_quat) = _attitude_target_quat.inverse() * attitude_vehicle_quat However, comment should be added to mention target_to_vehicle_quat(to_to_from_quat) is defined in the vehicle frame.

If Leonard is okay with this change and results, @BreederBai I can create PR for bug and comment fix.

[lthall]

@esaldiran @BreederBai I can't tell you how nice it is to have a couple people to bang this around with!!!

It is probably worth describing in words what this code is supposed to to, just to make sure we are all on the same page (I think we are but just to be safe). The purpose of this code is to move the angular rates that we are using to move the target attitude to the body frame of the aircraft. This ensures that the angular error between the target and body doesn't increase due to misalignment of the angular rates.

So to see the impact of this part of the code we need to create a situation where there is a significant angular error between the target and body frames. This will then maximise the error in this equation: Quaternion desired_ang_vel_quat = to_to_from_quat.inverse() * attitude_target_ang_vel_quat * to_to_from_quat;

We should then see an error in the target path and our error should change. I was thinking we could add an offset into the target being sent to the attitude controller so there was always a defined offset between our target and body frame.

@esaldiran I will go over what you have done there. It will take a good read :)

Thanks again both of you!!

[lthall]

Ok, I went through the code to try to work out why this didn't read properly and understand this bug. This is what I have come up with: https://github.com/lthall/Leonard_ardupilot/tree/20210402_Quaternion_Fix

The part I do not understand is I appeared to get a different result from the: _ahrs.get_quat_body_to_ned(attitude_vehicle_quat) vs: _ahrs.get_quaternion(attitude_vehicle_quat)

When I checked the results of attitude_vehicle_quat.inverse().to_euler to the current roll and pitch angles it was messed up.

I suspect that the current code is close for small lean angles but might go bad, maybe spectacularly so, at large lean angles.

Do you have any thoughts?

[esaldiran]

It is probably worth describing in words what this code is supposed to to, just to make sure we are all on the same page (I think we are but just to be safe). The purpose of this code is to move the angular rates that we are using to move the target attitude to the body frame of the aircraft. This ensures that the angular error between the target and body doesn't increase due to misalignment of the angular rates.

Yes, this part corresponds to the feedforward path in copter attitude control page.

So to see the impact of this part of the code we need to create a situation where there is a significant angular error between the target and body frames. This will then maximize the error in this equation:

Yes, this was my intention when I tested its effect. I disabled the acceleration limit by setting it zero in the sqrt controller, and minimized the time constant(ATC_INPUT_TC param.), and also disabled the angular rate limit. This allows us to inject step input to the input shaping algorithm without disabling feedforward calculation and smoothing step input.

We could also add offset into desired angular velocity calculation. I expect the vehicle to converge to the non-zero attitude. Ex. Let say desired angular velocity offset in the roll axis is pi/6, then the vehicle would stay at pi/6(ATC_ANG_RLL_P) roll angle.

The part I do not understand is I appeared to get a different result from the: _ahrs.get_quat_body_to_ned(attitude_vehicle_quat) vs: _ahrs.get_quaternion(attitude_vehicle_quat) When I checked the results of attitude_vehicle_quat.inverse().to_euler to the current roll and pitch angles it was messed up.

As I stated in the other issue, get_quat_body_to_ned and get_quaternion should correspond to the same thing. I will check this as well. If they correspond to the same thing then you won't get consistent results between attitude_vehicle_quat.inverse().to_euler and ATT logs as they are inverse of each other.

Also, in your branch I see you changed this lines from Quaternion to_to_from_quat = attitude_vehicle_quat.inverse() _attitude_target_quat; (1) Quaternion desired_ang_vel_quat = to_to_from_quat.inverse() attitude_target_ang_vel_quat to_to_from_quat; (2) to // rotation from the target frame to the vehicle frame Quaternion target_to_vehicle_quat = attitude_vehicle_quat _attitude_target_quat.inverse(); (3) // target angle velocity vector in the vehicle frame Quaternion desired_ang_vel_quat = target_to_vehicle_quat.inverse() attitude_target_ang_vel_quat target_to_vehicle_quat; (4)

But this formulation is not right. It is more wrong than the current formulation. With current formulation, if we insert (1) into (2), we get, _attitude_target_quat.inverse() attitude_vehicle_quat attitude_target_ang_vel_quat attitude_vehicle_quat.inverse() _attitude_target_quat. which can be interpreted as, rotate vector defined in vehicle frame to the target frame. The only problem is attitude_target_ang_vel_quat defined in the target frame.

Using formulation in your branch, and insert (3) into (4) _attitude_target_quat attitude_vehicle_quat.inverse() attitude_target_ang_vel_quat attitude_vehicle_quat _attitude_target_quat.inverse(), which can be interpreted as, rotate vector defined in the inertial frame to vehicle frame. Although the new vector is not defined in the target frame, it applies an operation to rotate it from the target frame to the inertial frame. But neither attitude_target_ang_vel_quat is defined in the inertial frame nor the result of the inner operation is defined in the target frame.

The correct way would be to take the inverse of equation (1) which is simpler than my original suggestion, which was switching inverse operation of first and last term in the right-hand side of eq (2). Take the inverse of eq(1) Quaternion to_to_from_quat = _attitude_target_quat.inverse() attitude_vehicle_quat; (5) insert eq(5) into eq(2) attitude_vehicle_quat.inverse() _attitude_target_quat attitude_target_ang_vel_quat _attitude_target_quat.inverse() * attitude_vehicle_quat; which can be interpreted as, rotates vector defined in target frame to the inertial frame and the rotate this new vector from inertial frame to the vehicle frame, which is exactly what we want.

If my explanation is confusing, I would happy to convey it with math formulas instead of variable names.

[esaldiran]

@lthall Do you happen to have any paper or any other materials that you used while writing sqrt controller? I would be happy to give it a read.

[lthall]

@esaldiran I see what you are saying and my mistake.

Can you have a look at this now. Does this look right to you now? https://github.com/lthall/Leonard_ardupilot/commits/20210402_Quaternion_Fix

[esaldiran]

@lthall yes, this formulation looks right. Did you obtain different result with this formulation than I do?

[lthall]

@esaldiran Thanks for checking that for me!

Sorry, I am not sure what results you are talking about there. I checked the input Quaternion and got the same results as you. The name does not appear to match the data and I am concerned about how else that is being used...

I forgot to answer your question on the sqrt controller: https://diydrones.com/profiles/blogs/arducopter-2-9-pid-loops-for-stabilize-acro-and-alt-hold

This is the only write I have done. It is very simple though, it is just a standard P term controller that limits the deceleration for larger errors.

Over the next few days I will do a PR for master ready for the next release. I am also going to put a thrust vector based controller in there to improve handling of yaw errors in auto modes and motor out situations (hex especially).

Thanks for all your help. With your permission I am going to see if I can work out how to give credit to both of you for this PR!

[esaldiran]

Sorry, I am not sure what results you are talking about there. I checked the input Quaternion and got the same results as you. The name does not appear to match the data and I am concerned about how else that is being used...

I was referring to this message . Although relevant code peace is different, math is the same.

If you are referring to the name of this line, then I think your name selection is appropriate. The comment above also explains the operation being performed. This line is taking from your branch. Quaternion target_to_vehicle_quat = _attitude_target_quat.inverse() * attitude_vehicle_quat;

I forgot to answer your question on the sqrt controller: https://diydrones.com/profiles/blogs/arducopter-2-9-pid-loops-for-stabilize-acro-and-alt-hold

Thanks for the link. I will give it another read.

[Hs293Go]

Quite disappointed that I missed all the action here! I understand this discussion is in an advanced state --- Feel free to ignore me if the upcoming PR solves the problem sufficiently, but here are my two cents:

@lthall there is one final way to check the maths: converting to_to_from_quat to rotation matrix first, then apply the rotation matrix to the angular velocity vector. This should be equivalent to applying the quaternion rotation formula, i.e. q.inverse() * v * q. Off the top of my head, the code might look like

/* ---- Implementation in AC_AttitudeControl: Fix my math --- */
Quaternion attitude_target_ang_vel_quat = Quaternion(0.0f, _attitude_target_ang_vel.x, _attitude_target_ang_vel.y, _attitude_target_ang_vel.z);

// rotation from the target frame to the vehicle frame
Quaternion target_to_vehicle_quat = attitude_vehicle_quat * _attitude_target_quat.inverse();

Quaternion desired_ang_vel_quat = target_to_vehicle_quat.inverse() * attitude_target_ang_vel_quat * target_to_vehicle_quat;

/* ---- Rotation Matrix Check ---- */

Matrix3f rotation_target_to_vehicle; 
target_to_vehicle_quat.rotation_matrix(rotation_target_to_vehicle);
Vector3f desired_ang_vel = rotation_target_to_vehicle * _attitude_target_ang_vel;

/* ---- Compare different code paths ---- */
assert(desired_ang_vel_quat.x == desired_ang_vel.x); // actually do compare float within a tolerance 
assert(desired_ang_vel_quat.y == desired_ang_vel.y);
assert(desired_ang_vel_quat.z == desired_ang_vel.z);

The reason why I suggest this extra check is that I came across the paper Why and How to Avoid the Flipped Quaternion Multiplication, which reports the existence of different conventions to rotate a vector by a quaternion. In all due respect, it might be helpful to make sure that this issue is a genuine error in maths, instead of a conflict of quaternion conventions.

[BreederBai]

@lthall After reading the code carefully, I found that when the angle compensation is not installed, the output results of "_ahrs.get_quat_body_to_ned(attitude_vehicle_quat)" and "_ahrs.get_quaternion(attitude_vehicle_quat)" should be the same, they are from "NavEKF3_core::getRotationBody3FNED(Matrix) &mat)" and "NavEKF3_core::getQuaternion(Quaternion& ret)" (if EKF3 is used), As can be seen from the picture above, their data sources are the same.The only difference is that the function "_ahrs.get_quat_body_to_ned(attitude_vehicle_quat)" is converted from a rotation matrix to a quaternion.In the usual sense, attitude_vehicle_quat represents the state after NED->body. "_Ahrs.get_quat_body_to_ned()" gives the impression from the function name that the quaternion it returns represents body->NED. But in fact what he said is still NED->body. This involves the conversion of quaternion and rotation matrix. In our project, the quaternion representing the pose means NED->body, but the transformed rotation matrix means body->NED. This is because we want to complete vn = Rmat * vb, in this case Rmat can only be body->NED. There is no right or wrong, this is a matter of strategy. But the name of the function "_ahrs.get_quat_body_to_ned()" does bring ambiguity.

[lthall]

Fixed in #17222