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
-
DeveloperPermalink 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 -
-
DeveloperPermalink 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
-
DeveloperPermalink 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
-
DeveloperPermalink Reply by William Premerlani on -
Sean,
We are closing in on this. The chief issue is probably some factors of 65536 here and there that you might have missed in doing the port because I am using integers throughout, and you are using floating point.
If you can get the floating point to work, in the end it would be better for you, because it is conceptually simpler. I started out with integers, because it is the most efficient, and I was not sure how much CPU power I would need. So I do not recommend that you switch over to integers, unless you run out of CPU power.
There are a couple of places in my code where I multiply two 16 bit numbers to produce a 32 bit result, and then I take the upper 16 bits as the result. This is equivalent to dividing by 65536. So this could throw you off a bit if you simply took my code and replaced integers with floats.
In my code, I use 16 bit and 32 bit integers to represent 2.14 and 2.30 fractional values. The compiler does not do anything special about the fractional format, it is entirely up to your code to do something about the implied binary point. The MicroChip dsp library has some routines that assume 1.15. I wrote some routines to work with 2.14.
In addition to the factors of 65536, which are probably chiefly responsible for the issues that you are seeing, there are some factors of 2 and 4 floating around in my code. For example, ggain is actually two times what I really want, to compensate for a divide by two that I do when reading the gyros.
Most (but not all) of the 16 bit integers in my code surrounding the DCM calculation are in 2.14 format.
By the way, the ggain in my code that you refer to is 6*1*0.025=0.15, not 6*1*0.25=1.5
Regarding the A/D, I am using a 10 bit A/D which is reporting its values in left justified format into a 16 bit integer. So my range is from 0 to 65535. So there is factor of 64 that you also have to take into account in porting my code if you are using right justified.
Best regards,
Bill Premerlani
-
Permalink Reply by Sean McDonald on
-
Bill,
Thanks for the further insight on this. I am going to study this thread contents in more detail again, as I work through my port of your code again.... I am also going to try to understand the Mahony papers a little better as I think that will be key for me to move forward.
As you say, we may be narrowing down my problems... I have coded up a simple UI with artificial horizon app, so that I can see the output of my code.... the output results seem to be generating more of an oscillation..... with some KP and KI set, I have seen that the results may actually be oscillating either side of pitch and roll axis, So likely I have not taking into account correctly the differences in A/D output justification and upper 16 bits results of the 2.30 fractional values.
I appreciate all your work on this algorithm etc. as well as your continued help to understand this all a little bit better each time....
Regards
Sean
-
Permalink Reply by michelle hughley on
-
how do i convert acceleration and velocity from body frame to ECEF frame with gravity?
given: long, lat, alt, bearing angle, time, and body attitude
Contests
Season Two of the Trust Time Trial (T3) Contest has now begun. The third round was a reliablilty/aerial photography round for both planes and copters, which is now closed. Stay tuned for the next round, beginning soon.A list of all T3 contests is here.
Top Content
Groups
-
DIYDrones - CNC Project
61 members
-
Moderators
57 members
-
ArduPilot (legacy) User …
95 members
-
ArduCopter Evolution Team
108 members
-
ArduCopter User Group
617 members
© 2012 Created by Chris Anderson.
