# DCM in python

Mostly as a understanding/learning exercise I am coding a DCM in Python based on the write up of the ArduIMU. My 'sensor' is a Wii Nunchuck with a Motion+ mounted inside the Nunchuck, interfaced to a PC via I2C.

The bit that I am having brain trouble with is the 'correction' of pitch/roll using the gravity vector. Hopefully someone here will be able to point me in the correct direction.

1). Does the gravity vector totally describe the pitch/roll(/yaw?) angles, assuming the sensor is not accelerating in any direction?

2). If you take the angle(s) between the gravity vector and a plane (X, Y, Z) you can infer roll and pitch if the sensor is only rolled  or pitched. How do you calculate if the sensor is rolled AND pitched at the same time?

3). Given that my sensor is not augmented with GPS, does it seem possible to correct the yaw if there is sufficient 'gravity vector' in the yaw angle - for example if the sensor was pitched 45' downwards?

Thanks,
Simon
(aka. Mungewell)

Views: 2215

### Replies to This Discussion

Hi Simon and Roy,

I went back over this discussion again, and finally understood what you both were talking about and asking, I hope. ;-) Basically, you are both right.

First, it is possible to get pretty good performance without a yaw reference vector, using just a down reference vector, depending on how stable the gyros are, and if the temperature is not changing much.

I just did a test using the roll-pitch-yaw demo running on the UAV DevBoard, without a GPS. The uncompensated yaw drift is only a few degrees per minute, its certainly way less than 0.1 degrees per second.

Then, I reran the demo with a deliberately large yaw offset, by rotating the board around the Z axis during power up. Of course, it was "dizzy" after power up, with yaw drift of a few degrees per second.

I then rotated the board 90 degrees, and the PI drift controller gradually readjusted the Z axis gyro back to zero. Then, when I rotated it back to level, the yaw drift was normal.

So, you guys are right about that. You can improve the Z axis gyro drift by changing the orientation of the gyros, and then moving them back. Makes sense.

However, I hope we are all agreed that you cannot drive the yaw drift to zero by simply mounting the gyros in an unusual way, and then keeping everything level. There is no free lunch in that regard.

Regarding "DCM lite", I discussed it in someone posting, I cannot find it now. Our team did not follow it up, because we need all 9 components of the matrix. Unless you are squeezed for CPU loading, you might as well use the full algorithm.

If you want to try it in any case, here is basically what you do:

1. Compute only the third row of the matrix. The update of the third row can be computed from the third row and the gyro vector.

2. Renormalize the third row. The only condition that needs to be satisfied is the sum of the squares of the 3 elements is 1.0

3. Compute the rotation error vector from the cross product of the third row with the measured gravity vector.

4. Use the rotation error vector as the input to the PI feedback controller.

Best regards,
Bill
Bill -

While we are discussing items about rotating the gyro axis', what do you think about rotating two accel axis' by 45 degrees to improve the accel sensitivity? Instead of 0,0,1 we could get 0, 0.707, 0.707 for level flight increasing the accel sensitivity. This increased sensitivy comes at the expense of complicating the math a bit to back correct the accel rotation offsets.

- Rich
Thanks, Bill. I am glad to see we are in agreement :-)

- Roy
Rich,

I don't see the benefit of this arrangement. Can you explain further? What do you mean by "sensitivity"?

- Roy
Hi Rich,

The sensitivity of the accelerometers has never been an issue, almost anything will work. With respect to the accelerometers, the issue is that they measure gravity minus acceleration, so you have to somehow separately compute acceleration and add it back in, to get gravity.

But the real crux of the problem is converting gyro rate information into attitude information. To do the best job on that, you need good gyros, and an algorithm such as the DCM algorithm, or extended Kalman filtering.

Bill
So, you guys are right about that. You can improve the Z axis gyro drift by changing the orientation of the gyros, and then moving them back. Makes sense.

Glad you can see the expected behaviour working in real life.... Did you adjust the code to compare the gravity vector to the pseudo/tilted yaw angle, or did it just work as is?

However, I hope we are all agreed that you cannot drive the yaw drift to zero by simply mounting the gyros in an unusual way, and then keeping everything level. There is no free lunch in that regard

I like free lunch!

I can't see a reason as to why the DCM won't cope with ALL gyros being mounted at an angle (ie. they are still orthogonal to each other and correctly aligned to the accelerometers).

I believe that the DCM will happily track the situation regardless of orientation of the board and that when I want to 'export' the euler angles, I can copy the DCM matrix values, perform a fixed rotation on that (temporary) copy and 'export' from there.

This would mean more math to perform which may be worth it in a system which would otherwise need addition hardware (meaning magnotometers; remember GPS doesn't work indoors).

It would also mean that any correction vectors, such as GPS would have to be adjusted to the tilted airframe reference before they were used.

Note: I am not suggesting that this is the best way of doing yaw correction, but think it might be a nifty trick for a low cost indoor quad.

BTW - it may just be my calibration being way off, but the Numchuk/Motion+ system I have seems to have pretty awful drift.

Yours,
Simon.
Roy -

I was trying to interpret Freescales app note AN3461 on sensitivity. But I guess as Bill pointed out it isn't a problem that really needs improvement.

- Rich
Rich,

I see what you mean now. I think one way to think about it is that, since the sensors are orthogonal, when one is at its least sensitive, the others are at their most sensitive, and vice versa.

- Roy

1

2

3

4

5

6

7

## Groups

529 members

52 members

564 members

67 members

• ### ArduCopter User Group

3157 members

Season Two of the Trust Time Trial (T3) Contest
A list of all T3 contests is here. The current round, the Vertical Horizontal one, is here