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!
I'm using the DCM code as provided with the CK devices "mongoose" as the starting basis for an opens-source wifi AHRS for real aircraft. (That means some of the difficulties of UAVs don't occur: no continuous high-speed rotations for a Cessna 182, or else you've got other problems...)
Comparing the Mayhony algorithm with the Madgwick algorithm they're very similar except in the function for the error vector.
One uses a linear combination of the magnetic and gravitational cross products, and the other uses the first iteration of a steepest descent estimation, based on the Jacobian of an error function yada yada yada.
Is there any significant difference in the performance?
I also think there are two simplifications that can be made in the algorithm:
Don't compute the Z (third) row of the R matrix in Eqn. 17 - just set it to the cross product of the X and Y rows. That removes any requirement ever to renormalize the third row.
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 new to this hobby, and kind of jumping straight into the deep end, but I have some questions about what a section of your code does. I'm not very fluent in the C family (yet), but I haven't been able to find what the 'union longww' means. And how does it relate to the VariableName.WW?
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!
Hi! I had this under control. But then I stopped thinking about it for a while and no I dont understand any more :( Could any one help my understand way you take the cross product betwene Z in the earth frame of reference and the acc vector to find the error in the body frame of reffrence??
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.
Replies
HI,
I'm using the DCM code as provided with the CK devices "mongoose" as the starting basis for an opens-source wifi AHRS for real aircraft. (That means some of the difficulties of UAVs don't occur: no continuous high-speed rotations for a Cessna 182, or else you've got other problems...)
Comparing the Mayhony algorithm with the Madgwick algorithm they're very similar except in the function for the error vector.
One uses a linear combination of the magnetic and gravitational cross products, and the other uses the first iteration of a steepest descent estimation, based on the Jacobian of an error function yada yada yada.
Is there any significant difference in the performance?
I also think there are two simplifications that can be made in the algorithm:
Don't compute the Z (third) row of the R matrix in Eqn. 17 - just set it to the cross product of the X and Y rows. That removes any requirement ever to renormalize the third row.
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
I'm new to this hobby, and kind of jumping straight into the deep end, but I have some questions about what a section of your code does. I'm not very fluent in the C family (yet), but I haven't been able to find what the 'union longww' means. And how does it relate to the VariableName.WW?
Thanks!
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 ?
-
1
-
2
-
3
-
4
-
5
of 7 Next