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!
In your paper you talk about how using gyros with accelerometers helps overcome acceleration issues such as false 'pitch during launch'.
The current ArduIMU DCM implementation shows considerable pitch changes during acceleration when tested in a vehicle. ie when you drive off from a standing start the DCM Eulers show up to 15deg pitch up and vice versa during braking
Is this an indication that something is not quite right in the ArduIMU DCM implementation?
I am currently trying to solve the following issue but do not seem to make much progress, and was hoping you could maybe shed some light on the subject (will be looking into it again tomorrow).
I am implementing the DCM algorithm for use in a land based vehicle and it has proven to work quite well (thanks!). The last few yards is now to get it to work correctly when the vehicle is reversing. This is a bit tricky since GPS speed is always positive. I jumped that hurdle by simply integrating the accelerometer value for a few seconds after GPS speed is zero, getting a direction indication easily enough (I know this is not 100% failsafe, but it is acceptable for my application).
In the forward direction the centrifugal effect is compensated for nicely when turning, but in the reverse direction the corrected pitch is actually tracking the raw roll angle (instead of being compensated) and vice-versa. Difficult to explain without a graph, but I hope you get the idea. Is there something obvious that I am missing for the reverse direction when it comes to the centrifugal correction.
Dear Bill, I have a question about the integration period of PI Controller of DCM.
It seems to be a long term period which covers whole flight time.
It doesn't like other general PID Controller which limitted only in a very short time gap.
Hello all! I come to you humbly to discuss using magnetometers in the DCM algorithm. I believe that my questions will reflect my lack of understanding here! Currently I am working with the propeller chip and have successfully demonstrated pitch/roll control using the Sparkfun 5dof IMU and the DIYdrones gyro daughter board. So far so good. I also have a GPS available but out of stubbornness I would like to use the HMC5843 3 axis magnetometer as a proof of concept platform.
Ok, so here it goes. Am I going about this correctly? My though process is as follows:
1) To use the three axis magnetometer I need to essentially rotate the magnetic readings from the inertial body coordinate system back to the earth reference frame. (Pitch and roll only.)
2) From there I can compute my heading.
3) Then I can essentially follow the steps from the DCM paper regarding yaw drift cancellation using a GPS signal.
Is this reasonable or is there a much easier way? I feel like there should be....
As i understood,the RMarix outputs measure values of the roll, pitch and yaw which corrected by GPS and acc.
I want to add a "fly by wire" function into DCM.
Following is my PID algorithm.
//From RMarix get corrected values of roll,pitch and yaw.
Meas_roll=(the roll angle from Rmarix);
Meas_pitch=(the pitch angle from Rmarix);
Meas_yaw=(the yaw angle from Rmarix);
//setpoint value is tilt angle of stick of RC transimitter .
Setpoint_roll=(the tilt angle of roll_stick);
Setpoint_pitch=(the tilt angle of pitch_stick);
Setpoint_yaw=(the tilt angle of yaw_stick);
//Error=Measure value-tilt angle of stick.
Error_roll=Meas_roll-Setpoint_roll;
Error_pitch=Meas_pitch-Setpoint_pitch;
Error_yaw=Meas_yaw-Setpoint_pitch;
// Integration of Error_roll
Error_roll_sum=Error_roll_sum+Error_roll;
Error_pitch_sum=Error_pitch_sum+Error_pitch;
Error_yaw_sum=Error_yaw_sum+Error_yaw;
//
Roll_control=Error_roll*P+Error_roll_sum*I;
Pitch_control=Error_pitch*P+Error_pitch_sum*I;
Yaw_control=Error_yaw*P+Error_yaw_sum*I;
// send above values into mixer() function to control servo.
Mixer(Roll_control,Pitch_control,Yaw_control);
I don't know whether this PID algorithm can work with DCM.
Somebody could give me a better suggestion about "fly by wire"?
In MatrixNav it seems to be only two kinds of control modes--Automatic and Manual.
How to intergate the stick signal into DCM?
Thank you for the excellent work that you, Paul and Louis has done for us drone diy'ers in deriving and formulating the complementary DCM attitude estimator using gyros, accelerometers and GPS data.
I am happily porting over your DCM code to a dsPIC hardware platform, and reconciling the differences in axes definition and gyro/accel sign directions amongst your paper, the version8a code and my code is causing a certain blankness in the mind :)
If it is not too much trouble, would you be able to assist in providing the DCM output from your board and code for a few IMU attitudes:
a. roll=0 pitch=45 yaw=0
b. roll=0 pitch=45 yaw=90
c. roll=45 pitch=45 yaw=90?
The data would be of great help for me to remap and confirm the output that I should be getting from my board and code.
I am using a 30F6014A board with IDG500 gyros, MMA7260 accels, MicroMag3 magnetometer, and floating point code.
AHHH HELP!!! OK, so I bought the ardupilot main board and the daughter boards to make a 6-dof monster. Currently, I am using the propeller chip and integer operations to get things up and going. Suffice to say that I have the bulk of the math in good working order!! The DCM estimation algorithm with orthogonality and normilization works as I would expect with small errors creeping in over time. Now comes the fun part, I cannot for the life of me figure out how to implement the accelerometer corrections. It seems to me that adding them to the bottom row of the DCM (the Z row) and then only using the X and Y rows for the orthogonality/normalization correction completely negates any information the accelerometers are providing.
So to summarize, I do not understand how adding the acceleration vector to the DCM Zrow and then multiplying it by the new gyro signals (which does not change the X or Y rows of the resulting matrix to reflect the Zrow of the original DCM) and then replacing the Zrow with the cross product of the X and Y rows retains any information regarding the acceleration. English is failing me here. Please let me know if this unclear... Thanks!
Hi Bill,
Last few days I started "really "looking into DCM (yes I know, I am behind the class :) ). I don't have a DevBoard, nor ArduIMU so I can't practically test it (I'll get one of the two) , but have more of a math question:
If dir. cosine matrix is as follows:
0.707107 0.707107 0
-0.707107 0.707107 0
0 0 1
how do you know if plane banked (roll) 45 degrees or rotated (yaw) for 45 degrees? (because matrix looks the same)
Do you make a decision based on sensors - ie. detected gyro sensor roll so I know I banked rather then noticed GPS heading change (for yaw). or is this taken care of in SW somewhere...?
Since I am looking at this from pure math perspective, matrix was set-up in 3D Cartesian coordinate such that +z axis is up, +y is through nose of the plane, and +x is out of right wing
Soo, is there any consensus on the applicability of Sparkfun's 5 dof IMU as it applies to the DCM estimation routine? Are the gyros (IDG500's) excessively noisy/drifty?(I don't think 'drifty' is a word, but whatever, it sounds cool.) I was hoping to save some money and pair the 5 dof unit with one of DIYDrones LISY300 daughter boards for the 6th dof... Comments?
Replies
In your paper you talk about how using gyros with accelerometers helps overcome acceleration issues such as false 'pitch during launch'.
The current ArduIMU DCM implementation shows considerable pitch changes during acceleration when tested in a vehicle. ie when you drive off from a standing start the DCM Eulers show up to 15deg pitch up and vice versa during braking
Is this an indication that something is not quite right in the ArduIMU DCM implementation?
Thanks
Andrew
I am currently trying to solve the following issue but do not seem to make much progress, and was hoping you could maybe shed some light on the subject (will be looking into it again tomorrow).
I am implementing the DCM algorithm for use in a land based vehicle and it has proven to work quite well (thanks!). The last few yards is now to get it to work correctly when the vehicle is reversing. This is a bit tricky since GPS speed is always positive. I jumped that hurdle by simply integrating the accelerometer value for a few seconds after GPS speed is zero, getting a direction indication easily enough (I know this is not 100% failsafe, but it is acceptable for my application).
In the forward direction the centrifugal effect is compensated for nicely when turning, but in the reverse direction the corrected pitch is actually tracking the raw roll angle (instead of being compensated) and vice-versa. Difficult to explain without a graph, but I hope you get the idea. Is there something obvious that I am missing for the reverse direction when it comes to the centrifugal correction.
I am talking about this bit of code of course:
gplane[0]=gplane[0]- omegaSOG( omegaAccum[2] , (unsigned int) sog_gps.BB ) ;
gplane[2]=gplane[2]+ omegaSOG( omegaAccum[0] , (unsigned int) sog_gps.BB ) ;
Regards,
Elardus
It seems to be a long term period which covers whole flight time.
It doesn't like other general PID Controller which limitted only in a very short time gap.
Ok, so here it goes. Am I going about this correctly? My though process is as follows:
1) To use the three axis magnetometer I need to essentially rotate the magnetic readings from the inertial body coordinate system back to the earth reference frame. (Pitch and roll only.)
2) From there I can compute my heading.
3) Then I can essentially follow the steps from the DCM paper regarding yaw drift cancellation using a GPS signal.
Is this reasonable or is there a much easier way? I feel like there should be....
I want to add a "fly by wire" function into DCM.
Following is my PID algorithm.
//From RMarix get corrected values of roll,pitch and yaw.
Meas_roll=(the roll angle from Rmarix);
Meas_pitch=(the pitch angle from Rmarix);
Meas_yaw=(the yaw angle from Rmarix);
//setpoint value is tilt angle of stick of RC transimitter .
Setpoint_roll=(the tilt angle of roll_stick);
Setpoint_pitch=(the tilt angle of pitch_stick);
Setpoint_yaw=(the tilt angle of yaw_stick);
//Error=Measure value-tilt angle of stick.
Error_roll=Meas_roll-Setpoint_roll;
Error_pitch=Meas_pitch-Setpoint_pitch;
Error_yaw=Meas_yaw-Setpoint_pitch;
// Integration of Error_roll
Error_roll_sum=Error_roll_sum+Error_roll;
Error_pitch_sum=Error_pitch_sum+Error_pitch;
Error_yaw_sum=Error_yaw_sum+Error_yaw;
//
Roll_control=Error_roll*P+Error_roll_sum*I;
Pitch_control=Error_pitch*P+Error_pitch_sum*I;
Yaw_control=Error_yaw*P+Error_yaw_sum*I;
// send above values into mixer() function to control servo.
Mixer(Roll_control,Pitch_control,Yaw_control);
I don't know whether this PID algorithm can work with DCM.
Somebody could give me a better suggestion about "fly by wire"?
In MatrixNav it seems to be only two kinds of control modes--Automatic and Manual.
How to intergate the stick signal into DCM?
.
i don't understand how "r(t) + r(t) x d0(t)" there's an addition and a cross product became matrix multiplication by a scalar in equation 17..
---
herry w.
Thank you for the excellent work that you, Paul and Louis has done for us drone diy'ers in deriving and formulating the complementary DCM attitude estimator using gyros, accelerometers and GPS data.
I am happily porting over your DCM code to a dsPIC hardware platform, and reconciling the differences in axes definition and gyro/accel sign directions amongst your paper, the version8a code and my code is causing a certain blankness in the mind :)
If it is not too much trouble, would you be able to assist in providing the DCM output from your board and code for a few IMU attitudes:
a. roll=0 pitch=45 yaw=0
b. roll=0 pitch=45 yaw=90
c. roll=45 pitch=45 yaw=90?
The data would be of great help for me to remap and confirm the output that I should be getting from my board and code.
I am using a 30F6014A board with IDG500 gyros, MMA7260 accels, MicroMag3 magnetometer, and floating point code.
Thank you,
Koide
So to summarize, I do not understand how adding the acceleration vector to the DCM Zrow and then multiplying it by the new gyro signals (which does not change the X or Y rows of the resulting matrix to reflect the Zrow of the original DCM) and then replacing the Zrow with the cross product of the X and Y rows retains any information regarding the acceleration. English is failing me here. Please let me know if this unclear... Thanks!
Last few days I started "really "looking into DCM (yes I know, I am behind the class :) ). I don't have a DevBoard, nor ArduIMU so I can't practically test it (I'll get one of the two) , but have more of a math question:
If dir. cosine matrix is as follows:
0.707107 0.707107 0
-0.707107 0.707107 0
0 0 1
how do you know if plane banked (roll) 45 degrees or rotated (yaw) for 45 degrees? (because matrix looks the same)
Do you make a decision based on sensors - ie. detected gyro sensor roll so I know I banked rather then noticed GPS heading change (for yaw). or is this taken care of in SW somewhere...?
Since I am looking at this from pure math perspective, matrix was set-up in 3D Cartesian coordinate such that +z axis is up, +y is through nose of the plane, and +x is out of right wing
-
2
-
3
-
4
-
5
-
6
of 7 Next