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!
There are several methods for using a magnetometer for gyro drift compensation. Here is the method that I like to use:
1. Transform the 3D measured magnetic field vector from body frame to earth frame.
2. Take the cross product of the horizontal plane projections of actual and measured earth frame magnetic field vectors. This gives you the sine of the yaw error angle. Call this the yaw correction in earth frame.
3. Transform the earth frame yaw correction vector back into body frame and use it for gyro drift compensation.
You may find some of the discussions on this page interesting:
Regarding renormalization introducing drift, in practice, it does not create much drift. I have done static (motionless) tests on the UDB5 hardware with drift correction turned off and measured the residual drift from all sources (such as noise, renormalization, offset shift) to be on the order of one degree per minute. There are much larger sources of error in flight due to gyro calibration errors. See for example:
In any case, you need to apply continual drift compensation in flight to prevent accumulation of attitude estimation error. This is done with the aid of known reference vectors.
Regarding the bottom row of the matrix, it represents the image of the earth vertical axis in the body frame. You can use the accelerometer as a reference vector for vertical if you compensate for centrifugal acceleration. There are a couple of ways of doing that, DCMDraft2 describes one way to do it, the following describes another way:
Regarding the interpretation of the sign of the accelerometer data, depending on your preference, you can think of it as producing gravity minus acceleration, or acceleration minus gravity. I have been told by some of the members of the MatrixPilot team that the way that I think of it is opposite to the rest of the world, so I apologize if I have confused you.
Finally, here is another source of information on this subject, it is nicely organized, fun to read and easy to understand:
I've been reading everything i could find about IMUs. Thanks to a basic Robotics course, I have a decent understanding of reference frames, rotations, concatenations and kinematics and dynamics.
I've been trying to go at it alone, and so far I've been unsuccessful. My algo works, but picks up way too much noise. And that's ok because there is practically no filter.
I've gone directly with quaternions, and I got decent results but sensors are very very noisy.
Now what I couldn't understand in everything I've read is how measures get fused together.
I may be asking a lot, but would you make a scheme of the algorithm? A process chart, just like a matlab simulink worksheet would really help me understand. Trying to follow it from code is cumbersome, although I'm very familiar with C (been programming in C for 10 yrs, PICs for half that).
So far I get the accelerations and the rotations, I calculate the coriolis correction vector as speeds * angular_speeds (integration of speeds is another matter altogether!), and get instant acceleration vector. Then from the angular speed vector I calculate the intermidiate quaternion and operate a quat multiplication between the old quaternion and the intermidiate to obtain the updated one (angles are obviously scaled by sampling_interval). Using basic rotations (planar) I'm able to see it works, but as I said it picks up way too much noise and drift. Even if yaw drift can't be compensated, I expected a better behaviour.
The approach I used is to run some specialized PIDs for about 30s to zero out the Gyros and the ACCs before starting the real algorithm. Then the ACCs' PIDs are stopped as ACCs do not have drift, while the Gyros' PIDs remaing active with very low dynamics and what is essentially an hystereis to follow the slowly moving drift. This appears to work quite well, fact is that each sensor axis has its own quirks: not two ACCs signals have the same FS or center. Let alone Gyros.
What I've been able to discern is that most of the working methods do not involve direct filtering of the signals before it enters the algorithm as I do, but I see citations that you use PIDs somewhere.
What do you use PIDs for?
And how filtering is ultimately obtained?
Can yuo give me a noise figure for your sensor? To estimate if mine are unsuitable for the task, or is it just plain me (which in part surely is).
Thank you
Claudio
P.S. I've received suggestion of filtering the Gyros with an HP filter. I tried but the spikes such filtering gave were huge. Besides I can't exlude a-priori that the device will eventually maintain some attitude different than leveled, in such case the signal filtered by an HP filter would eventually drop to zero thus yeilding wrong results.
P.P.S. will you eventually design a 9dof module? Among the low cost IMUs I found, yours is the one that suits me the most as it includes a very powerful DSP and one I know how to program. I've seen the Sparkfun Ultimate IMU but I haven't been anywhere near an ARM core, so I wouldn't know where to start. And that's a huge con for me right now.
I'm using the DEV Board (RED old one) in IMU configuration (No GPS/Magnetometer yet) using a fairly new firmware (Mag Demo) in the attempt to stabilize a VTOL in hover mode.
Note that I'm not really in a helicopter setup (I have electrical turbines everywhere so very high frequency vibrations just like in a plane - it's something like a Harrier jet)
I've tried to obtain the absolute acceleration of the model regardless of it's attitude (taking out the gravity component from the accelerometer sensor by simple rmat operations - code bellow) and sending the X and Y axis absolute accelerations to PID controllers similar to the head-locking GY401 hoping that the hover will be as precise as the GY401 tail locking in helis... .
The code for obtaining the absolute accelerations I've used is:
// gravity measured in the frame of reference of the plane fractional gravity_p_ref[] = { 0 , 0 , 0 } ;
// gravity component in the plane reference fractional abs_acceleration_p[3] = { 0 , 0 , 0 } ; ... // get the gravity component in the plane frame of reference: MatrixMultiply( 3 , 3 , 1 , gravity_p_ref , rmat_transpose , gravity ) ; // substract the gravity component from the accelerometer readings: VectorSubtract( 3 , abs_acceleration_p , gplane , gravity_p_ref ) ; // project the absolute acceleration of the plane frame of reference to the the ground frame of reference: MatrixMultiply( 3 , 3 , 1 , absolute_acceleration , rmat, abs_acceleration_p) ;
and it seems to be working: I've played with the board and, regardless of the attitude, the absolute_acceleration shows something only if the position of the board changes, not the attitude.
The problem is that the absolute acceleration values should became 0 as soon as the board gets steady but it's not the case... it converges very slowly and aventually never gets to 0 at all. Maybe some errors accumulate, I don't get it.
As for the speed of convergence, it's a killer for the control PIDs, especially in the "head-locking"="position locking" in my case configuration.
The question: is there a way to tune/setup the DCM algorithm for this particular case where tha acelerometers mesures mostly the gravity with only few acceleration spikes (from controlling elements)?
May it also be imperative to use higher quality sensors (thinking of SCA3000 as accelerometer i.e. and lower drift gyros)?
I will also try to rely only on gyro sensors to calculate the attitude and implicitly the gravity component of accelerometers (disable the accelerometer correction) for the short time of hovering but I'm sure that the gyro drift will be annoying.
BTW, I've noticed an even higher convergence time in attitude estimation in EKF based AHRS modules (PNI SpacePoint) ~ tens of seconds :( - I didn't have the "budget" to try the Micro Strain or other commercial ones ... It would be nice to be able to make it with UAV Dev Board :)
I am a mechanical engineering student who is going to design an autopilot system with the UAV DevBoard. After reading your Direction Cosine Matrix documentations I have some questions:
- Do you completely ignore the dynamics of the system in the whole autopilot? So it is not neccesary to make any dynamic model of the airplane?
- Should I use the GPS signal or the Rmatrix to determine the position of the plane and to compare it with a desired trajectory?
One hot-off-the-press propeller spin based DCM algorithm coming up! Admittedly it is quite simplistic compared to the matrixpilot, but I hope it will give some people the first step towards something great!
Dear Bill:
The altitude holding algorithm of MatrixNavRv2 make me very confused. MatrixNavRv2 can do a accurate altitude holding. But I don't know the MarixNav how to know current altitude? The DCM algorithm seems not provide a way to do a altitude measure.
Best Regards
Jack Chen
first of all thanks for your excellent share.
i implemented (tried at least) dcm algorithm in C#.
connected my 6 dof IMU to PC via serial port and read raw data.
and tried a wide range of PI gains but i never achieved a good result.
is it because of gains?
one more thing is i confused with accelerometer signs. should down to earth be positive or negative?
thank you all.
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.
Replies
Johnson,
There are several methods for using a magnetometer for gyro drift compensation. Here is the method that I like to use:
1. Transform the 3D measured magnetic field vector from body frame to earth frame.
2. Take the cross product of the horizontal plane projections of actual and measured earth frame magnetic field vectors. This gives you the sine of the yaw error angle. Call this the yaw correction in earth frame.
3. Transform the earth frame yaw correction vector back into body frame and use it for gyro drift compensation.
You may find some of the discussions on this page interesting:
http://diydrones.com/page/uav-devboard
Best regards,
Bill
Johnson,
Thanks for your questions.
Regarding renormalization introducing drift, in practice, it does not create much drift. I have done static (motionless) tests on the UDB5 hardware with drift correction turned off and measured the residual drift from all sources (such as noise, renormalization, offset shift) to be on the order of one degree per minute. There are much larger sources of error in flight due to gyro calibration errors. See for example:
http://diydrones.com/profiles/blogs/sustained-rotations-pushing
In any case, you need to apply continual drift compensation in flight to prevent accumulation of attitude estimation error. This is done with the aid of known reference vectors.
Regarding the bottom row of the matrix, it represents the image of the earth vertical axis in the body frame. You can use the accelerometer as a reference vector for vertical if you compensate for centrifugal acceleration. There are a couple of ways of doing that, DCMDraft2 describes one way to do it, the following describes another way:
http://diydrones.com/forum/topics/an-improved-method-for-accounting...
Regarding the interpretation of the sign of the accelerometer data, depending on your preference, you can think of it as producing gravity minus acceleration, or acceleration minus gravity. I have been told by some of the members of the MatrixPilot team that the way that I think of it is opposite to the rest of the world, so I apologize if I have confused you.
Finally, here is another source of information on this subject, it is nicely organized, fun to read and easy to understand:
http://www.starlino.com/dcm_tutorial.html
Best regards,
Bill
Hello William and everybody else.
I've been reading everything i could find about IMUs. Thanks to a basic Robotics course, I have a decent understanding of reference frames, rotations, concatenations and kinematics and dynamics.
I've been trying to go at it alone, and so far I've been unsuccessful. My algo works, but picks up way too much noise. And that's ok because there is practically no filter.
I've gone directly with quaternions, and I got decent results but sensors are very very noisy.
Now what I couldn't understand in everything I've read is how measures get fused together.
I may be asking a lot, but would you make a scheme of the algorithm? A process chart, just like a matlab simulink worksheet would really help me understand. Trying to follow it from code is cumbersome, although I'm very familiar with C (been programming in C for 10 yrs, PICs for half that).
So far I get the accelerations and the rotations, I calculate the coriolis correction vector as speeds * angular_speeds (integration of speeds is another matter altogether!), and get instant acceleration vector. Then from the angular speed vector I calculate the intermidiate quaternion and operate a quat multiplication between the old quaternion and the intermidiate to obtain the updated one (angles are obviously scaled by sampling_interval). Using basic rotations (planar) I'm able to see it works, but as I said it picks up way too much noise and drift. Even if yaw drift can't be compensated, I expected a better behaviour.
The approach I used is to run some specialized PIDs for about 30s to zero out the Gyros and the ACCs before starting the real algorithm. Then the ACCs' PIDs are stopped as ACCs do not have drift, while the Gyros' PIDs remaing active with very low dynamics and what is essentially an hystereis to follow the slowly moving drift. This appears to work quite well, fact is that each sensor axis has its own quirks: not two ACCs signals have the same FS or center. Let alone Gyros.
What I've been able to discern is that most of the working methods do not involve direct filtering of the signals before it enters the algorithm as I do, but I see citations that you use PIDs somewhere.
What do you use PIDs for?
And how filtering is ultimately obtained?
Can yuo give me a noise figure for your sensor? To estimate if mine are unsuitable for the task, or is it just plain me (which in part surely is).
Thank you
Claudio
P.S. I've received suggestion of filtering the Gyros with an HP filter. I tried but the spikes such filtering gave were huge. Besides I can't exlude a-priori that the device will eventually maintain some attitude different than leveled, in such case the signal filtered by an HP filter would eventually drop to zero thus yeilding wrong results.
P.P.S. will you eventually design a 9dof module? Among the low cost IMUs I found, yours is the one that suits me the most as it includes a very powerful DSP and one I know how to program. I've seen the Sparkfun Ultimate IMU but I haven't been anywhere near an ARM core, so I wouldn't know where to start. And that's a huge con for me right now.
Hello everyone,
Thanks to all contributors for the amazing work.
I'm using the DEV Board (RED old one) in IMU configuration (No GPS/Magnetometer yet) using a fairly new firmware (Mag Demo) in the attempt to stabilize a VTOL in hover mode.
Note that I'm not really in a helicopter setup (I have electrical turbines everywhere so very high frequency vibrations just like in a plane - it's something like a Harrier jet)
I've tried to obtain the absolute acceleration of the model regardless of it's attitude (taking out the gravity component from the accelerometer sensor by simple rmat operations - code bellow) and sending the X and Y axis absolute accelerations to PID controllers similar to the head-locking GY401 hoping that the hover will be as precise as the GY401 tail locking in helis... .
The code for obtaining the absolute accelerations I've used is:
// gravity measured in the frame of reference of the plane
fractional gravity_p_ref[] = { 0 , 0 , 0 } ;
// gravity component in the plane reference
fractional abs_acceleration_p[3] = { 0 , 0 , 0 } ;
...
// get the gravity component in the plane frame of reference:
MatrixMultiply( 3 , 3 , 1 , gravity_p_ref , rmat_transpose , gravity ) ;
// substract the gravity component from the accelerometer readings:
VectorSubtract( 3 , abs_acceleration_p , gplane , gravity_p_ref ) ;
// project the absolute acceleration of the plane frame of reference to the the ground frame of reference:
MatrixMultiply( 3 , 3 , 1 , absolute_acceleration , rmat, abs_acceleration_p) ;
and it seems to be working: I've played with the board and, regardless of the attitude, the absolute_acceleration shows something only if the position of the board changes, not the attitude.
The problem is that the absolute acceleration values should became 0 as soon as the board gets steady but it's not the case... it converges very slowly and aventually never gets to 0 at all. Maybe some errors accumulate, I don't get it.
As for the speed of convergence, it's a killer for the control PIDs, especially in the "head-locking"="position locking" in my case configuration.
The question: is there a way to tune/setup the DCM algorithm for this particular case where tha acelerometers mesures mostly the gravity with only few acceleration spikes (from controlling elements)?
May it also be imperative to use higher quality sensors (thinking of SCA3000 as accelerometer i.e. and lower drift gyros)?
I will also try to rely only on gyro sensors to calculate the attitude and implicitly the gravity component of accelerometers (disable the accelerometer correction) for the short time of hovering but I'm sure that the gyro drift will be annoying.
BTW, I've noticed an even higher convergence time in attitude estimation in EKF based AHRS modules (PNI SpacePoint) ~ tens of seconds :( - I didn't have the "budget" to try the Micro Strain or other commercial ones ... It would be nice to be able to make it with UAV Dev Board :)
Thanks and Regards,
Dragos
I am a mechanical engineering student who is going to design an autopilot system with the UAV DevBoard. After reading your Direction Cosine Matrix documentations I have some questions:
- Do you completely ignore the dynamics of the system in the whole autopilot? So it is not neccesary to make any dynamic model of the airplane?
- Should I use the GPS signal or the Rmatrix to determine the position of the plane and to compare it with a desired trajectory?
Thanks for the support!
Piet
http://www.diydrones.com/forum/topics/propeller-dcm-code?xg_source=...
One hot-off-the-press propeller spin based DCM algorithm coming up! Admittedly it is quite simplistic compared to the matrixpilot, but I hope it will give some people the first step towards something great!
Enjoy and thanks again!
The altitude holding algorithm of MatrixNavRv2 make me very confused. MatrixNavRv2 can do a accurate altitude holding. But I don't know the MarixNav how to know current altitude? The DCM algorithm seems not provide a way to do a altitude measure.
Best Regards
Jack Chen
first of all thanks for your excellent share.
i implemented (tried at least) dcm algorithm in C#.
connected my 6 dof IMU to PC via serial port and read raw data.
and tried a wide range of PI gains but i never achieved a good result.
is it because of gains?
one more thing is i confused with accelerometer signs. should down to earth be positive or negative?
thank you all.
best regards.
evren.
phi = atan2(-DCM23, DCM33)
theta = atan2(DCM13, sqrt(1-(DCM13)^2))
psi=atan2( -DCM12, DCM11)
and the firmware on ArduIMU_pack,
theta=asin(DCM[3,1])
---
the angle of phi & psi is varied from 0 to 180 then -180 to 0, that is good to be used in labview cube, the cube could rotate 360o
problem is on theta which is varied from -90 to 90, it doesnt represent a 360o rotation in labview's cube, it screw the rotation visualization..
any thought ?
---
another question about Gyro_Gain and Ki Kp value, how to find it's best value ?
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
-
1
-
2
-
3
-
4
of 4 Next