 ### 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-angular-velocities-are-vectors-while-rotations-arent) 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

2013-10-08.DIY-OmegaTest.xlsx

#### Replies

• T3

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

• T3

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 )

- __builtin_mulss( rmat6 , omegagyro )) *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 is the pitch rate in body frame of reference

omegagyro 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

• T3

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

• 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

2013-10-17.DIY-Gyro Signal vs. Derivatives.xlsx

https://storage.ning.com/topology/rest/1.0/file/get/3692860557?profile=original

### Activity

RT @_JonMyer: 🚨Attention DeepRacer's including #UndergroundDeepRacer🚨 Check our our LIVE stream that including @IAM_dbro Take a few moments…
Aug 5
RT @MarvelmindMaxim: Extreme precision for 60 swarming robots. #marvelmind #autonomousrobotics #robotics #swarmrobotics #rtls #ips #indoor…
Aug 3
RT @MarvelmindMaxim: Precise (±2cm) tracking for racing boats and autonomous boats. Works outdoor and indoor. #autonomous #AutonomousVehic…
Aug 3
RT @MarvelmindMaxim: Helping PixHawk folks to fly autonomous quadcopters using PX4 and ArduPilot. https://marvelmind.com/drones/ Equally suitab…
Aug 3
RT @chr1sa: The @DIYRobocars @donkey_car virtual AI car race is starting in 15 minutes! Watch it live on Twitch https://www.twitch.tv/mossmann3333 htt…
Aug 1
RT @chr1sa: Don't miss our monthly @DIYRobocars @donkey_car virtual AI car race tomorrow at 10:00am PT live on Twitch. Head-to-head racing…
Jul 31
RT @sparkfun: Our completed tutorial on building an @NVIDIA Jetson Nano-powered @Sphero RVR gets your bot up and running via teleoperation…
Jul 30
RT @SmallpixelCar: Freeway test https://t.co/4V5tV9lhIP
Jul 29
Very small autonomous cars racing, thanks to an overhead camera: https://control.ee.ethz.ch/research/team-projects/autonomous-rc-car-racing.html
Jul 29