# Rotating Gyro vector using DCM for PID control

Hi,

Implementing PID controller requires calculation of error, integral of the error, and time derivative of the error. Good attitude system (like the DCM that I am using for my quad) usually provides quite smooth estimate of error; integral error even smoother as integration serves as a filter. However, derivative of the error is subject to (relatively) large deviations as small errors in attitude estimate and timing of the control loop may result in large jumps in derivative due to its differential nature.

One of the options that I am investigating is to replace the derivative term (which represents the angular speed about the respective axis) with the actual with the actual angular speed reported by the gyros. The issue here is that the gyros measure angular speed in the BODY frame while the PID controller takes as input error and its derivative and integral in the EARTH frame! That means that values measured by gyros need to be brought into the EARTH frame.

While rotation is not a vector, angular velocity is a true vector (please check http://farside.ph.utexas.edu/teaching/301/lectures/node100.html, http://physics.stackexchange.com/questions/286/how-is-it-that-angul...) and as such could be rotated by the DCM. Thus the gyro vector in EARTH frame could be obtained from gyro measurements in body frame using the following formula:

GE = {DCM} x GB.

I decided to test this assumption using the telemetry from the actual flying quad captured using on-board data logger. Data points are collected at 10 msec interval at the end of each iteration through the control loop. All angles are measured in radians and angular velocity – in rad/sec. The formula works perfectly when the Yaw is negligible – rotation speed around the Y axis (rate of Pitch change) calculated as a derivative of actual Pitch angle and the one obtained from the rotated Gyro vector practically coincide with the angular velocity obtained from Gyro is smoother than the calculated derivative. However with Yaw increasing the phase of angular velocity obtained from rotated Gyro vector lags behind the actual derivative and with Yaw close to pi radians the lag results practically in inverting the value of angular velocity! Interesting to point out that only Yaw changes have such detrimental effect on rotated Gyro vector – relatively high values of Roll (up to 0.3 rad.) do not result in deviation of Pitch angular velocity from the calculated Pitch angle derivative.

On the chart below is the small fragment of the data collected; RC-Yaw and RC-Pitch are the control values obtained from receiver; Roll, Pitch, and Yaw are the attitude angles; Pitch values are accompanied by the charts for derivative (thin green line) and angular velocity obtained from the rotated Gyro vector (thin yellow line). Due to the large swing Yaw and RC-Yaw are plotted against the secondary vertical axis that goes from -2.0 to +2.0. All other values are plotted against the primary vertical axis in the range from -0.5 to +0.5.

The complete Excel spreadsheet with the raw data is attached.

Apparently some of my logic above is invalid, but I cannot find where is the flaw so I would greatly appreciate any comments or suggestions.

Thank you,

--Alex

Views: 2596

Attachments:

### Replies to This Discussion

Continuing this experiment, I concluded that for rotating the Gyro vector to the Earth frame I should ignore the Yaw angle, which represents rotation of the model around the Earth Z-axis. Roll and Pitch angles of the model do not change with the rotation around the Earth Z-axis. These angles represent the attitude of the model calculated against the X-Y plane of the Earth coordinate system and the angle between the axis and the plane does not change with the orientation of the model’s X-axis respective to the Earth X-axis, which is Yaw.

To test this assumption I decided to use specially constructed rotation matrix instead of the DCM. To build this matrix I take the Roll and Pitch angles from DCM and set Yaw=0 and then calculate rotation matrix using Euler formula. This matrix is being rebuilt at every step of the control loop and then used to rotate Gyro vector to Earth coordinate frame. The actual telemetry from the flight reporting both calculated derivative and angular speed obtained from rotated Gyro vector represented on the following chart.

Now we may see that roll derivative practically coincide with the value of angular speed obtained by rotating the Gyro vector. The complete spreadsheet with the raw data is attached

Attachments:

Alex,

There are a couple of things I can tell you for sure:

1. Rotation rate is a vector, so yes, you can transform it from body to earth frame.

2. Roll and tilt information is the last row of the matrix, it represents the view of the earth Z axis in the body frame. It is independent of yaw.

3. In matrix pilot, roll and pitch leveling control is done by driving the last row of the matrix to { 0 , 0 , 1 }, independent of yaw.

Beyond that I will need to read your post more carefully and give it some thought.

Best regards,

Bill

Alex,

In matrix pilot, we do not use Euler angles in any of the computations, so I am used to thinking about this in entirely different terms. We use matrices and vectors for all computations. For pitch control, we transform the gyro vector into the earth frame, and compute the component in the earth horizontal plane that is responsible for pitching of the plane relative to the horizon. The implementation itself is trivial, two multiplies and a subract:

pitchAccum.WW =

( __builtin_mulss( rmat8 , omegagyro[0] )

- __builtin_mulss( rmat6 , omegagyro[2] )) *2  ;

pitchrate = pitchAccum._.W1 ;

rmat6 is the projection of the earth z axis along the wing

rmat8 is the projection of the earth z axis along the z axis of the body

omegagyro[0] is the pitch rate in body frame of reference

omegagyro[2] is the yaw rate in the body frame of reference

pitchrate is the pitch rate in the earth frame of reference.

As you can see, pitch rate is the "D" term in the PID pitch control, computed directly from the gyros. Very smooth, and very precise.

Best regards,

Bill

Alex,

By the way, there is another way of looking at the MatrixPilot computation of pitch rate: it is one of the components of the cross product of the earth axis as seen in the body frame with the gyro rate vector in the body frame.

Best regards,

Bill

Bill,

Thank you for your reply! I decided to use Euler angles (obtained from DCM) because for a quad you not only leveling it, but also have to account for Roll-Pitch signals coming from RC. So in my implementation the Proportional component for, let's say, PID Pitch control is defined as

Kp*(PitchRC - PitchDCM).

If the elevator control is at the center position (PitchRC = 0), then it becomes just the leveling task. If I understand you correctly, your input into the PID controller would be the components of the cross product of Body vertical (last column of DCM) and true Earth vertical, which is {0, 0, 1} - am I correct in this assumption?

If so, what would be the way to introduce into this algorithm the input from RC?

Thank you,

--Alex

Bill,

OK, if I correctly understood your two previous posts, assuming that Earth Z axis in the body frame is Ze/b = {Zx, Zy, Zz} (the last row of DCM) and my gyro vector in the body frame is Gb = {Gb-x, Gb-y, Gb-z} the cross product of Ze/b and Gb will give me the respective rates in the Earth frame? That is:

Ge-x  = Zy * Gb-z - Zz * Gb-y,      (roll rate)

Ge-y  = Zz * Gb-x - Zx * Gb-z,      (pitch rate)

Ge-z  = Zx * Gb-y - Zy * Gb-x.      (yaw rate)

The second formula above (pitch rate) is matching the one that you provided:

pitchAccum.WW =

( __builtin_mulss( rmat8 , omegagyro[0] )

- __builtin_mulss( rmat6 , omegagyro[2] )) *2  ;

except that you multiply the result by "2" - what is the role of "2" there?

Also it is kind of understandable that this cross product will give the roll and pitch rate, but I still have an uneasy feeling about the yaw rate - when the model is strictly vertical the Zx and Zy both will be 0 resulting in Ge-z being also 0 independently of gyro vector. Looks like to determine yaw rate I need to take the Z-component of the cross product of the gyro vector with either Earth X or Y axis in the body frame - and it should not matter which one as long as it does not coincide with the Earth vertical...

While it is not applicable to my quad as it is not "acrobatic", but I am not sure how these formula will behave when one of the angles is at 90 degrees when Earth vertical will coincide with X or Y axis of the Body.

Would really appreciate your further insights into this topic.

Thank you,

--Alex

Alex,

The cross products do not transform the rotation rate vector from one frame to another, I think I misled you in one of my replies about that. I am not trying to do a frame transformation, though you could take that approach. Using cross products is a different way to do it.

The cross products go all the way back to the theory of the update of the direction cosine matrix. The rate of change of any of the rows or columns can be computed as the cross product of the row or column with the gyro rate vector.

The bottom row of the matrix is the earth Z axis as seen in the body frame. So, the cross product gives you the rate of change of the earth Z axis in the body frame. Two of the components are good indicators of the rate of change of roll and pitch. The third one doesn't tell you much of anything.

The factor of 2 in MatrixPilot implementation is because we are using Q2.14 format for fractional numbers.

Regarding your question of how you would inject the desired roll or tilt into the computation, one method is to use what Mark Whitehorn calls "virtual pose", which basically is specifying the desired values for the direction cosine matrix. Each column of the matrix is an axis of the aircraft in the earth frame. Call this matrix "Rd" (d for desired). It completely specifies the orientation.

Then, suppose the actual orientation of the aircraft is give by "Ra" (a for actual).

Then, in the frame of reference of the aircraft, the rotation matrix for rotating the aircraft from its actual orientation to its desired orientation is given by Ra^T*Rd, where Ra^T is the transpose of Ra.

I would suppose that is how the Arducopter might operate, I know for sure that is how Mark Whitehorn's quadcopter controls work for "MatrixPilot".

Best regards,

Bill

Bill,

Thank you for these additional details - I need to do a little bit more thinking and spend some time on the papers you references. At some time (about 30 years ago) I was quite comfortable with Linear algebra, but now will need a "refresher" course :)

On the positive side, I am now quite comfortable with the rotation in the plain so I implemented (successfully!) a switch in my control algorithm to easily move from "+" configuration to "X" configuration for my quad. I also implemented the "Corse Lock" a-la DJI Phantom so that control sticks preserve their intended orientation independently of the Yaw angle of the craft - was really fun to see how the quad rotates while maintaining direction of the flight.

I limit my Control Loop to 100 Hz (motor updates every 10 msec), but just for fun removed the extra delay that I put in the loop - it looks like I can run updates at about 600-700 Hz, so all my DCM update calculations and motor control calculations nicely fit into about 1.5 msec despite the fact that I use floating point presentation for all my calculations. However I decided to drop it down to 100 Hz as this allows me to take average of 10 gyro and acc samples for better stability of the IMU estimates.

This winter I am planning to put together a new board with 2 MPU6050 from Invensense with the sensors' axis at about 60 degrees - I believe that merging results from 2 independent sensors will assist in eliminating some jitter from vibration and would let me run a faster control loop.

That's some of the recent updates.

Regards,

--Alex

1

2

3

4

5

6

7

8

9

10

## Groups

1468 members

630 members

280 members

1123 members

• ### ArduCopter User Group

3163 members

Season Two of the Trust Time Trial (T3) Contest
A list of all T3 contests is here. The current round, the Vertical Horizontal one, is here