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 William Premerlani on -
Adrien,
Regarding the velocity vector used in the centrifugal compensation calculation, we are ignoring all of the effects that you mention. We assume that the velocity is in the direction the aircraft is pointed.
We need an approximate answer that is accurate on the average, for high centrifugal acceleration. Most of the attitude information is from the gyros, not the accelerometers. So we are talking about an adjustment on an adjustment, its a second order effect.
The centrifugal acceleration is highest when the aircraft is moving at high velocity. In that case, on average, it moves in the direction that is pointed. So the approximations that we are making are best precisely when they need to be.
Test flights so far have shown that the compensation works rather well.
Regarding the wind, what we do is use PI drift controller gains that are large enough to compensate for wind in a few seconds. Whenever the IMU turns in windy conditions, it adjusts to the wind. When the aircraft makes a turn in windy conditions, for a few seconds it will remember the previous adjustment. However, the PI drift controller will use the GPS information to readjust in a few seconds. The result is that the aircraft will "crab" by exactly the right amount to maintain the desired course over ground.
Having said that, I hasten to add that Louis LeGrand is working on seeing if we can do better than that, to see if we can adjust for gyro drift and calibration error, as well as determine the wind vector, using accelerometers, gyros, GPS, and DCM. He has some promising ideas, but the work is not done yet.
Best regards,
Bill
-
Permalink Reply by AdrienB on -
Hi Bill,
Thanks for the reply !
"Test flights so far have shown that the compensation works rather well."
I have no doubt that the compensation already works rather well, (even without compensation the complementary filter should work in many cases). Still, I think the angle-of-attack estimation presented in Mahony's paper is here for a reason (I'm trying to understand why, since they put a lot of effort in identifying the longitudinal model, etc...).
In fact, let's think of a situation where the plane is constantly turning with the same radius. Since it's a long period of time, won't the estimator eventually adjust to what the accelerometers say ? Therefore, my supposition is that the compensation should be as good as possible with "accelerometer measures" minus "estimation of centrifugal forces" being as close as possible from gravity, in order to get the good orientation during constant turns.
Best,
Adrien
-
Permalink Reply by William Premerlani on
-
Hi Adrien,
Thanks for your continued interest and discussions and for reading Mahony's papers.
Regarding the compensation for centrifugal acceleration, and the angle-of-attack estimation in Mahony's paper....
Angle-of-attack estimation is in Mahony's paper for good reason, there are some calculations that you might want to do in which it would be nice to have a good estimator for the angle-of-attack, such as in altitude control, or in aerobatics, for example.
I agree with you that if you want to exactly cancel the centrifugal acceleration, you need to include angle-of-attack.
However, for what I have done so far, return-to-launch and stabilization, I have been able to get by without estimating the angle-of-attack. Eventually, when I get into aerobatics, or tight altitude control, I might need it. But I do not think it is needed for centrifugal acceleration compensation in any case.
For compensation for centrifugal acceleration, there are a few reasons why I can ignore the angle-of-attack to simplify the calculations.
1. I am only trying to compensate approximately for centrifugal acceleration. I get good performance in continuous, banked, turns if I remove most of the effect, I do not have to get rid of all of it. There is a certain error that you get in a banked turn if you ignore centrifugal all together. You can get rid of most of that by doing the compensation without including the angle-of-attack, leaving a small residual error.
2. In the situation that you describe, a continuous banked turn, the residual centrifugal error is small if you ignore angle-of-attack, because the rotation vector is approximately perpendicular to the velocity vector. The rotation vector is approximately perpendicular to the earth. The velocity vector is approximately in the horizontal plane. The two vectors are approximately perpendicular to each other. Therefore the magnitude of the cross product is about as large as it can be, and the error is approximately equal to 1 minus the cosine of the angle-of-attack.
3. In return-to-launch and stabilization, I can live with a small amount of error in the estimate of the bank angle.
4. The effect that I am accounting for becomes important only at high speeds, in which case the angle-of-attack becomes smaller.
Best regards,
Bill
-
Permalink Reply by AdrienB on
-
Hi Bill,
I understand your remarks and it indeed makes sense that the "1 minus the cosine of the angle-of-attack" error should be negligible.
Thanks for the clear explanations,
Best,
Adrien
-
Permalink Reply by Sean McDonald on
-
Hi Bill,
With yours and Louis' previous help in posts, I now have the a matrixnav DCM ported to my ARM7. It seems to work, so long as I don't set any KPPitchRoll and KPitchRoll gains. I now have the gyros giving a good approximation of the change in attitude in roll and pitch (I am not dealing with yaw to start). The problem is that the DCM does not return to position close to that prior to any rotations. For example, If I place the gyros and accel on desk at 0 deg, then move 45 deg pitch angle, hold a second, then return to 0 deg on desk and let go, the DCM gives me approx 45 deg (so my gyro gain should be close), but then returns to something like 10 or 15 degrees pitch when it should be closer to 0 deg pitch.
When I then try and set some KP and KI gains, what happens is that the PID adjustment then has a runaway... as I can't see any limit set to the KI gains as one would normally have in a PID algorithm. I was thinking the the accels would be used to counter gyro drift and also assist in the correction of gyro errors accumulated.
The gplane variable is set to be [0 0 1] (x=0, y=0, z=1) to start, then updated with accel readings (adc units scaled g's, so should be 0 0 1 with unit standing still) to which are adjusted by the zero offset calibration on startup. As rmat is normalized prior to the roll_pitch_drift function, I normalize my gplane vector as well, before I do the crossvector of gplane and rmat.
Does the gplane variable related to x y z accel? which then correspond to same x y z gyros? Is errorRP vector also error in x y z?
I don't get why it should run away when I set PID's...
I continue to reread the technical papers on this algorithm... I hope it will sink in soon...
Thanks
Sean
-
Permalink Reply by bcr on -
Hi Sean,
Are you willing to share your ARM7 code? I'm currently working on an STM32/x86 Linux port myself. I've created basic vec + mtx math functions to replace the dsPIC libraries, and built a few simple SW test cases (don't have HW yet and in any case, I want to study and verify the algorithm on a PC first). Right now I am trying to get basic 1 gyro axis rmat updates working, first in floating point, then 2.14 fixed point (my math functions make it easy to switch between the two). Bill's code is great but it would be helpful to see another implementation, especially for such a similar MCU.
Regarding your question, it's just a guess, but you may be having an overflow problem with the fixed point math. I think Bill's code maps the HW accel range (say, -1g to +1g) onto -1/2 to +1/2 (in 2.14) for each gplane axis. That's a simple divide-by-2 in Bill's code since the dsPIC HW provides the ADC samples in 1.15 (I suppose 0 is 1.65V, -1 is 0V, +1 is 3.3V). This is an unusual HW feature, most MCU ADCs just give you a raw right-justified 10-12 bit integer in a uint16_t, so you'll need to convert to 2.14 yourself to reuse the rest of Bill's code.
So, at rest with +1g on the Z axis due to Earth gravity, I think gplane should be [0 0 1/2] assuming your accel is in 1g mode (divide by 6 for 6g mode). You should not need to normalize gplane (scale it to have magnitude 1) and doing that might cause an overflow later on (I'm inferring that Bill picked +-1/2 instead of 1 to avoid overflow/underflow). Finally, you need to pick the KPitchRoll constants carefully so that you do not overflow 2.14 during the VectorScale in roll_pitch_drift(). It looks non-trivial to calculate a formal upper bound (depends on the maximum possible errorRP) so I would leave a lot of margin. dsPIC can HW saturate but ARM will just wrap around, wrecking the calculation
Bryan
-
Permalink Reply by Sean McDonald on
-
Thanks Bcr.
Let me first tidy up my code over the next few days as I work through it more with the new insights from Bill below that I had not appreciated fully.
Thanks
Sean
-
Permalink Reply by Sean McDonald on
-
Hi Bryan,
I cleaned up the code a little... still rough... attached file here.
I have not yet ported the yaw compensation parts... need to work on that as I want to use magnetometer for yaw compensation...
I also need to look into a few things...
1. the DCM gives me some roll when I move the IMU board around the earth Z axis when I have it in a pitch attitude other... seems like the amount of roll is more with greater pitch up or down...
2. the DCM gives me pitch up or down, if I move the IMU board around the earth Z axis after I have the IMU board say in a roll at 45 degrees banked. The pitch gyro is obviously integrating to give a pitch impact as it would see some pitch and yaw gyro outputs as it is at a 45 roll attitude. I need to eliminate that. Is that solved by implementing the yaw compensation?
Anyways, my progress so far is shown in a quick (sorry about quality) video.
Any suggestions, comments etc.?
Thanks.
Sean - Attachments:
-
-
matrixnav.c, 15 KB
-
-
Permalink Reply by William Premerlani on
-
-
Permalink Reply by Sean McDonald on
-
Thanks Bill,
The write up really helps and puts more clarity around a number of concepts that I now have a better handle on... With the soak time, I think that the whole DCM algorithm is now really making more sense to me. I add made first attempt at using magnetometer (3 axis compass HMC6343 - tonight I was just using this units standard roll/pitch compensated output that it has with internal accelerometers...for now... need to use raw data and adjust for DCM roll and pitch estimates...which are based more weight on gyros).
I now need need to work on getting the gains better to just remove any drift without overcompensating the results, but the DCM seems far more stable now with some adjustments I have made based on your draft explanation paper.
Again, many thanks for your work and assistance as I work through this....
Sean
-
Permalink Reply by Greg Gibbons on -
Sean,
Thank you for sharing your code.
Would you mind posting the 4 header files listed in your matrixnav.c file, I am working on porting this to my LPC2368 AP.
Thanks
Greg Gibbons
-
Permalink Reply by Sean McDonald on
-
Hi Greg,
I see that I don't need the header files.... they were carryovers from other code... (at least the first two header files):
a) "uNav/matrix.h" - was just a set of matrix routines I had compiled from uNav1.5 code - not being used in this DCM implementation. For now, my code has all matrix routines embedded in this c file.
b) "matrixnav.h" - this is just Bill header file.... I have not used its components - just #define RMAX 1.0
c) "imu.h" - this is just a header file containing a data structure for accel and gyro readings, zero point calibration values, compass heading etc... my main data structure for consolidating key data.
d) - this is just a std compiler header file for cos sine and tan functions etc...
For what its worth, just some of my key finding/learnings have been:
1. ensure that the gyro raw data is filtered sufficiently to remove unwanted noisy signals, which seemed to be causing most of my previous frustration with run away DCM elements...
2. ensure that rmat normalization code is working and that rmat is orthogonal before playing with drift gains;
3. relearning about matrix and vectors etc.... (not being an engineer, I did matrix maths some time back in high school, so getting refresher on vectors etc really puts things in perspective)
4. using the DCM elements directly (cosines of roll and pitch) rather than conversion to Euler angles using the atan functions etc. that I had in the code I uploaded earlier. That seemed to be causing some unwanted coupling effects (change in pitch as a result of change in yaw etc)
Hope this helps to provide you with further insights.
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
-
Moderators
24 members
-
Documentation Editors
51 members
-
ArduRover User Group
184 members
-
Japan ArduCopter Group
51 members
-
Taiwan ROC ArduPilot Gro…
8 members
Advertisement
© 2013 Created by Chris Anderson.
Powered by
