Arducopter quaternion based attitude control

minhtq98 · October 17, 2018, 2:14am

Hi Leonadt Hall,
Recently I have some time to look back at the attitude control by quaternion. When looking back at Copter-3.5 AC_AttitudeControl.cpp line 615, I’ve found the definition
Quaternion yaw_vec_correction_quat = thrust_vec_correction_quat.inverse()*att_from_quat.inverse() att_to_quat;
According to this definition thrust_vec_correction_quatyaw_vec_correction_quat = att_from_quat.inverse() att_to_quat = _attitude_ang_error.
This would mean that first we apply the yaw_vec_correction_quat, then apply the thrust_vec_correction_quat which is not like what you’ve said that the algorithm first try to rotate the thrust vector, then try to rotate the yaw vector. IMO I think that this code should be there instead yaw_vec_correction_quat thrust_vec_correction_quat = att_from_quat.inverse()*att_to_quat.
Therefore yaw_vec_correction_quat = att_from_quat.inverse() att_to_quat thrust_vec_correction_quat.inverse() and this code should replace the definition in line 615.

minhtq98 · October 17, 2018, 2:17am

I don’t understand why the * sign is lost in some space and the part after that becomes italic.

minhtq98 · October 17, 2018, 2:49am

The last try
Hi Leonadt Hall,
Recently I have some time to look back at the attitude control by quaternion. When looking back at Copter-3.5 AC_AttitudeControl.cpp line 615, I’ve found the definition
Quaternion
yaw_vec_correction_quat = thrust_vec_correction_quat.inverse()*att_from_quat.inverse()att_to_quat;
According to this definition
thrust_vec_correction_quatyaw_vec_correction_quat = att_from_quat.inverse()att_to_quat =
_attitude_ang_error.
This would mean that first we apply the yaw_vec_correction_quat, then apply the thrust_vec_correction_quat which is not like what you’ve said that the algorithm first try to rotate the thrust vector, then try to rotate the yaw vector. IMO I think that this code should be there instead
yaw_vec_correction_quatthrust_vec_correction_quat = att_from_quat.inverse()*att_to_quat.
Therefore
yaw_vec_correction_quat = att_from_quat.inverse()att_to_quatthrust_vec_correction_quat.inverse() and this code should replace the definition in line 615.

Leonardthall · October 17, 2018, 9:48am

Hi minhtq98,

I have not had time to check this code again as it takes me quiet a long time to go over all the maths. Generally I find that I take more time to check the code than people take to check their criticisms.

Most of these issues I find are because people are confused with which frame we are moving too and from.

I will get back to it and check over it again but it will take time.

Thanks for pointing out a potential problem. I apologise that it has taken longer than you would like for me to get back to you.

minhtq98 · October 18, 2018, 12:29am

Thank you for the reply. It may take time for you to look at the issue, but it’s more important that you know about that.

Leonardthall · October 18, 2018, 12:46am

Thanks minhtq98,

I appreciate your understanding.

minhtq98 · November 2, 2018, 1:47am

Some time has gone. I hope you had time to look back at your calculations. What’s your opinion about my question?

minhtq98 · November 3, 2018, 3:47am

Hi Leonadt Hall,
In a similar manner, in line 419 of AttitudeControl.cpp, I found
_attitude_target_quat = _attitude_target_quat * attitude_target_update_quat;
But I think here we should also apply _attitude_target_quat first, then comes attitude_target_update_quat.
This way, the right side should be in the reserve order:
attitude_target_update_quat * _attitude_target_quat;

Leonardthall · November 3, 2018, 10:46am

The code looks right.

This is just the way we calculate the components of the rotation, not the way we apply them.

I checked this too and again the code is correct.

Maybe you should set the equations up in Matlab and play with them to see what is happening.

Thanks for taking the time to read through the controller!

minhtq98 · November 5, 2018, 2:50am

I don’t understand. From the way you calculate the component of the rotation, we can conclude the way they are applied. From your definition
yaw_vec_correction_quat = thrust_vec_correction_quat.inverse() * att_from_quat.inverse() * att_to_quat;
I can conclude that att_from_quat * thrust_vec_correction_quat * yaw_vec_correction_quat = att_to_quat;
The left side means that first we apply the yaw_vec_correction_quat, then thrust_vec_correction_quat and last the att_from_quat. Can you give an explanation?

Leonardthall · November 5, 2018, 3:24am

I would suggest that you do a quick matlab experiment and draw the path from a large attitude error and use the method I used here and the one you suggested. It won’t take you long and will make it much easier for us to discuss it.

minhtq98 · November 5, 2018, 3:34am

I don’t know how to do a matlab experiment, can you suggest the way to do it?

Leonardthall · November 5, 2018, 6:58am

Now I think you are wasting my time.

minhtq98 · November 5, 2018, 7:28am

OK, I may not know that you’re too busy to answer such a request. But I still believe that the order of quaternion multiplication is important.

minhtq98 · November 6, 2018, 1:44am

May be that in your country matlab is very popular, but in my country I rarely use it. That’s why I don’t know its experiment. But without it now I begin to understand the algorithm. For large attitude error, it may take several fast_loop cycles to reach the desired attitude. In first cycles the algorithm only uses thrust_vec_correction_quat to rotate the vehicle attitude to the desired attitude, then when the attitude error becomes smaller, it uses together with yaw_vec_correction_quat. Saying that the algorithm first try to rotate the thrust vector, then try to rotate the yaw vector is only the way to calculate the quaternions, but when they are actually applied, they are converted to motor.set_roll, set_pitch and set_yaw and applied all at the same time. With this knowledge, you can define the yaw_vec_correction_quat either from thrust_vec_correction_quat * yaw_vec_correction_quat or yaw_vec_correction_quat * thrust_vec_correction_quat depending on the fact that which way makes the calculation simpler and in the same reference frame.
May be I still have the last request to Leonadt Hall. Can you explain simply what is the feedforward and why can’t I find the target_quat updates when feedforward is enabled?

Leonardthall · November 6, 2018, 2:43am

There are lots of ways you could experiment with the equations to check your ideas. Matlab is just one of many options. If you are capable of understanding quaternion maths then you should have plenty of options. You should not need may help to do this. I honestly started to think you were trolling me. Everything is fine though.

The basic idea of the feed forward is if we tell the rate controller what rate it needs to achieve as we change the target angle then the angle error will always be zero (without disturbances and in a perfect system). So with feed forward enabled the angle controllers are only correcting for the disturbances and imperfections of the system. This results in much more direct pilot feel and confidence.

The feedforward is the initial value of _attitude_target_ang_vel and then the correction is added in this line and the lines following:

github.com

ArduPilot/ardupilot/blob/72440e2af3c31e0c701bd299d3428b41ade9f4e5/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp#L556


      
          // Compute the angular velocity target from the attitude error
          _rate_target_ang_vel = update_ang_vel_target_from_att_error(attitude_error_vector);
          
          // Add feedforward term that attempts to ensure that roll and pitch errors rotate with the body frame rather than the reference frame.
          // todo: this should probably be a matrix that couples yaw as well.
          _rate_target_ang_vel.x += constrain_float(attitude_error_vector.y, -M_PI/4, M_PI/4)  * _ahrs.get_gyro().z;
          _rate_target_ang_vel.y += -constrain_float(attitude_error_vector.x, -M_PI/4, M_PI/4) * _ahrs.get_gyro().z;
          
          ang_vel_limit(_rate_target_ang_vel, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));
          
          // Add the angular velocity feedforward, rotated into vehicle frame
          Quaternion attitude_target_ang_vel_quat = Quaternion(0.0f, _attitude_target_ang_vel.x, _attitude_target_ang_vel.y, _attitude_target_ang_vel.z);
          Quaternion to_to_from_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat;
          Quaternion desired_ang_vel_quat = to_to_from_quat.inverse()*attitude_target_ang_vel_quat*to_to_from_quat;
          
          // Correct the thrust vector and smoothly add feedforward and yaw input
          if(_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE*2.0f){
              _rate_target_ang_vel.z = _ahrs.get_gyro().z;
          }else if(_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE){
              float feedforward_scalar = (1.0f - (_thrust_error_angle-AC_ATTITUDE_THRUST_ERROR_ANGLE)/AC_ATTITUDE_THRUST_ERROR_ANGLE);
              _rate_target_ang_vel.x += desired_ang_vel_quat.q2*feedforward_scalar;

minhtq98 · November 7, 2018, 12:35am

Thank you Leonadt Hall for the explanation. Although still not so clear for me, but I think it’d be a good start for me now.

minhtq98 · November 16, 2018, 1:03am

Hi Leonadt Hall,
Its me, sorry for bothering you again. In line 556, you’ve commented that it is rotated to vehicle frame. But I think you may mistake here, because the rotation to vehicle frame should be: Quaternion desired_ang_vel_quat = to_to_from_quat* attitude_target_ang_vel_quat * to_to_from_quat.inverse();
What do you think?

Leonardthall · November 16, 2018, 7:05am

Ok, you just made all the time I spent talking to you 100% worthwhile, thanks.

So, to answer your question, no.

Quaternion to_to_from_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat;

Is the same as

 // Record error to handle EKF resets
_attitude_ang_error = attitude_vehicle_quat.inverse() * _attitude_target_quat;

So I think the problem is:
1: I should have called this from_to_to or even better vehicle_to_target.
2: I have calculated _attitude_ang_error twice.

Thanks!!

minhtq98 · November 16, 2018, 7:33am

OK, I’m happy that my finding can help.I think the way you name quaternion is not as signifigcant as the way you define it. Now I can say I begin to understand your algorithm.