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 OlivierD. on -
William, this is impressive work. An IMU board that could interface with ArduPilot? That's like a dream.
This way, there could be IMU based autopilots for low budgets and the UAVDev board for higher budget.
Regardless, I am saving up for the UAV Dev board but I wouldn't mind seeing my ArduPilot work with IMU in a near future. It would be fun to experiment with both.
Congrats to you and Paul for the outstanding work so far. Truly inspiring stuff. I can't wait for actual flight reports. I am checking the stock situation for the UAVDev Board on sparkfun everyday, I am almost ready!
Regards,
-
Permalink Reply by William Premerlani on -
OlivierD,
Thank you very much.
I will give flight progress reports here as soon as I do some flying. I am afraid that I will have the firmware ready way before any of the places that I fly are ready. I live in upstate New York, and all of my fields will be bogs as soon as the rest of the snow melts. It has been a long, hard winter, and I am more than ready to go flying, but it may be a while.
best regards,
Bill
-
Permalink Reply by OlivierD. on
-
Same here, I am in Montreal, it's mud time for the next little while but spring is around the corner at last! The consolation : More time to build. I am building a 12 foot span glider with a power pod. For now, ArduPilot with FMA sensors but I have been watching your developments carefuly, regardless, I will buy a UAV Dev board because I think IMU is superior to IR. Too many lakes and trees where I fly so IR is not reliable, unless I hit an open field. I will purchase my UAV Dev by mid to late April, shortly after tax time! Until then, it's torture. Good think I have the mighty little ArduPilot as well to play with otherwise I would have to seek therapy! Chris and Jordi have done amazing work with this as well.
kind regards,
Olivier D.
-
Permalink Reply by Alvaro on -
Hi William, sorry for the intromission...
I will try to port the algorithm to the ARM Cortex M3 as it is right now. I just made the PCB and will start ADC testing and scheduling this weekend. I am trying to made a IMU board that will transmit serial data at a high speed to any microcontrolled autopilot. I was thinking about Sigma Point Kalman Filter to integrate gyros, accelerometers, magnetometer and GPS...
As you can read below in this post, I have a magnetometer too, could it be integrated in the "Premerlani-Bizard robust direction cosine matrix estimator"?
-
Permalink Reply by William Premerlani on
-
Alvaro,
A magnetometer could be integrated. In fact, the paper that describes the theoretical basis of our algorithm was written specifically for using a magnetometer for yaw adjustment.
BIll
-
Permalink Reply by UFO-MAN on -
Alvaro, I would be interested in taking a look at the Cortex M3 design you are working on. What dev board and what toolchain are you using?
UFO-MAN
-
Permalink Reply by UFO-MAN on
-
Hi William, the work you have done looks very promising. I have your board and I am uploading the code later this week to test. I would very much like to dig into the Mahony, Cha, Hamel paper but have problems to understand the jargon and terminology used. It would be very helpful if you (or other members of the group) could try to explain some of the terms in an understandable way. Below is my question list. Any help with questions in that list would be very much appreciated.
-What is a quaternion and why do we use that instead of vector notation?
-What is meant by a rotation group?
-What is a rotation matrix?
-What does it mean to maintain orthogonality of the rotation matrix?
-What is an anti symmetric matrix?
-Can you briefly explain kinematics in this rotation matrix context
-Can you briefly explain dynamics in this rotation matrix context
UFO-MAN
-
Permalink Reply by William Premerlani on
-
UFO-MAN,
Paul Bizard and I are writing a paper that will answer these questions in detail, its taking longer than we thought. In the meantime, here are some short answers to your questions.
All of this is concerned with rotations. Physically, what we are trying to do is represent the orientation of our aircraft with respect to the earth. There are several ways to do this. Mahony's paper discusses two alternate ways of doing this, rotation matrices and quaternions. In my opinion, rotation matrices are the best way to do it, so I am not going to get into quaternions.
A rotation matrix describes the orientation of one coordinate system with respect to another. The columns of the matrix are the unit vectors in one system as seen in the other system. A vector in one system can be transformed into the other system by multiplying it by the rotation matrix. The transformation in the reverse direction is accomplished with the inverse of the rotation matrix, which turns out to be equal to its transpose. (The transpose is just the swap of rows and columns.)
The rotation group is the group of all possible rotations. It is called a group because any two rotations in the group can be composed to produce another rotation in the group, any rotation in the group has an inverse, and there is an identity rotation.
Mahony's idea is that the rotation matrix that defines the orientation of your aircraft can be maintained by integrating the nonlinear differential equation that describes the kinematics of the rotation. This is done by recognizing that the integration can be accomplished via a series of matrix compositions.
However, numerical integration introduces numerical errors. One of the key properties of the rotation matrix is its orthogonality, which means that if two vectors are perpendicular in one frame of reference, they are perpendicular in every frame of reference. Also, that the length of a vector is the same in every reference. Numerical errors can violate this property.
The rotation matrix has 9 elements. Actually, only 3 of them are independent. The orthogonality property of the rotation matrix in mathematical terms means that any pair of columns (or rows) of the matrix are perpendicular, and that the sum of the squares of the elements in each column (or row) is equal to 1. So, there are 6 constraints on the 9 elements. In our implementation, Paul and I enforce the 6 constraints in a process we call renormalization, which will be described in more detail in our forthcoming paper.
An antisymmetric matrix is a matrix in which each element in the matrix is equal to the negative of the element with swapped row and column index. So, for example, if the element in the first row, third column is 0.5, then the element in the third row, first column must be -0.5. Also, the elements on the diagonal of an antisymmetric matrix must be zero.
It turns out that a small rotation can be described with an antisymmetric matrix.
In our case, kinematics is concerned with the implications of rigid body rotation. It results in a nonlinear differential equation that describes the time evolution of the orientation of the body in terms of its vector rotation rate. The direction cosine matrix is all about kinematics.
Dynamics in our case is the application of Newton's laws to describe the time rate of change of the rotation rate vector in terms of the applied torques.
By the way, the dynamics in Mahony's paper are NOT accurate for planes, they were concerned mainly with helis and vertical take-offs. Mahony's paper describes how to implement a combined orientation measurement and control algorithm. What Paul and I are doing involves kinematics only. We have completely ignored dynamics for now. The kinematics (rotation matrix) by itself is very useful for providing a basis for orientation control of model airplanes.
-
Permalink Reply by Garfiled on -
Bill,
Thanks for sharing so much with the community. I have read the Rmatrix algorithm in your pitch-roll demo, a question is about the roll_pitch_drift() sub. Would you please explain the meaning of gplane x rbuff[6 7 8]? I understand you use this term to correct gyro drift, but what is the physical meaning of the cross product? And how did your choose the KPitchRoll value?
Can't wait for the release of MatrixNAV, keep up the good work!
-
Permalink Reply by William Premerlani on
-
Garfield,
You also might want to take a look at my roll-pitch-yaw demo.
Rbuff and Rmat are direction cosine matrices stored in a linear fashion, row by row. Rbuff and Rmat are basically the same thing, just different stages of the calculation. The columns of the matrix are the projections of the axes of the plane projected on the ground frame of reference. The rows of the matrix are the projections of the axes of the ground frame of reference on the plane frame of reference.
The cross product of two vectors is a vector that is perpendicular to both of them, with a magnitude equal to the product of their magnitudes, times the sine of the angle between them. Since the cross product is perpendicular to both vectors, it is parallel to the axis that you would have to rotate one of the vectors around to bring it into parallel alignment with the other. This is perfect for what we want to do: we want to know how much more or less gyro rotation we would need to line the reference vectors up.
The cross product of gplane with rbuff[6 7 8] is measuring the misalignment between gravity as measured by the accelerometers (adjusted for centrifugal acceleration), and the vertical axis of the ground reference frame. It produces a rotation vector that you would need to bring those two vectors in alignments.
Regarding the feedback gains, Paul and I worked together. Paul performed lots of simulations to see how accuracy and transient response depended on the gains. I did a lot of walking around my neighborhood carrying the demo in my hands. There is a broad optimum for the roll-pitch gains. If you raise the gains, the recovery from gyro saturation is faster, but the accuracy is not as good. If you lower the gains, accuracy improves, but recovering from gyro saturation is slower.
In practice, there is a wide range of gains that will work for roll-pitch drift compensation.
Yaw drift is another matter. I found that the EM406 has about a 15 second delay in reporting course over ground. As a result, you have to keep the yaw drift feedback gains on the low side to prevent oscillations. You also have to account for the delay, which we partially account for by delaying the stream of Rmatrix vectors to the yaw drift calculation by 11 seconds.
The gyro feedback that I use in the rudder control loop keeps the yaw gyro from saturating.
Regarding MatrixNav, its almost done. I have made several test flights, working mostly on the elevator and rudder control feedback gains. I think I have the right gains now, but the weather forecast for the next few weeks is for rain and high winds. As soon as the weather breaks, I will go for one more test flight, and then I will release the firmware.
-
Permalink Reply by Garfiled on
-
Bill,
Thanks for the reply!
Now the error between
1. F: force vector (centrifugal acceleration compensated) as measured by the 3 accelerometers
2. Z: the [0 0 1] vector in ground reference frame which is converted to the plane frame as rmat[6 7 8]
is calculated, which is represented by a vector in the plane frame, "perpendicular to both of them, with a magnitude equal to the product of their magnitudes, times the sine of the angle between them."
Bear with me if the following questions are totally trivail, please :)
1. Do you use the assumption that the angle between F and Z is small, so that you can use sine(angle) as approximation of angle, such that the cross product result can be added to gyro input in rupdate(): VectorAdd( 3 , omegatotal , omegagyro , omegacorr ) ;
2. KPitchRoll relates omegacorr to omegagyro, is it possible to calculate a theoretical value of this factor, base on the value of omegagyro, so they fall in the same range? Maybe even normalize the omegacorr, and add the normalized vector to theta, which is ggain adjusted omegagyro?
3. If the plane is accelerating, then the force measured by the accelerometers is vector sum of gravity+accelertion, would that bring error to the attitude? I read one of your previous post saying that the attitude is mainly from gyro, so the accelerometers won't mess up the result too much. But according to the algorithm, the error is directly added to the gyro input, how do you justify that the effect of acceleration is small? Is it through ajusting the KPitchRoll? If you keep high throttle for a few second, will that affect the pitch output during that period of time, and consquently affect the control of the plane?
Thanks again for answering questions with all the details explained, I really learned alot reading through the discussions. Wish you guys all the best with the paper and flying test!
-
Permalink Reply by William Premerlani on
-
Garfiled,
Regarding the acceleration of the plane along the roll axis, there are two factors that prevents any significant errors from appearing in the direction cosing matrix as a result:
1. It is acceleration, not velocity, that would cause an error. When you keep high throttle, there is an acceleration, but the time duration is very short until the plane reaches a new equilibrium velocity. It is very difficult to produce sustained large accelerations for a long period of time. The direction cosine matrix (DCM) acts as an integrator, so it filters out short duration transients. What matters is the acceleration times the duration of the acceleration, which is equal to the change in velocity.
2. The feedback gains, such as KPitchRoll, are set to relatively low values. The feedback is very low, only enough to prevent drift, so any error is attenuated before it is fed back.
There is a situation that sheds some light on your question: hand launch.
I test my flight firmware on a Gentle Lady, which I hand launch.
With my previous firmware, "GentleNav", I used mostly accelerometers for pitch control. Because of the effect that you mention, the accelerometers would go crazy during hand launch, and I would have to turn pitch stabilization off during launch.
I am presently in the final stages of testing "MatrixNav", which is based on DCM. It is so accurate regarding pitch, that I can finally turn on pitch stabilization during hand launch. There is zero detectable response of the elevator to the high amounts of acceleration during the hand launch.
Best regards,
Bill
Top Content
Groups
-
ArduCopter User Group
477 members
-
ArduPlane User Group
299 members
-
ArduCopter (traditional …
77 members
-
ArduPilot (legacy) User …
77 members
-
Moderators
50 members
-
ROS User Group
31 members
-
Team Prometheus
11 members
© 2012 Created by Chris Anderson.
