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
Replies
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
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,
I am finally getting around reading your post, but first I must read your posting more carefully before answering fully.
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