I think this is what you are looking for: http://ardupilot.org/plane/docs/common-apm-navigation-extended-kalman-filter-overview.html

# Arducopter quaternion based attitude control

**minhtq98**(minhtq98) #5

Thank you very much. This is the first answer I’ve received. It then takes time from me to read documents in the direction you’ve pointed to me.

**minhtq98**(minhtq98) #6

Hello Ppoirier,

I think the EKF2 you mentioned is the sensor fusion algorithm, but here I’m asking about the attitude control algorithm with quaternion. That means that the sensor fusion algorithm will give the measured roll, pitch and yaw angles then the attitude controller would compare those values with the desired values set on RC and based on the difference the controller works out the the new roll, pitch and yaw.

This process is based on DCM before but since 3.4 version it moves to quaternion and this is my question now about the quatenion based attitude control algorithm.

**ppoirier**(ppoirier) #7

You might be refering to this: http://ardupilot.org/dev/docs/apmcopter-programming-attitude-control-2.html

Have a look in the attitude libraries, and https://github.com/ArduPilot/ardupilot/tree/master/libraries/AP_AHRS where you find DCM and AHRS attitude control.

**minhtq98**(minhtq98) #8

Hello Ppoinier

Thank you for your time and help on my question. I’ve already read the first link. It sorry contains many old information which is true only for Arducopter 3.2. I had also tried with the second link, but I think my main problem is in https://github.com/ArduPilot/ardupilot/blob/master/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp. In the attitude control it has been moved to quaternion from DCM. I cannot follow the code because the description is not detailed enough and there is no.description of the algorithm. Do you have any idea about quaternion attitude control?

**ppoirier**(ppoirier) #9

Well… this is my last shot

Look at the commits

And you can ask @Leonardthall he is the man behind this

**minhtq98**(minhtq98) #10

Thank you Ppoirier anyway for the help. I’ll read carefully the link you provided. Meanwhile I came to an important conclusion that attitude_controller_run_quat() only relates to the P angle controller. I believe that I’ll understand the code structure in the end.

**Leonardthall**(Leonard Hall) #11

The quaternion controller is used to calculate the attitude angle error and to define the path taken between the current position and the commanded position. It also deals with the various input axis and types.

After the attitude error is calculated the rest of the loop is just a P angle controller with square root limit followed by a PID rate controller.

**minhtq98**(minhtq98) #12

Thank you Leonard Hall for the reply. I think it may be a little different from what you said, although now I’ve known that you are behind the quaternion idea. The quaternion controller ìn fact is a P angle controller. It used angle error to calculate the angular velocities and pass to PID rate controllers. Am I right?

**Leonardthall**(Leonard Hall) #13

Yes, as I mentioned, we use quaternion math to calculate the angle error between our current attitude and where we want to be. We also use it to define our current and our desired attitude and to define the path we take as we move.

After all that it is basically an Angle P -> Rate PID controller in three axis.

**minhtq98**(minhtq98) #14

Thank you again, now I’m understanding the topic, not yet in every detail. To that level there is still a need to much more reading, but I think I can do that.

**minhtq98**(minhtq98) #15

Hi, Leonadt Hall,

I tried to look deeper into the software. According to the document there are 5

possible ways to control the attitude of the vehicle depending on flight modes, and in the end all call quaternion attitude controller attitude_controller_run_quat(). I tried with this function call, but still have problem understanding thrust_heading_rotation_angles(). Could you provide me with some lights or the link to the algrithm?

**Leonardthall**(Leonard Hall) #16

Sorry but I wrote this algorithm from scratch so there is no link.

Basically the algorithm first looks at rotating the thrust vector back to where it should be then it rotates the heading. So it is the combination of two quaternion rotations.

**minhtq98**(minhtq98) #17

In Thrust_heading_rotation_angle() I saw your definition of the thrust vector as the last column of the rotation matrix. My question is that how do you come out with this definition. After that, the thrust_vec_correction_quat rotates the att_from_thrust_vec to att_to_thrust_vec, my question is that what really does this quaternion and why do we need to rotate it back with att_from_quat?

**weihli**(li weihli) #18

@minhtq98 , it’s similar with the px4firmware’s attitude controller in mc_att_control; First align current attitude’s z axis with target attitude’s z axis; then rotate z axis to align xy axis. you can read paper “High performance full attitude control of a quadrotor on SO(3)” get the ideas.

**Leonardthall**(Leonard Hall) #19

There are a couple of things here.

We need to rotate the thrust vector to the desired thrust vector. We then look at rotating the heading to the desired heading.

We need to use the desired rates to update the desired attitude.

We need to apply the desired rates in the desired attitude frame to the current body frame.

So there is a bit going on in that section.

**Leonardthall**(Leonard Hall) #20

Thanks for the link to that paper I have not spent any time looking at papers on multi rotor control algorithms or maths so I will be interested to see what DJI did there.

edt: Well the paper is a good description of why quaternions are used and has a brief introduction to the attitude error calculation. However, the controller they propose is extremely basic and does not address the vast majority of practical challenges of multi rotor control design.

**minhtq98**(minhtq98) #21

I asked that how did you come out that thrust vector is the last column of the rotation matrix?

**minhtq98**(minhtq98) #22

Thank you for the reply. At the moment I don’t have access to ‘High performance full attitude control of a quadrotor on SO(3)’, but I think I’ll read it when I have access to.

**Leonardthall**(Leonard Hall) #23

I don’t have time to search through the code trying to understand your question but if you point to specific lines in the code I may be able to help.