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: https://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:
Loads more to come, I'll keep you guys posted.
Comments
Hi Justin,
I agree with you. In the latest version of MatrixPilot, wind related computations have been moved to the navigation layer. The computation of the direction cosine matrix elements is now "pure".
We have worked out a way to estimate airspeed and wind speed without needing air flow sensors. The estimated wind speed is then used in the navigation calculations.
Airspeed is used for speed control.
Best regards,
Bill Premerlani
EE 71 - You guys are super math gods - thanks for letting me follow you discussions -
Hi Roy,
In the past, I have used CORDIC functions, table lookups, and low-order Taylor series, any of them could be used. I think that for this application I would use a combination of a table lookup and a low order Taylor series. I would use a rather coarse lookup table, but it would include a few coefficients for a Taylor series for each entry in the table. If one keeps the steps between entries in the table small, that would do it just fine. And, of course, you can use the symmetries of trig functions to advantage. I am thinking 256 entries in the table between 0 and 90 degrees, so the Taylor series expansion would be in terms of a delta x no larger than 0.006 radians. I It would wind up being rather accurate, without requiring much memory or CPU load.
Of course, in order to keep the accuracy from getting diluted from other sources of error, such as gyro calibration error, you will probably want to do things like in flight auto-calibration of gyro gain.
Eventually I might want to be able to support rotations at 2000 degrees/second. At 40 Hz frame rate, that would be 50 degrees per step, at 200 Hz frame rate, that would be 10 degrees per step.
Best regards,
Bill
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
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 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
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
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 ! :)
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.
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
-
1
-
2
-
3
of 3 Next