robust estimator of the direction cosine matrix
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
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
Replies to This Discussion
-
Permalink Reply by Navigator on -
Hi Greg,
I am working on DCM code for getting orientation of smartphone, if you still remember, You have mentioned in your comments that to use DCM elements directly rather than conversion to Euler Angles using the atan function, Cn you plz explain a little bit that how I can get these angles(roll pitch yaw) from DCM.
regards
navigator
-
Permalink Reply by Greg Gibbons on -
Hi navigator,
I don't think the way I did it actually worked, so I used the Euler angle conversion.
Greg
-
Permalink Reply by Navigator on
-
Hi Greg,
then how you managed to get rid of gimbal locks, because in my case the orientation works as long as pitch angle is less than 90, over 90 it distorts the roll value,
regard s
navigator
-
-
Permalink Reply by William Premerlani on -
Navigator,
Here is a work around to gimbal lock.
Another approach to solving gimbal lock is simply redefining the meaning of the angles for 90 degree pitch. For example, for 90 degree pitch, you could define new angles (not Euler angles) that relate the orientation to a defined orientation that has a 90 degree pitch. There are probably some discussions on the web about solving gimbal lock, and still use angle.
The third approach is to not use Euler angles, but use axis and angle, or quaternions, to describe the orientation. Quaternions for example do not suffer gimbal lock.
Here is a method to convert a rotation matrix to a quaternion.
A very good book on the subject is 3D Math Primer for Graphics and Game Development.
Best regards,
Bill Premerlani
-
Permalink Reply by Darryl Uchitil on
-
Sean,
Nice video and I love the dashboard you are using. I am just getting started with this hardware for a land-based rover, and I see a lot of debugging ahead. What are you using for the dashboard display, and can I get a copy?
Thanks,
Darryl
-
Permalink Reply by Sean McDonald on
-
Darryl,
The dashboard was just a quick and dirty visual basic application using the artificial horizon provide by Tom at http://tom.pycke.be/?pg=2. It also depends on the telemetry data stream format etc. Since this video was done, I have implemented the magnetometer etc and upload on parameters etc on the fly. I have also changed the dashboard a fair amount. The dashboard did help me some for debugging and tuning.It is fairly easy For one to code up.
Regards
Sean
-
Permalink Reply by Darryl Uchitil on
-
Sean,
Thanks for the artificial horizon link. Regarding your dashboard, it is often easier to modify an existing application rather than create(design) one from scratch. That said, I am somewhat less than a VB guru, and have only dabbled in VB at work. I will probably put something together in Java because it is a little like C and C++ that I am used to.
But it is easy to spend a lot of time on the visuals, I wonder if you could send me a clear screen shot (old or new) so I have a starting point on the organization and complexity required.
Thanks again,
Darryl
-
Permalink Reply by Sean McDonald on
-
Hi Darryl,
I am certainly no expert VB programmer and so my application is by no means the most efficient nor does it follow best coding practices. Attached to this reply should a copy of my updated work in progress version - no warranties or guarantees includes - perhaps it helps you with your project.
What type of land vehicle are you looking at controlling?
What hardware are you using?
Have you coded the DCM?
Regards,
Sean
-
Permalink Reply by Darryl Uchitil on
-
Sean,
In the long run, I want something to cut my lawn (2.5 acres). For now (testing) I have a salvaged Power Wheels run by a 32-bit Coldfire-based industrial computer which was left over from a project at work. The lashup has GPS and I plan to send it DGPS corrections over WiFi. But GPS does not update fast enough and if It should go under a tree and lose the GPS, it needs dead reckoning to recover and otherwise fill in the gaps.
For now I plan to use the Red board that Mr. Premerlani has been developing for. I hope to move the XRATE analog to a different pin, so I can use the board as a SPI slave. This gives me two serial ports that I can use for the GPS and the DGPS.
I have not purchased the Red board yet, both for budgetary reasons and because I am not ready. I have some mechanicals to work out yet (steering) and a bunch of control software to write to make use of the ultrasonic and IR range sensors that I recently installed.
Thanks for the software, I am sure it will keep me busy for a while.
Darryl -
-
Permalink Reply by William Premerlani on
-
Bryan,
Thank you very much for helping. I am sure that Sean, you, and I will get his code working eventually.
Best regards,
Bill
-
Permalink Reply by William Premerlani on
-
Sean,
I am not entirely sure what is going on, but here are some clues that might help you get everything finally working:
1. Without the feedback gains turned on, the algorithm will drift. You can detect drift rate by setting the feedback gains to zero, starting the algorithm up, and then just watch matrix elements, they will indicate drift.
2. It sounds like you have the gyro gains about right, to get the 45 degree reponse.
3. If you rotate your board too fast, you will exceed the gyro maximum value. This will result in an angle error. Possibly after you rotate 45 degrees, you are rotating back too fast. What is the maximum range of your gyros?
4. Regarding your PI gains, there are two likely problems. Possibly there is a sign error somewhere in your coordinate system, or the sign of your gyros. In that case, the accelerometers might not agree with your gyros. If that is the case, the drift cancellation will generate a large transient that will eventually settle down. The other possibility is that there is a misunderstanding of where the binary point is in my implementation, so you might have the effective gains multiplied by 65000, which would cause it to break into oscillation.
5. Regarding signs, on my board, the Y axis gyro is mounted backwards, so I flip the sign. Also, my firmware is aligned with the accelerometer on the board, not with the usual aircraft convention. In my firmware, Y is forward, X is to the left, Z is down.
6. In my implementation of DCM, I am using 2.14 fractional format with an implied binary point. There is one sign bit and one integer bit, and 14 fractional bits. So, there are lots of factors of 2 and 4 floating around. In many places I use the dsPIC hardware multiplier to multiply two 16 bit integers to get a 32 bit integer. If you let the compiler generate the code to multiply two 16 bit integers, it will give you a 16 bit result, unless you use casting liberally to force it to do what you want.
7. I do not bother to normalize the gravity vectors in computing the roll-pitch drift. Since the magnitude of gravity is constant, I simply do not bother to normalize. But I am careful to take into account the value that I expect to see.
8. In the earliest versions of my firmware, I did not have the initial values of some of the variables set to their correct values. This generated a transient on startup. You might take a look at the latest version of roll-pitch-yaw, or MatrixNav, or AileronAssist.
Best regards,
Bill Premerlani
-
Permalink Reply by Sean McDonald on
-
Bill,
Thanks for all the great ideas. I think that as I have converted to code to try and use it without 2.14 format, just using the maths on the ARM7 using floating points (doubles). You are likely correct and this is likely where something has gone wrong in my port. I may just have to rewrite it to use 2.14 format on the ARM7 as this is likely where mine has gone wrong...
I believe I may obviously misunderstand the dsPIC compiler results for ADC conversion etc. and the 2.14 format conversions going on.
In my code I have 10 bit ADC result. So my readings for gyro and accel have range 0-1023 (with zero gyro movement giving me approximately 511 at mid point. Accel_x and Accel_y gives me 511 while Acc_z gives me approx 613 for 102 being 1g) I see your RADPERSEC defined for 'one radian per second, in AtoD/2 units' is 5632 and GRAVITY as 5280 (gravity in AtoD/2 units)
Is this all in 1.15 format?
The dsPIC has 10 bit ADC correct?
Are the ADC results in 1.15 format, for 'xaccel.value' for example?
At rest, are all xaccel, yaccel and zaccel values = 0 (close to it except for noise in measurement)?
Similar for gyro readings?
What does the dsPIC compiler term 'fractional' result in a value of? For example, the value of ggain in your code seems to be (6 * 1 * 0.25) = 1.5. Is ggain in your code then equal to 0.5? Does 'fractional' then just take the 14 fractional bits and ignore the integer bit?
Thanks for being so patient with me on this.... I am also going to review wiki for a better understanding of vectors and rotation matrices etc.
Thanks
Sean
Social Networking
Top Content
Contests
Season Two of the Trust Time Trial (T3) Contest has now begun. The fourth round is an accuracy round for multicopters, which requires contestants to fly a cube. The deadline is April 14th.A list of all T3 contests is here.
Groups
-
German Ardu-Group
106 members
-
ArduCopter User Group
1279 members
-
ArduPlane User Group
671 members
-
ArduRover User Group
183 members
-
Andropilot Users Group
182 members
Advertisement
© 2013 Created by Chris Anderson.
Powered by
