Replies to This Discussion

Permalink Reply by Simon Wood on January 21, 2010 at 9:42pm

I managed to get a little further (I had my axis mucked up), but I still don't really see how you can do compensation pitch when you have both a roll and pitch angle to deal with.

Simon.
(aka. Mungewell)

Attachments:

• Screenshot.png, 23 KB
• motion_nunchuk.c, 7 KB
• live_graph.py, 9 KB

• ▶ Reply

Permalink Reply by Roy Brewer on February 5, 2010 at 3:05pm

Simon,

I'm writing a complementary filter implementation in Python as well. I may use Python to code a DCM like algorithm. To answer your questions:

1) Yes. If the sensors are not physically accelerating, then the accelerometers will measure the gravity vector, from which you can determine two of the three orientation angles

2) I believe you can use the following code to determine the pitch and roll angles using an accelerometer (assuming the net acceleration is one g). I have not tested this in flight yet, but it appears to work in experiments where I'm holding the sensor board in my hand and turning it around:

# xacc, xacc, xacc are the accelerometer outputs in g (or m/s/s or any other valid engineering unit)
# Depending on the orientation of your sensors, you might not need the minus signs

yzmag = sqrt(yacc**2 + zacc**2) # net component of the sensed gravity vector that is perpendicular to the X axis
angley = -atan2(xacc,yzmag) # "pitch" angle

xzmag = sqrt(xacc**2 + zacc**2) # net component of the sensed gravity vector that is perpendicular to the Y axis
anglex = -atan2(yacc,xzmag) # "roll" angle


I don't think your code is correct.

target = arcsin( dot(accv, x_vec) / accl)

which I believe is analogous to

xyzmag = sqrt(xacc**2 + yacc**2 + zacc**2) # magnitude of the entire sensed gravity vector
angley = -atan2(xacc,xyzmag) # not quite the "pitch" angle


Your computed pitch and roll angles won't be orthogonal to each other in the general. It will work if one of the angles is zero, which is perhaps why you made the statement in your comment above.

3) No. The accelerometers cannot sense any rotation around the gravity vector, no matter how your sensors are oriented. You can think of the gravity vector as tracing out paths along a sphere as the sensor moves. The sphere is only 2-dimensional - you can get latitude and longitude information (equivalent to pitch and roll), but not "yaw"

- Roy

• ▶ Reply

DeveloperPermalink Reply by William Premerlani on February 6, 2010 at 11:31am

Hi Simon,

I think its really cool that you are going to code up the DCM algorithm for Wii, using Python.

I see that Roy answered your questions. (Thank you Roy!) But I thought I would offer my two cents worth anyway.

First of all, you can do the computations entirely with vectors, which is my preference. You can do them with Euler angles as well, but personally, I think that vectors are easier. Here is how you can do it:

The combined roll-pitch error can be determined from two vectors:

1. The vector formed by the earth frame Z axis, as seen in the plane frame of reference. Call this vector 1. This vector is the bottom row of the direction cosine matrix.

2. The gravity vector, as measured in the plane frame of reference. Call this vector 2.

If there is no roll or pitch error, vector 1 and vector 2 are parallel. If they are not parallel, that means there is a roll and/or pitch error. The angle between the two vectors is an indication of how big the error is, and the perpendicular to the plane that contains both vectors is the axis that we can rotate one of the vectors to align it with the other one. This is a job for a vector cross-product.

If we take the cross product of vector 1 with vector 2, we will get a third vector whose magnitude is proportional to the sine of the angle between vector 1 and vector 2, and whose direction is perpendicular to them both. Because it is perpendicular to them both, it is parallel to the axis that we can use to rotate one of the vectors towards the other one. So, what we do in the MatrixPilot firmware for the UAV DevBoard, is to take the cross product of vector 1 and vector 2, to provide a third vector that can be used directly to adjust all three gyros. Each component of the cross product vector represents the drift error in the corresponding gyro.

If you have not already read it, I suggest you read an explanation of the DCM algorithm.

You also might want to read the rmat.c file in the MatrixPilot firmware.


You mentioned that you read the ArduIMU code. If you look at where Jordi does the roll-pitch compensation, it is my understanding that he uses vector cross products, and not Euler angles. He does compute Euler angles for display purposes, but the roll-pitch compensation is done using vectors.

Best regards,
Bill Premerlani

• ▶ Reply

Permalink Reply by Simon Wood on February 6, 2010 at 3:40pm

I had seen the DCM document that you wrote, and am basing my code off that.

I discovered that I had made a mistake in the code (at least 1 ;-) so here's the correct DCM code with the extra '-' in 2nd row.
--
# rotate dcm by delta
update = array([ [0, -time_delta * omega[2], time_delta * omega[1] ],
[time_delta * omega[2], 0, -time_delta * omega[0]],
[-time_delta * omega[1], time_delta * omega[0], 0]])

dcm = dcm + (dcm * update)
--

I'm working on using the acceleration vector to compute/correct the drift, but the code is rather messy at the moment. I'll post this when it's cleaned up.

Simon.

• ▶ Reply

Permalink Reply by Roy Brewer on February 7, 2010 at 10:31am

Simon,

I stand corrected. Although the accelerometers can't sense pure yaw motion about the gravity axis, by tilting the gyro's axis you can use them to determine the drift in all three gyros, which is what you asked (as Bill noted). However, I'm not sure you necessarily need to mount the sensors at an angle since the aircraft will spend some time tilted away from horizontal as it flies. I suppose it depends how much you want to correct drift in the yaw axis vs pitch and roll. For my application (quadrotor), the yaw axis isn't as important as pitch and roll. Of course its still better to have another correction vector since all the above scheme is doing is spreading that "unobservable" yaw drift around to the other axes. But, since the gyros don't seem to drift that much, so this may be good enough. I'll let you know when I finish building it!

Also, as Bill said, it would be better to limit the use of trigonometric functions where possible. It probably doesn't matter running Python on a PC, but those functions are fairly computationally "expensive" for an on-board microprocessor (assuming that building one is your ultimate goal)

Thanks again Bill for your comments.
- Roy

• ▶ Reply

DeveloperPermalink Reply by William Premerlani on February 7, 2010 at 11:29am

Hi Roy,

I hope I did not cause any confusion. As far as I know, you do not stand corrected about anything.

I think you were referring to the yaw drift. Although the roll-pitch correction in general does produce corrections for all three gyros, it only produces something for the yaw gyro when the IMU is not level. For example, if you roll the IMU 90 degrees, the yaw gyro now measures pitch.

So, you cannot correct for yaw drift by tilting the board. This idea comes up from time to time, it does not work. There were a couple of discussion threads on the topic, I cannot find them off hand.

What it comes down to is this: If you tilt the board, roll-pitch compensation cannot detect a gyro drift vector that is parallel to the Z axis of the earth. When the board is level, this means that it cannot detect drift in the yaw gyro. But when the board is not level, it means that it cannot detect a weighted sum of drifts, where the weights are the elements of the bottom row of the R matrix.

What this means is that when you tilt the board, you will wind up with residual drift errors in all 3 gyros. However, there will be two constraints on what the three values can be, based on the orientation of the board. Basically, what you will wind up with is the residual uncorrected yaw drift of the 3 gyro system being spread over all three gyros.

Best regards,
Bill

• ▶ Reply

DeveloperPermalink Reply by William Premerlani on February 7, 2010 at 12:32pm

Hi Roy,

Here are some more comments on yaw correction:

In order to determine an attitude reference for gyro drift compensation, you need two reference vectors. One vector will not do the job, because a rotation around an axis parallel to the reference vector is not detectable.

So, you cannot adjust for yaw drift by using gravity alone as a reference vector. A rotation of the body frame of reference around an axis parallel to gravity is undetectable, so you cannot detect yaw error. And you cannot get around the problem by tilting the gyros. If you do, all that will happen is that you will wind up with a residual drift in each gyro, forming a gyro drift 3D vector that is parallel to gravity.

This is not inconsistent with the fact that the roll-pitch compensation produces corrections for all three gyros. It comes out that way because we are working in symmetrical 3D coordinate systems in which all vector computations have three components.

Best regards,
Bill

• ▶ Reply

Permalink Reply by Simon Wood on February 8, 2010 at 12:01pm

And you cannot get around the problem by tilting the gyros. If you do, all that will happen is that you will wind up with a residual drift in each gyro, forming a gyro drift 3D vector that is parallel to gravity.

I may just be being dumb, but I don't understand why this would give a residual drift.

In a static sensor set up each (orthogonal) gyro will have a drift due to inaccuracies in capacitive sensing it's vibrating mass - there will be no cross coupling of drift between gyros. If there is some portion of the gravity vector in the gyro's plane then this vector can be used via a PI (or PID) loop to compensate the drift.

I don't see why tilting the gyros pack so that all 3 gryos have a portion of the gravity vector would cause 1 or more to 'flip out' and become inaccurate. Obviously at some rotations of the airframe the gravity vector will be perpendicular to the gyros axis and there unusable - when this occurs I would propose 'freezing' the drift error correction and using the last known best guess.

With the gyro pack pitched down (say by 30') in the airframe the DCM should still function correctly in itself, for extracting the 'true' euler angles a snapshot of the DCM could be pitched up by 30' and then the angles computed. More computation is required, but may be worth it if correct yaw drift for a significant portion of the time.

Note that I am thinking about the quad-copter platform which does not really work with forward motion/GPS yaw correction.

Simon.

• ▶ Reply

DeveloperPermalink Reply by William Premerlani on February 8, 2010 at 12:41pm

Simon,

Perhaps we are in agreement and don't know it, so let me state what happens more concisely.

What I am saying is that you need two reference vectors to compensate for the gyro drift of all three gyros. This is a well known fact. Typically those two vectors are gravity, estimated by an accelerometer, and course over ground, furnished by a GPS, or perhaps a 3D magnetometer.

If you try to get by with accelerometers only, you wind up with only one reference vector, and you cannot use it to figure out which way is north, no matter what you do. The accelerometers will tell you which way is down only. So, what will happen is that the estimated coordinate system of the body frame, represented by the direction cosine matrix will slowly drift around the the earth frame Z axis.

This means that the third row of the direction cosine matrix will be constant, and accurate, while the elements of the first two rows will slowly change their values, representing a slowly drifting yaw angle between the earth and the body coordinate systems.

Do we agree on that? If so, we are saying the same thing. If not, let me know, I'll see if I can explain it better.

Best regards,
Bill

• ▶ Reply

Permalink Reply by Simon Wood on February 8, 2010 at 1:36pm

Thank you for taking the time to explain. I 'kind of' follow your explanation, but I think that you are looking at a larger problem/situation than me (at the moment).

If you try to get by with accelerometers only ... you cannot use it to figure out which way is north

Right now I do not care which way is North, I am thinking more of stablisation rather than auto pilot. What I want to achieve is a quadcopter which can hold a consistant yaw when hovering (the user will have pointed it in the direction they want already).

In my code I correct the drift error 'outside' the frame of the DCM, that is on a per-gyro basis. I measure the difference between each gyro plane (using euler angles) and part of the gravity vector (when appropriate) and then feed this into a PI loop to provide a correction to the gyro measurement before it is used to update the DCM.

As you mention in order to find North, I follow that I would need a changing GPS location or a magnetometer. For a heli/quad copter GPS would have to be adjusted if the copter knew it was flying sideways.

Simon.

• ▶ Reply

DeveloperPermalink Reply by William Premerlani on February 8, 2010 at 2:55pm

Simon,

OK, it looks like you and I are agreed. I lost track of the fact that you were going to use a separate method to find north.

In that case, you could use what I call "DCM lite", which computes only 3 of the 9 elements of the matrix, basically, just the bottom row of the matrix.

If you look at the calculations, the bottom row of the matrix is updated from the bottom row of the matrix and the 3 gyro signals. And the roll-pitch drift compensation can be computed from the bottom row of the matrix and the accelerometers.

Best regards,
Bill

Permalink Reply by Roy Brewer on February 8, 2010 at 10:41pm

Hi Bill,

Actually, I'm not sure Simon agrees with you.

I think we all agree that in theory, the accelerometers will not be able to sense any rotation about the gravity vector. In practice, however, the gyros drift pretty slowly. In my own experience with a handheld sensor board, and anecdotal evidence from other folks who are flying their own quadrotors, the gyros only need a periodic correction to adjust for drift. So, as long as the sensor board is tilted for some amount of time (as will naturally happen in a quadrotor), the z-axis gyro should get some correction. It may not be perfect, but it should act to reduce the drift in all of the gyros, even if it cannot completely eliminate it.

- Roy

p.s. I had thought I had come up with an original control / sensing scheme by just tracking the gravity vector and the orientation around the gravity vector, but it looks as if you and your colleagues already figured most of it out with "DCM lite". Is this algorithm explained somewhere? Is there code available?

• ‹ Previous
• 1
• 2
• Next ›
• Page