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
You need to be a member of diydrones to add comments!
Thanks exactly what I did. Just wanted to make sure I did it correctly b/c this is all new to me. I'm a software engineer and i'm trying to learn how to build simulations so i'm taking an online course where nothing is explained. This is a huge learning curve for me. Again thank you so much. All of your help is greatly appreciated.
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...
Thank you for the interesting algorithm, and for all the detailed information given in the comments !
I think the compensation of the centrifugal force is very promising, and I wanted to understand a little bit better how you implemented it with the GPS velocity and to get your thoughts.
In particular, how is the GPS velocity used to compensate the centrifugal force ? My impression (is it correct ?) after very quickly looking at the code, is that the velocity is simply assumed to be along the x body axis.
Do you think there could be an algorithm to obtain a better estimate of the velocity vector ? (For example, in Mahony's paper there is an angle-of-attack model, etc...). Because the better the velocity vector is approximated, the better the centrifugal force compensation is.
For example, if the GPS velocity vector is used (as opposed to only the norm), it can be transformed to the body frame if the yaw is known (I assume the yaw can be inferred from the GPS heading). But is the GPS velocity vector reactive enough during turns, I don't know.
A problem also, is probably in case of side wind : in this case the yaw is not really the GPS heading anymore, and it is probably harder to get right (I guess magnetometers are needed here - same as when the estimator is used on a helicopter). So in case of wind, the GPS speed cannot be transformed to the body frame accurately I suppose.
Btw, have you already addressed the problem of wind in your algorithm ?
I need help building a transformation matrix from NED to ENU. I have to multiply roll * pitch * yaw. I've been trying to figure this out but this isn't my area of expertise. Can someone please assist me with this problem?
Bill/Louis,
The info on the matrix elements is great. I now have it responding to my inputs once I adjusted the gyro gain etc. Now I need to work on the p and i gains for pitch & roll. Without compensation for pitch roll by accel, the euler angles representation by phi the and psi drifts as expected.
If the elements are used for demo and control without conversion to quarterions or Euler angles, how would I be able to correct a magnetometer for pitch and roll? I see Louis has mag being used so I must read the paper again ... Hopefully it sinks in and becomes more evident to me. Thx all info to date. Seano
Replies
given: long, lat, alt, bearing angle, time, and body attitude
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
Thank you for the interesting algorithm, and for all the detailed information given in the comments !
I think the compensation of the centrifugal force is very promising, and I wanted to understand a little bit better how you implemented it with the GPS velocity and to get your thoughts.
In particular, how is the GPS velocity used to compensate the centrifugal force ? My impression (is it correct ?) after very quickly looking at the code, is that the velocity is simply assumed to be along the x body axis.
Do you think there could be an algorithm to obtain a better estimate of the velocity vector ? (For example, in Mahony's paper there is an angle-of-attack model, etc...). Because the better the velocity vector is approximated, the better the centrifugal force compensation is.
For example, if the GPS velocity vector is used (as opposed to only the norm), it can be transformed to the body frame if the yaw is known (I assume the yaw can be inferred from the GPS heading). But is the GPS velocity vector reactive enough during turns, I don't know.
A problem also, is probably in case of side wind : in this case the yaw is not really the GPS heading anymore, and it is probably harder to get right (I guess magnetometers are needed here - same as when the estimator is used on a helicopter). So in case of wind, the GPS speed cannot be transformed to the body frame accurately I suppose.
Btw, have you already addressed the problem of wind in your algorithm ?
Thanks in advance for your answer !
Best,
Adrien
Thanks in advance,
Michelle
UFO-MAN
i am several days with a problem and i would like to consult.
I have implemented your DCM update in MATLAB.
omegacorr = omegacorrPAcc + omegacorrIAcc + omegacorrPMag + omegacorrIMag;
omegatotal = gyro1 + omegacorr;
theta = omegatotal;
rup = [ rmax -theta(3) theta(2)
theta(3) rmax -theta(1)
-theta(2) theta(1) rmax];
rmat = (rmat*rup);
I am making a comparison with euler integration:
i_roll = i_roll + gyro(1);
i_pitch = i_pitch + gyro(2);
i_yaw = i_yaw + gyro(3);
there are one first stuff than i can not understand
i am scaling gyro measurement (in rad/sec) * 0.00075!!!!
result seems to be correct
but this "integrated" DCM is very strange, it is only correct to North direction!!!
when i turn to south then roll value is exacly inverse values!
Any idea?
The info on the matrix elements is great. I now have it responding to my inputs once I adjusted the gyro gain etc. Now I need to work on the p and i gains for pitch & roll. Without compensation for pitch roll by accel, the euler angles representation by phi the and psi drifts as expected.
If the elements are used for demo and control without conversion to quarterions or Euler angles, how would I be able to correct a magnetometer for pitch and roll? I see Louis has mag being used so I must read the paper again ... Hopefully it sinks in and becomes more evident to me. Thx all info to date. Seano
I have the problem of the jumping from -pi to +pi
I will think this through as I continue my debugging of the code.
Thx
Sean
Digikey lists it for USD 707,- which I think is very expensive.
Datasheet here by the way: http://www.analog.com/static/imported-files/data_sheets/ADIS16405.pdf
UFO-MAN
-
3
-
4
-
5
-
6
-
7
of 7 Next