# DCM IMU Theory : first draft

For those of you who are trying to understand the "direction-cosine-matrix" algorithm, or who are struggling to understand Mahony's papers or the firmware for my UAV DevBoard, a first draft of the document, Direction Cosine Matrix IMU: Theory, is now available. It is a work in progress. Although Paul Bizard and I have not yet incorporated all of the great suggestions we received from reviewers, especially from Louis LeGrand and UFO-MAN, I think there is an audience for what we have so far.

Views: 17203

Tags: DCM

Comment by Alex Zarenin on July 10, 2010 at 9:40pm
Hi Bill,

I am still working on the Roll-Pitch correction piece using accelerometers; with this regards have couple of questions regarding algorithm described in the “DCM draft”.

As far as I understand, the cross product of the Z row of the direction cosine matrix with the normalized gravity reference vector (from accelerometer) defines a set of 3 angles that the calculated Z row need to be rotated to match the normalized gravity vector (eqn. 27); these three angles are the components of the cross-product pseudo-vector with the x component defining roll correction, y component defining pitch correction, and z component – yaw.

Eqn. 29 calculates total correction using PI algorithm, which then should be fed back into the calculation by adding to the gyro signals in Eqn. 16.

However the gyro signals give us the rate of rotation (which need to be multiplied by delta-t to get the angles) while components of the cross-product give us the actual angles – technically we should not sum the rate with the angles!

As your implementation works, then there is either an error in the document or an error in my line of thinking.

Would greatly appreciate your further explanation.

--Alex
Comment by ionut on July 11, 2010 at 12:05am
Bill,
I saw you use North direction to orientate your plane.I saw there is also an different method for matching physical space(gyro position) relative to a known image space(Google Earth).It's called Point Based Rigid Registration or Absolute orientation by Horn.
http://isiswiki.georgetown.edu/zivy/writtenMaterial/pbrr.pdf

Basically you select points on the ground and then match them on your navigational map.If your gyros are accurate you don't need GPS navigation at all.
Comment by William Premerlani on July 11, 2010 at 6:41pm
Hi Alex,

You are correct in your observation that the raw error computed by the cross product is basically a vector of angles.

You are also correct that the adjustment we make is to a vector of gyro rates.

That is exactly what we want to do to smoothly compensate for the gyro drift, to perfectly compensate for the gyro offset, without introducing much noise and error into our estimate of the direction cosines.

When you are driving behind another car, and want to maintain a certain distance between your car and theirs, you do so by controlling your speed. You speed up when the distance is too large, you slow down when the distance is too small. You do not directly control the spacing between the cars, you control it indirectly by adjusting your speed.

That is what we are doing with the gyro rates. We do not want to compensate for the angle errors all at once, we want to spread the adjustments over time. We do that by adjusting the rates, not the angles.

What happens is that as the gyros drift (an error in rate), there is a gradual accumulation of error in angles. By adjusting the rate error, we gradually close the loop and "lock" on. Once we are locked, our use of a rate adjustment rather than an angle adjustment filters out the noise and variation that you get from the accelerometers.

So, the gains in the PI controller have units. The output is a rate, the input is an angle. So the units of KP is 1/sec. That is, KP is the reciprocal of a time constant. In fact, that is how we set it, KP is equal to 2/tau, where tau is the desired time constant of the dynamic response of the adjusment to a disturbance.

Similarly, the units of KI are 1/(sec*sec). In fact, we set KI to 1/(tau*tau).

Then, you wind up with a second order feedback loop. There are two integrators in the loop, the integrator for the KI term, and the DCM algorithm itself. If you set the KP and KI as I have suggested, the dynamic response is critcally damped with a time constant of tau.

Best regards,
Bill
Comment by Alex Zarenin on July 11, 2010 at 8:39pm
Hi Bill,

This is a very interesting point! Let’s say I make measurements every 20 msec. As part of the integration of gyro rates I multiply them by the time interval, 20 msec in my case, which is equivalent to dividing the rate values by 50.

In this case if I add the angles computed from Roll-pitch correction to the rates of gyros, I effectively reduce the Kp and Ki by a factor of 50. If I reduce the measurement interval to 10 msec, the Kp and Ki will be effectively reduced by a factor of 100. Basically you came up with the design for a PI controller with the “variable” Kp and Ki – do you have any references for this design? I definitely would like to read more on this…

The other question that comes to my mind is what is the range of time intervals over which this approach will work? If my measurement interval is, let’s say, 1 sec – will adding correction angles to gyro rate work?

Last night after making my post I went ahead and tested my board with the logic that adds the correction angles dampened by Kp=0.2 and Ki=0.01 to the gyro angles (after integration), so I have deviated from your suggested approach as I did not completely understood it. As a test I pitched my board down ~90 degrees and returned back, then banked it to the right ~90 degrees and returned back – without Roll-Pitch compensation after this test I usually had a “drift” error about 5-10 degrees. However with the roll-pitch compensation the drift disappeared completely! I knew that this is what was expected, but to actually see it working was a big surprise and a lot of satisfaction!

Now I am going to test it with the Roll-Pitch correction added to the gyro rates instead of gyro angles.

Regards,
--Alex
Comment by Hari on November 25, 2010 at 4:30am
Hi William Premerlani,
In your DCMDraft2.pdf you referred an paper of mahoney[1] in page 14. which paper is it among the 4 papers of Mahoney which you recommended to download here.
Thanks
Comment by Jeroen van de Mortel on March 14, 2011 at 11:42am

Hi Bill,

I building my own quadrocopter based on a dsPIC, I have flown with a complementary filter but it is not hover stable. So I want to try the DCM filter, I copied the code from the Arducopter project in to my C file. It does do something but it is far from stable.

I have been able to fly with a Kp=1 and Ki=0. Indoor it is flyable but outside not because of the wind. Sow i did some research and Aeroquad uses a Kp=0.0005 and Ki=0.00000012 with the same sensors that I have (ITG-3200, BMA180). But I can't get it stable I have tried alot of settings, with Kp=0.5 and Ki=0.0005 the dirft is away but not flyable due alot of overshoot.

I can't find the problem, my sensor data is all floating point with deg/sec and G's then I convert deg/sec to rad/sec before they go thru the DCM filter.

Do you have any tips to solve my problem ?

Thanks

Comment by Alex Zarenin on March 14, 2011 at 6:00pm

Jeroen,

I am working on a similar project and also on a dsPIC platform. As a first step I made sure that the attitude data coming from my implementation of the DCM algorithm is stable under a broad range of conditions (that I managed to emulate :)). However having stable attitude data is just half of the problem - the second half is how to translate it into motor control signals. This also requires implementing some PID controller to convert attitude “error” into the motor control signals.

Either one of those can attribute to the instability of the flight.

So, what coefficients Kp and Ki are you talking about – those in the DCM algorithm or those in the motor control PID?

Regards,

--Alex.

Comment by Jeroen van de Mortel on March 15, 2011 at 12:09am

I am talking about the Kp and Ki of the DCM, my PID controlloop for controlling the attitude is working fine (with complementary filter). But I can't get reliable data from the DCM.

Do you have some code availble ?

Comment by Alex Zarenin on March 15, 2011 at 6:31pm
I sure do and would gladly share it. However it is rather large an I cannot just paste it into the post; I also do not have a WEB site to post the code. On the other hand, if you provide me with your e-mail, I will send you the whole project together with the schematics for the board that i use to "host" it.
Comment by Hamish on March 15, 2011 at 6:50pm
dude google docs is the best place for sharing files (any files, not just for docs)

Comment

Join DIY Drones

1

2

3

4

5

6

7

8

9

10

## Contests

Season Two of the Trust Time Trial (T3) Contest has now begun. The fourth round is an accuracy round for multicopters, which requires contestants to fly a cube. The deadline is April 14th.

A list of all T3 contests is here

1288 members

24 members

78 members

51 members

682 members