If you downloaded MatrixNav from this page before 4/29/2009, you should be aware that there is a newer version of the firmware, MatrixNavRv2, that reduces the GPS latency, and will perform much better than the first version.

I have been working with Paul Bizard on something we call the "Premerlani-Bizard robust direction cosine matrix estimator". It is based on the work of Mahony et al. The idea is to continuously update the 3X3 matrix that defines the relative orientation of the plane and ground reference frames, using GPS and 3 gyros and accelerometers. The basic idea is to use gyro information as the primary link between the two reference frames, and to use GPS and accelerometer information to compensate for gyro drift. We are working on the theory together. Paul is performing simulations. I am testing ideas in my UAV DevBoard. We have made a great deal of progress. There are demos available, and control and navigation firmware is available. The steps of the algorithm are:
1. Use the gyro information to integrate the nonlinear differential equations for the time rate of change of the direction cosines.
2. Renormalize the matrix to guarantee the orthogonality conditions for a direction cosine matrix.
3. Use speed and gyro information to adjust accelerometer information for centrifugal effects.
4. Use accelerometer information to cancel roll-pitch drift.
5. Use GPS information to cancel yaw drift.

By the way, the algorithm should work in any GPS, gyro, accelerometer nav system on a plane. Without magnetometer information, it will not work on a helicopter.

This discussion will provide progress reports from time to time. At this point we have completed all steps. Firmware and documentation for various demos and flight firmware are available on the UAV DevBoard main page.

Firmware and documentation of a roll-pitch-yaw demo program are available. There is also a first draft of an explanation of the algorithm.

If you have a UAV DevBoard, I highly recommend that you try the demo program, it is very easy to use, and runs without a GPS. During its development, I found that the gyro drift was much less than I thought it would be. After I added the drift compensation, the resulting roll-pitch peformance is nothing less than astounding.

Flight testing of "MatrixNav" is also complete. Firmware and documentation are available on the UAV DevBoard main page for stabilization and return-to-launch functions for inherently stable aircraft that are controlled by elevator and rudder. MatrixNav is implemented with a direction cosine matrix, and supercedes GentleNav. Anyone who has GentleNav should replace it with MatrixNav. Pitch stabilization is excellent under all conditions. Return to launch performance is excellent under calm conditions, and good under windy conditions. If you have the UAV DevBoard and an inherently stable plane, you will definitely want to try out MatrixNav.

Finally, AileronAssist, for the stabilization and RTL aircraft that have ailerons, is available.

What Paul and I are going to tackle next is altitude control.

Bill Premerlani

Views: 24655

Reply to This

Replies to This Discussion


Good point about needing the velocity of the plane. I note that Mathias is using pitot tubes. That will give him the velocity information.

By the way, it turns out that air velocity, not ground velocity, is the correct variable to use in the calculation of centrifugal compensation. In the past, I was mistaken about that. Since then I took a closer look at the math and the physics, and realized that in order to get the acceleration in the ground frame of reference, you actually should use the air velocity in the computations.

Ground velocity will give you an approximate answer that is good enough, as long as the winds are not too strong. So, we use ground velocity from GPS in the UAV DevBoard firmware, because that is all we have. It seems to work well enough, as demonstrated by many flights, with many different airframes. But airspeed would have been better.

Best regards,
Bill Premerlani
The pitot tubes are always mounted on same direction with the X axis of plane.
Only when plane fly forward it can indicate the airspeed of plane.
But for vertical take off and landing aircraft which can move in any direction, the pitot tubes can still work well?
If they doesn't work. The gps will be only choice for vertical fly aircraft?
Because it's impossible to mount the pitot tube in every direction.
When the vertical fly aircraft hovers in the air. The calculation of centrifugal compensation will stop(gps requires movement).
But I don't worry about it. Because the same will occur to the centrifugal.
I don't know whether my understanding is right? Thanks.

It was my impression that Mathias was talking about a fixed wing aircraft, so its fine there.

For a helicopter, its a different problem all together, but I don't want to get into a discussion about helicopters tonight.

For a plane that can do vertical takeoff and landing, a pitot is fine, because the centrifugal effects are not strong, until the plane gets up to a good speed, in which case the air will pass parallel to the chord of the wing. So, what you need to do to compute centrifugal effects is the wind speed over the wing, which you can measure with a pitot that is fixed to the wing, and you need to know the angle of the wing with respect to the plane, which should be available information. So, the problem is a little more complicated, but still tractable.

Bill,I am very grateful for your help. Now I'm preparing to build a quadrocopter.
So l always can not help thinking about how to use DCM on a vertical takeoff and landing aircraft. I'm very sorry I interrupted the discussion.
Best Regards
Jack Chen
Dear Bill, I have a question about the integration period of PI Controller of DCM.
It seems to be a long term period which covers whole flight time.
It doesn't like other general PID Controller which limitted only in a very short time gap.
Hi Jack,

The gains of the PI gyro drift controller can be low because the residual drift of the gyros is low. The residual drift is what is left over after the initial calculation of the gyro offsets, which are computed while the gyros are motionless during power up.

In the case of the roll-pitch drift compensation, its pretty easy to do, and works over a wide range of gains, because there is plenty of data from the accelerometers, and all you have to do is compensate for centrifugal effects and filter out the variations due to transient accelerations.

In the case of yaw drift compensation, the gains have to be high enough to track the wind, but not too high, so that noise is filtered out.

For the gains that we have selected, the time constant of the drift compensation is about 10 seconds.

Hi Bill,

I am currently trying to solve the following issue but do not seem to make much progress, and was hoping you could maybe shed some light on the subject (will be looking into it again tomorrow).

I am implementing the DCM algorithm for use in a land based vehicle and it has proven to work quite well (thanks!). The last few yards is now to get it to work correctly when the vehicle is reversing. This is a bit tricky since GPS speed is always positive. I jumped that hurdle by simply integrating the accelerometer value for a few seconds after GPS speed is zero, getting a direction indication easily enough (I know this is not 100% failsafe, but it is acceptable for my application).

In the forward direction the centrifugal effect is compensated for nicely when turning, but in the reverse direction the corrected pitch is actually tracking the raw roll angle (instead of being compensated) and vice-versa. Difficult to explain without a graph, but I hope you get the idea. Is there something obvious that I am missing for the reverse direction when it comes to the centrifugal correction.

I am talking about this bit of code of course:

gplane[0]=gplane[0]- omegaSOG( omegaAccum[2] , (unsigned int) sog_gps.BB ) ;
gplane[2]=gplane[2]+ omegaSOG( omegaAccum[0] , (unsigned int) sog_gps.BB ) ;

Hi Elardus,

I will answer your question after I have time to think about it, but it begs the question:

In a land based vehicle, why do you need to track roll and pitch?

When I started out, I was using an RC truck. I found that an amazingly simple thing worked: I steered using GPS only, and used a gyro to measure turning rate, to provide damping for stability.

It worked like a charm. You would think it would have trouble at zero speed, but not so. What would happen was that when you started up, it went in a random direction until there was enough GPS information, then it would swing toward the desired direction. After that, everything was fine.

So, I am sure that you have a good reason for not using a simple approach. I would love to hear what it is.

Best regards,
Hi Elardus,

As you mentioned, the GPS speed over ground (sog_gps.BB) will always be positive. In the following equations, the assumption is made that the vehicle is going forward:

gplane[0]=gplane[0]- omegaSOG( omegaAccum[2] , (unsigned int) sog_gps.BB ) ;
gplane[2]=gplane[2]+ omegaSOG( omegaAccum[0] , (unsigned int) sog_gps.BB ) ;

If the vehicle is going backwards, than you need flip the signs:

gplane[0]=gplane[0]+ omegaSOG( omegaAccum[2] , (unsigned int) sog_gps.BB ) ;
gplane[2]=gplane[2]- omegaSOG( omegaAccum[0] , (unsigned int) sog_gps.BB ) ;

I am not sure if this explains why you see what you are seeing, but possibly.

Of course, you have a similar issue in the yaw drift correction algorithm that is based on course over ground. What will happen when you reverse is that the GPS will flip the course over ground by 180 degrees. So, you have to take that into account.

Best regards,
Hi! Im reading the DCM matrix IMU:theory draft bye William Premerlani and Paul Bizard. But I lose the thread from eqn. 15 to the matrix eqn.17. Can any one help me on whats happening here?? How do you get that matrix???
Hi Geir,

This is a frequently asked question, so I finally prepared a few pages explaining the equivalence of equation 15 and 17 of that paper that you are referring to.

Best regards,
Thanks what a greate answer :) Starting to get it now :) But I have more questions. You say that the matrix is the same result as Manhoney's get. Were is that?? I see omega_x is the same only whit diag 0's instead of 1.
And is using a PI loop like you do the same as a complementary filter???? Thanks in advance this will help a lot :D

Reply to Discussion


© 2020   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service