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
I would like to drop in a comment about the need for wind estimation in or around DCM.
I believe the original design of DCM was to *let the wind yaw the matrix*, in this way the UAV began to compensate for the wind, and could still navigate. How quickly it did this depended on the yaw drift correction. This was probably because compass corrected DCM was expensive, or rarely in place. It did well on long legs, and poorly on turns.
My opinion is that wind has no place in DCM, and it does not need estimating. the DCM should show the correct attitude and correct yaw, all the time, and not assume that the plane moves in the direction the nose is pointing. Precisely because of wind. Wind is compensated at a navigation level, using the GPS ground track. If the plane has to settle on a pronounced yaw with respect to the ground track, to move along said ground track, that is what it does. You should not need to estimate winds (which are inherently variable anyway) to get a good result, the GPS ground track reveals the wind at all times. The job of flying in a cross-wind, or flying in a variable wind is nothing to do with the DCM, and is everything to do with the navigation layer. A good navigation layer responds dynamically to the presence and variability of wind, or the difference in ground speed and air speed.
I'd be interested to know where exactly in a full sized plane wind is continually estimated and input into flight control movements. For final approach a guess at wind is useful but ultimately how the plane responds second by second trumps any wind estimation. It can be inferred, yes, by your movement over ground versus the compass heading of the nose, and your airspeed, but is merely an interesting and often variable by-product of navigation over ground, rather than a vital bit of information needed up front. I feel this idea has sprung from the original DCM paper, which talked about the need for wind estimation to improve the way yaw was being drift corrected.
Someone convince me that continual wind estimation and input (where?) is vital for an autopilot.
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 Bill,
It seems we are thinking along similar lines, although I am more focused on quaternions (and related parameterizations, at least for the moment) and you are sticking with rotation matricies. Once you know omega and delta_t, you can find the "exact non-linear" delta rotation. The only downside is that the delta rotation computation requires trig functions in all representations (as far as I can tell). Are you planning to use low-order talyor series approximations in your calculations? CORDIC functions? Table lookup? Something else? Since you should know your max omega and your delta_t, you know how large delta_theta could get. I'd imagine its still pretty small such that low order trig approximations would work pretty well.
- 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
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,
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
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
-
1
-
2
-
3
-
4
of 4 Next