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 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
-
Permalink Reply by William Premerlani on
-
Garfiled,
A couple more things relative to the effect of forward acceleration on the direction cosine matrix algorithm:
1. What is important is the average forward acceleration over a long period of time, which is zero.
2. Whatever errors are generated, they are removed by the feedback loop eventually.
Bill
-
Permalink Reply by Dave_fr on -
Looking forward to reading the "forthcoming paper" !
Very intresting, thanks for sharing !
-
Permalink Reply by William Premerlani on
-
Dave,
The weather is going to be bad this weekend, so I am going to work on the paper. I hope to have it done in a few weeks, by the end of April at the latest.
-
Permalink Reply by Dave_fr on
-
I'm busy learning about the Kalman filter. This mathematical tool is amazing. I'm getting to have some ideas. I own a small rc autogyro ( Rotorshape ) which exibits an amazing pitch and speed stability. It can be flown without making any use of the pitch control, the rate of climb being controlled by the engine power, the speed remaining pretty constant. Furthermore, it doesn't stall.
I would like to take avantage of these flight carateristics to create a basic stabilisation unit using 8 or 16 bit pic microntrollers and 2 gyroscopes sensors.
I would use the first gyro to read the rate of bank ( rad/s ) then integrate this value.
The second gyro would read of rate of turn and corrected the bank throught the feed back loop working out at each iteration the new gain. (kalman)
Obviously the relation between the rate of turn and the bank depends on the speed which is concidered constant and fully determined but, it isn't lineare ;( .
Maybe you can give me or point of view on this projet (as long as it doesn't slow you down in the writing of your paper ;-) ). The autogyro airframe is very draggy but also very light in and doesn't have much problem with control gain throuhout the speed range remaining equally responsive at all times.
Best regards
Dave
-
Permalink Reply by William Premerlani on
-
Dave,
This would seem to me to be a perfect application of the direction cosine matrix (DCM) approach, which will give you the orientation of your aircraft, especially the bank information, without any model information about the dynamics of your aircraft, though you would need three gyros and three accelerometers to do it. My paper on the subject should be done by the end of April. In the meantime, if you have not seen it, you might want to read Mahoney's paper on the subject.
The direction cosine matrix "respects the nonlinearities of the rotation group" whereas linearized Kalman filter approaches do not. It is my opinion that DCM with 6 DoFperforms better than linearized Kalman with 4 DoF.
There are fully nonlinear versions of the Kalman filter, but what they do is to combine DCM with Kalman filtering, and would need 6 DoF.
Having said all that, I probably should add that since your aircraft is so stable to begin with, you can probably do something simple with 4 DoF, without DCM, and without Kalman, if you include the dynamic model of your aircraft in your thinking about the controls. I have not thought about your type of aircraft, so I do not have any suggestions along those lines. However, I caution you about trying to integrate the bank gyro without the benefit of something like DCM, because there are risks of either drift if you do not address the offset of the gyro, or getting the wrong answer if you try to compensate somehow with an approach that does not take into account nonlinearities. Also, you might want to use acclerometers too, but you have to be careful with accelerometers, because they do not give you exactly what you want. DCM integrates gyros and accelerometers into an accurate answer.
Best regards and the best of luck,
Bill Premerlani
-
Permalink Reply by Sean McDonald on -
Hi Bill,
I have been following along with your work on the DCM and very impressed... seems a lot more concise compared to code like used in MicroGear1.3.1 (which I ported but not happy with results as I guess the default gains etc are not correct for my gyros etc.). I am trying to port your DCM code to use it with ARM7 LPC2138 micro (32 bit micro), using the onboard 10bit AD and some filtering for now for gyros and accel. I have ported the code, but not getting the correct results. I am using the 5dof board with another gyro for yaw running off a spare 5dof board.
Could you please provide me with some more information on the format of rmat matrix?
Is the structure the same as the standard rotation matrix?
What do each matrix element represent? ie in terms of cos and sin etc.
I am trying to get roll, picth and yaw from the rmat matrix.
Working thought other info on the net on matrix etc (http://morrowland.com/apron/article/technical/matrix/index.php), I thought it should work out to be something like assuming rmat is a std rotation matrix format:
pitch(radians) = -asin(rmat[2]);
roll(radians)=atan2(-rmat[5],rmat[8]);
yaw(radians)=atan2(-rmat[1],rmat[0]);
I see that your code used rmat[6] for roll, rmat[7] for pitch, rmat[4] for yaw.. so I am not sure about the rmat structure. I am still working through this code, and I am no expert of matrix and vectors (did it a while a ago at school)...
Any insight appreciated.
Thanks
Sean
-
Permalink Reply by William Premerlani on
-
Sean,
There are a few details of my board, firmware, and the MicroChip library that you have to take into account in a port.
1. My board is not mounted in a plane with its axes lined up with the standard convention. The three axis accelerometer is mounted on the board with the Y axis parallel to the long dimension of the board. The way that most users of the board mount the board in their plane, myself included, is rotated 90 degrees from the standard convention. X points toward the left wing, Y points forward, and Z points down. There are X-Y-Z labels on my board that I could look at while writing my firmware, so I found it more convenient to work in that coordinate system.
2. For layout reasons, the Y gyro on my board is mounted backwards from normal way. I flip the sign of the Y gyro in the firwmare.
3. I use a fractional integer representation of all variables, with a sign bit plus one bit integer, (2.14) to cover the range of (approximately) -2 to +2. Whenever you multiply two numbers in this system using integer multiply, you need to multiply the result by 4, which I do. However, whenever you use the Microchip vector-matrix library, it assumes 1.15, and it has multiplication by 2 built in. So when you use the Microchip library, you need to multiply the result by 2. In some places I fold this factor of 2 into a constant, such as in the gyro conversion constant.
4. I am using left justified signed fractional representation option for the A/D converter. When I subract the offset for each channel, I first divide the signal and the offset by 2 to prevent overflow.
5. The GPS reports angles clockwise from the north. I convert them to the mathematical convention, which is counter clockwise from the east.
6. In some places in my code I found it convenient to flip a sign by interchanging the operands of a cross product.
I am going to foward your comment to Louis LeGrand, perhaps he can give you some advice as well. I know that he was able to port the code to his board.
-
Permalink Reply by William Premerlani on
-
Sean,
In my code, the matrix is stored in the array as follows:
The first three elements, rmat[0], rmat[1], rmat[2] are the first row of the matrix. They represent the projection of the earth X axis on the X, Y, and Z axes of my board.
The next three elements, rmat[3], rmat[4], rmat[5] are the second row of the matrix. They represent the projection of the earth Y axis on the axes of my board.
The last three elements represent the projection of the earth Z axis.
In my firmware, for the earth frame, X is east, Y is north, Z is up.
For the board frame, X points to the left wing (west), Y points forward (north) and Z is down.
You can see that there is a sign change between the earth and board X and Z axes, because GPS uses a right handed coordinate system with Z pointing up, and the convention for aircraft (and accelerometers) is Z pointing down.
I can certainly see this will cause some confusion for anyone porting my firmware.
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
-
Andropilot Users Group
203 members
-
Moderators
24 members
-
ArduPlane User Group
720 members
-
ArduCopter User Group
1355 members
-
ArduRover User Group
207 members
Advertisement
© 2013 Created by Chris Anderson.
Powered by
