We've recently made the move from the old clunky DCM implementation to a new Quaternion approach, and it seems to perform better, or at least the code is much smaller and takes almost no time at all to execute on our 72MHz 32-bit flight controller. I'll be writing an article on the Quaternion method as part of our series on Engineering Insights.
We've also added MAVLink support, and full integration with QGroundControl GCS. This helps greatly with debugging, and of course enables waypoint flying. The ROFL quadcopter kits have GPS built-in, and wireless versions that include an integrated XBee module are available.
As always, more videos on our youtube channel: http://www.youtube.com/user/UAirLtd
And the kits are still on sale! http://www.universalair.co.uk/catalog/rofl
Still to come: waypoint flying, and FPV: The flight controller GPS code just needs a bit of tuning, and we'll have the thing flying autonomously in no time. The full-range of waypoint options in QGroudControl are supported, including waypoint tolerances, and yaw angles. (See image below):
Tuning the quad is also possible over QGroundControl, this is a massive improvement in useability over our previous custom solution:
Hi Henry,
Very well said, a good description of the situation. The only statement that I would take issue with, it must have been a typo, was:
"The DCM is a 9x9 matrix
The Quaternion is a 4x1 vector."
I am sure that you meant to say:
"The DCM is a 3x3 matrix
The Quaternion is a 4x1 vector."
And I would like to follow up on your statement, "Both of them are control specialists and looked rather disparagingly at my linear approximations!"
I agree with your control specialist friends. When I ever get some time, I am going to redo the core IMU and control computations of MatrixPilot to eliminate a few of the linear approximations that are usually made, and use the exact nonlinear equations instead.
I also agree with you, that it would be best for everyone if we provide the community at large with useful information.
Regarding a comparison of rotation representations, an obvious thing for folks to do is to look at the wiki entries for "rotation matrix" and for "quaternion rotation".
At the risk of repeating myself, the best side-by-side comparison of matrix and quaternion representations is done in Robert Mahony's papers on the subject of control of VTOLS. Mahony is an advocate of quaternions, but in his papers he does a parallel treatment of matrix and quaternion formulation of his ideas.
Finally, I want to point out that the latest version of MatrixPilot uses both matrices and quaternions. I have found a few places where quaternions were better than matrices, so I bit the bullet and used them in those places. I have routines for converting back and forth, they do not take much CPU power or use much memory.
Best regards,
Bill
Oh yes! Woops, good spot Bill. 3x3 is 9 but the DCM is a 3x3 matrix ... !
Yes, perhaps "clunky" wasn't the best word there! I just got a little carried away with the fact I had got my gyro updated variables down from 9 to 4, I guess it satisfied my mathematical "less is best" brain!
But I can certainly see why you have both the DCM and Quaternions stored in your latest version of MatrixPilot. In the ROFL code we recreate parts of the DCM from the Quaternion for accelerometer feedback purposes. At a later date I'm going to see if it's possible to use some "complex " - hehe (sorry, weak joke) quaternion maths (which I am still learning, it has many subtleties!) to take a more direct route. But for now, both representations have their merits, and it's possible that the shortest route to the accelerometer feedback is indeed via the DCM elements.
Yes, because today you still need to switch back and forth between the quaternion and the DCM representations for feedback purpose, the DCM algorithm is not so "clunky" !
And thank you Bill for having given DCM to all the DIY Drone community a couple of years ago. If somebody finds an algorithm as straightforward but more accurate, please tell us about it. But don't bite the hand that feeds you ! :)
For those reading this thread that are completely new to the maths for representing orientation ( e.g. DCM and Quaternions and Euler Angles ), I have found this book ( 3D Math Primer for Graphics and Game Development ) to be helpful. It is for undergraduates, starts with first principles, gives a good overview of the strengths of each representation, and when to use them. The book works right through to examples of code and libraries in C.
Best wishes, Pete
I think this grammar needs some explaining: "the old clunky DCM implementation". The "clunky" part of this paragraph refers to the "old implementation", not the "DCM". In this sense we are talking about "the old (previous) and clunky implementation of the DCM algorithm", as opposed to "the implementation of the old and clunky DCM algorithm".
We make no claims to have attempted or presented a comparison between these two algorithms in the general case.
Great discussion, guys!
I would just like to make the following points and ask some questions. Please correct me if I am mistaken on any of these.
- No one, especially not control specialists, should disparage linear approximations! They are very powerful and useful (which is not to say thay can't be imporved upon with non-linear methods)
- The dsPIC, like the Cortex M3, does not not have floating pont hardware. The UDB firmware uses fixed point computations and takes advantage of built-in DSP multiply-and-accumulate routines to increase vector / matrix processing throughput. (note, the Cortex M4 has both DSP features and floating point hardware for elementary funtions - including square roots, but not including transendental functions).
- It seems to me that quaternions may have a computational advantage in that they only have 4 coordinates that need to be normalized, as opposed to 3 x 3 coordinates for DCM plus the orthogonaliztion (or 2 x 3 orthonormalization plus a cross-product, if you prefer). I think this holds even if you need to convert the quaternion to a DCM for use in further calculations.
- Although (ortho-)normalization is necessary for quaternions and DCM, the process can actually be a source of error. There are other, more exotic quaternion-like representations (rodrigues parameters and modified rodrigues parameters) which do not require normalization. I do plan to investigate all these representations examing both their computational efficiency and their accuracy.
- You don't need to switch to DCM for feedback purposes. In fact, if you are willing to explore the extra computation to calculate an error (with DCM, quaternion, or the other representations I mentioned above), you can directly determine the torque vector (or angular velocity vector) required to bring the error to zero.
- Bill - do you care to elaborate on the linear approximations in MatrixPilot you plan to eliminate? I assume you mean in the approximation of the integral to the kinematic equation where (I beleive) you were using the first term of the Taylor series and now you seem to add the second quadratic term? Also, do you mean to replace the use of elements of the DCM in feedback as approximations to the actual (Euler) angles?
- Finally, Bill, I noticed in one of the many fine documents on your site "fastRotations.pdf" you note the approximation of "tan(delta-theta) / delta-theta". Where does the tan() function come in? I always thought the true integral to the kinematic equation of rotation (assuming constant angular velocity) was a (complex) exponential, not a tangent....?
Thanks again
Roy
Hi Roy,
Yes, I eventually plan to eliminate two linear approximations. The first one is the integral of the kinematic equation, to use the exact nonlinear formulation of a finite rotation. That one is a high priority for me. The second one is to use a nonlinear control. That one is not such a high priority.
Regarding the approximation "tan(delta-theta) / delta-theta", that one is a fun and interesting question. In my present implementation of MatrixPilot, I have added the third term in the Taylor series, not the second one, based on a careful comparison of the nonlinear equations with the linearization.
Disregarding the second term in the Taylor series merely causes a distortion of the magnitudes of the rows and columns of the direction cosine matrix. That does not cause any sort of error in the orientation estimate, and in any case, the renormalization process will compensate.
However, ignoring the third term in the Taylor series causes an orientation error. You can analyze how much the error is by drawing a right triangle and a circle. Start with a base equal to 1.0, and with a circle of radius 1.0, center aligned with the left side of the base. Then, draw another radius of the circle, angle delta-theta. (This is the true, nonlinear angle.) Then draw a vertical line on the right side of the base, length delta-theta. (This is the linearized delta.) You will find that the angle formed by the triangle is arctan(delta-theta), but the angle that you really want is delta-theta.
So, the amount of shift that you wanted should have been tan(delta-theta), but you got delta-theta instead.
Also, the technique that I am using is not that I am adding terms in the Taylor series for sine and cosine, rather what I am doing is taking a Taylor series for the error introduced by the linearization of the matrix update. In particular, during a rotation that is approximated by a perpendicular increment to a unit vector, the resultant vector has inflated values of both the length of the vector and the amount of vertical shift, but the angle of the shift gets deflated.
In any case, I tested the compensated computations on a 78 rpm record player, they were extremely accurate. In "fastRotations.pdf", you can see the improvement in Figure 12 compared with Figure 10.
Best regards,
Bill
Hi Roy,
One more comment in response to your comment: "- Although (ortho-)normalization is necessary for quaternions and DCM, the process can actually be a source of error. There are other, more exotic quaternion-like representations (rodrigues parameters and modified rodrigues parameters) which do not require normalization. I do plan to investigate all these representations examing both their computational efficiency and their accuracy."
That is a good observation....
I have done an analysis of the errors that arise from renormalization of direction cosine matrix elements, and discovered that the root of the problem is linearization of the nonlinear kinematics update. Renormalization transforms the linearization errors in interesting ways. For example, linearization of the update causes an increase in the magnitudes of the rows and columns of the direction cosine matrix, and in the magnitude of the quaternion. However, since the inflation is not the same for each element in a row or column of the matrix, or for each element in the quaternion, after the renormalization step, there is a small orientation error. For example, for a row that is almost in alignment with the axis of rotation, the combination of linearization and renormalization will tilt the row away from the axis of rotation. Of course, the gyro drift correction will move it back, but the integral term in the feedback will cause trouble during fast rotations. (That is why I now turn off the drift compensation integrator during fast rotations.)
However, if you use the exact, nonlinear form for the rotation update, then the subtle errors almost vanish, and the effect that I just described vanishes.
Best regards,
Bill Premerlani
HI Bill,
(sorry I'm so late in my reply)
I created the figure you recommend, and I think I see what you are getting at. Let me see if we are on the same page:
With a first order (linear) approximation to the kinematic integration soultion, we assume that the angular velocity omega is constant over the integration time delta_t. In this case, we want to form a rotation of delta_theta radians, where delta_theta = (magnitude of omega)(delta_t)
Hence, we rotate a unit vector v(t) such that v(t + delta_t) = v(t) + delta_t(omega x v(t)), where "x" indicates the vector cross product. However, this will produce a v(t + delta_t) which has a distorted magnitude and angle. Normalizing will correct the magnitude, but not the angle. However, if you add a vector that is in the same direction as omega x v(t), but has magnitude tan(delta_theta), and then normalize, you will eliminate both the magnitude and the angle errors. In your code, you are using the first terms of the taylor series to approximate the tan() function. Is this correct?
I have the these follow-up questions:
1) I think the taylor series for tan(x) = x + x^3/3 + 2x^5/15 + .... So the second term is x^3/3, or x^2/3 if you divide by x. I'm still not sure how you are getting that this is the third term - what am I missing?
2) I wonder if a similar correction could be applied to the quaternion formulation (or one of the other orientation representations)? I don't think the correction would be tan(delta_theta), though.
3) Finally, it should be possible to determine the exact vector to add to v(t), such that you arrive at the correct v(t+ delta_t) without having to normalize. This may save some computation and increase accuracy (although, I suspect that other sources of error would eventually require a manual orthonormaliztion anyway, so it may not be worth the effort).
- Roy
Hi Roy,
I am delighted to continue this discussion with you. You are exactly correct in your comments.
1. I am counting 0*x^2 as the second term in the expansion. Sorry about that. In any case, it is the x^3/3 term that is the correction. So, what I am doing is taking x, which I was going to use in the first place, and multiply it by (1+x^2/3).
2. There is probably a correction you can apply to any of the linear update equations, if you want to have some fun, you might want to work out what it would be for the quaternion formulation.
3. Finally, the best thing to do is to abandon the approach of treating the update as a problem of figuring out what to add. There is a better, nonlinear approach. The composition of two rotations can be computed exactly by multiplying the two matrices that represent the rotations. One matrix will be the orientation from the previous time step. The other matrix will be the exact, nonlinear matrix that corresponds to the rotation delta_thet....
In other words, I am recommending using the Rodriques' rotation formula that you mentioned the other day. I did not realize that you and I were talking about the same thing, because I did not know the name of the formula.
Best regards,
Bill Premerlani
Comment
139 members
356 members
996 members
1420 members
143 members
© 2017 Created by Chris Anderson. Powered by
You need to be a member of DIY Drones to add comments!
Join DIY Drones