Well here it is, the first DCM propeller code (that I have seen). This is based on the wealth of work put forth by Bill Premerlani and others on the Ardupilot/matrix pilot side. I must admit that this is the first enormous coding project that I have undertaken and as such represents a major work in progress. Consider this V0.0001.
I am using my own custom board with two propeller chips and the following inertial hardware:1 x Sparkfun 5dof board (The older green one)(analog)
1 x LISY300 gyro for the z-axis (analog)1 x HMC5843 3-axis magnetometer
1 x MAX1270 ADC1 x MPXV5004DP Dual pressure sensor (analog)
1 x MPXA6115 Barometric pressure sensor (analog)
I also have a Copernicus GPS mounted but currently unused. Telemetry is handled by a 900MHz X-bee pro XSC.
My tuning methodology was as follows:Initialize the DCM matrix with an error in one axis. The values can be computed via Premerlani's paper. I chose to offset the matrix by ~10degrees
In the DCMupdate vector, make the off axis gyro signals zero, this will effectively turn your board into 1-dof. I.E. if you are tuning the x-axis, then make the y and z gyro values zero. Just make
sure you do not move it around a non-functioning axis. Turn the kIa gain to zero and adjust the kPa
gain until you are satisfied with the settling time. Then slowly increase the kIa gain to eliminate the steady-state offset. That's all there is to it! This completely neglects the countless hours I spent trying to figure that out..
This tuning methodology works equally well for the z compass axis.
Once you have the PI gains set, you can adjust your gyro scaling by placing your board at a known angle (30 degree drafting triangles work well here), waiting for the DCM to stabilize and then rapidly removing the block and making the board level. Overshoot? Reduce the gains. Undershoot? Make them larger. You can also adjust them analytically if you know the correct scaling signal, mine is a bit funky due to the ADC setup. And my X and Y axis seem to be slightly different. IDG-3200 here I come.
The DCM matrix implementation is done using fixed point arithmetic. I chose to scale everything by 2^14 so as to reduce the chance of overflow errors when multiplying. Please feel free to change these values but do so knowing that you may run into overflow issues within the matrix multiplication routine. I also chose to scale the accelerometer readings to 1g = 2^14 as a legacy decision. If this is changed, know that the associated kPa and kIa gains will need to be retuned.
I was hoping that we (me and whoever else) could start working on this in a more distributed fashion. Once the code is verified we would need to add support for GPS headings, velocity corrections, wind estimation and the lot. How should we move forward on this?!
Very nice work. It certainly is, as you said, "enormous". I wish you well, and hope that a community grows to help you continue to develop it.
Could you post a picture of your board? I would like to see it.
The pressure sensors are a good idea.
I like the way you are determining the PI drift compensation feedback gains. Very nice.
Regarding the gyro scaling, what I do with the UAVDevBoard is to run the "roll-pitch-yaw demo" without any yaw reference. (No magnetometer). Recently, we added an "integrate and dump" technique to the sampling, as well as a high rate of sampling, that has lowered the uncompensated drift rate to less than 1 degree per minute, so we can measure the gyro scaling by spinning the board on an old record player that conveniently lets you remove the center spindle. I spin the board 10 or 20 revolutions, and then look at the accumulated yaw error. This lets me estimate the gyro gains to better than 1% accuracy. I revise the gains and repeat until I can spin the board 20 times with zero accumulated error.
I assume that the gains for the roll and pitch axes are the same, but they could be measured the same way if you are willing to build something to hold your board on its side while you test it.
Very cool (digging up old thread). Any developments on this over the last year?
It is great work, I am working on a bit different project, I want to use DCM to get correct orientation(Yaw Pitch Roll) of Smartphone, I have Gyro, Accelerometer and Compass in Smartphone, I am using compass instead of GPS for Yaw correction,
The problem I am facing is that when I change only pitch value(When I move mobile up/down), I get correction in pitch value but my roll value is distorted (It should be near zero), I get abrupt change of -180 to 180 in roll value, I am using atan2 to get the roll value.
I get the correction when pitch value is less than 90 but above 90 it distorts the roll value, How Can I overcome this problem.
There are two related problems that you might be running into:
1. If you are treating the magnetometer as a tilt compensated compass, it will not work for pitch values near 90 degrees. A better way to do it is to use the magnetometer measurement as a reference vector instead. Use the direction cosine matrix to transform the magnetometer vector from body frame to earth frame, which works in any orientation, even with 90 degree pitch. Then, extract the horizontal component of the magnetometer earth frame vector, and take the cross product of it with the known horizontal component of the earth's magnetic field. The result is a rotation error vector that you transform back into the body frame, and use it to compensate for gyro drift. That is technique we are using in MatrixPilot, it works for any orientation. A combination of two reference vectors (magnetometer and accelerometer) will provide a 3 axis lock.
2. If you are using Euler angles to represent orientation, they do not work for 90 degree pitch. There is an effect known as "gimbal lock" that throws off the roll. It is better to use either an angle and rotation axis representation, or quaternions.
From your description of the problem, it sounds like you are simply running into gimbal lock.
Thank you very much for your explanation, Now I understand where is the problem (gimbal Lock), I have gone through IMUdraft2.pdf, but I could not understand properly how I can use an angle and rotation axis representation to find the three angles. I have also seen the Matrixpilot code (by PaulBizard attached) there also roll , pitch and yaw are calculated using rotation matrix elements and using function atan2.
I am still waiting for some response from you, Can you please a little bit explain that how I can represents the orientation using and angle and rotation axis instead of Euler angles. (Sorry I am not so good at Math)
I wished I'd done/understood more linear algebra. I've been spending a lot of time with this algorithm and code variants lately.
If it helps, in MatrixPilot (e.g., older version 2.5.1) there's a function, mag_drift(), in rmat.c, which may have some hints for us on how to do this.
If you haven't, try reading through these a few times:
Looking through mag_drift() I kind of get what's going on at least up to the point of calculating error. Still trying to grasp the rest. I'd been looking at v3.2.1 with delay compensation; v2.5.1 doesn't have it and may be easier to grasp. I'm looking at that now.
Best of luck.
It turns out that after calculating error, the rest of the code is intended to automatically null out the magnetometer offsets. "Note that the yaw compensation takes only six lines, it is a lot easier to do in terms of direction cosines than it is in terms of tilt-compensating the magnetometer in terms of Euler angles" from http://gentlenav.googlecode.com/files/MagnetometerOffsetNulling.pdf
Most of the theory of using rotation matrices in MatrixPilot is explained here. There are two fundamental concepts:
1. The rotation matrix can be used to transform vectors between body and earth frame of reference.
2. Reference vectors can be used to detect gyro drift. The cross product of two reference vectors that should be aligned produces a rotation error vector that can be used directly to adjust gyro drift.
For more information, see the wikipedia entry on rotation matrices.