trying to implement DCM...
Hi, all
I have read William Premerlani's theory paper several times and a lot of it is beginning to soak in, but I must be missing somthing in my implementation.
I am trying to keep an accurate view of earth-relative pitch and roll angles on an embedded device with 3 axis of rate gyros and 3 axis of accelerometers.
My approach is apparently flawed, and I'd appreciate any and all feedback. The approach is to maintain only the bottom row of R matrix as follows:
(1) initialize the bottom row of R matrix floating point values (eventually I'll use integer but for now, I'd be happy to get the floating point working even though it's much CPU cycles) by performing necessary trig operations on an initial known, stable value for pitch (theta) and roll (phi), obtained from accelerometer with nothing but 1g acting on the accelerometers.
(2) thereafter, perform the following at a fixed, quick time interval (I've tried both 20 ms and 40 ms intervals).
(2A) measure the three gyro ADC values.
(2B) construct the assymetric matrix on page 15 of the theory paper by multiplying these readings (normalized to radians per second) by the time interval (ie, 0.020 or 0.040).
(2C) update the bottom row of the R matrix by multiplying its current row values by this assymetric matrix.
(2D) calculate current pitch from -asin ( R matrix row 3, col 1)
(2E) calculate current roll by knowing 2D value and using asin of R matrix row 3, column 2)
So... I'm only maintaining the bottom row of R matrix.
Somewhere, my logic is flawed! I have a fixture on which I can rotate around earth's yaw axis and also adjust and fix target's roll and pitch angles relative to earth. I am constantly feeding the values back for real-time display at PC.
I fix the pitch at 45 degrees and roll at 0 degrees, for example. Before turning the fixture, I see values at the PC for pitch, roll, and also the 3 values for the bottom row of the R matrix.
Before moving the fixture, I see a nice 45 degree pitch, 0 degree roll, and -0.707, 0, 0.707 displayed at the PC. These drift a bit after 20-30 seconds, but that's ok, for now, since I'm doing nothing to correct for drift yet.
But when I spin the fixture, the values immediately go very wrong.
What flaw(s) are in my approach?
Any and all comments are greatly appreciated.
I have read William Premerlani's theory paper several times and a lot of it is beginning to soak in, but I must be missing somthing in my implementation.
I am trying to keep an accurate view of earth-relative pitch and roll angles on an embedded device with 3 axis of rate gyros and 3 axis of accelerometers.
My approach is apparently flawed, and I'd appreciate any and all feedback. The approach is to maintain only the bottom row of R matrix as follows:
(1) initialize the bottom row of R matrix floating point values (eventually I'll use integer but for now, I'd be happy to get the floating point working even though it's much CPU cycles) by performing necessary trig operations on an initial known, stable value for pitch (theta) and roll (phi), obtained from accelerometer with nothing but 1g acting on the accelerometers.
(2) thereafter, perform the following at a fixed, quick time interval (I've tried both 20 ms and 40 ms intervals).
(2A) measure the three gyro ADC values.
(2B) construct the assymetric matrix on page 15 of the theory paper by multiplying these readings (normalized to radians per second) by the time interval (ie, 0.020 or 0.040).
(2C) update the bottom row of the R matrix by multiplying its current row values by this assymetric matrix.
(2D) calculate current pitch from -asin ( R matrix row 3, col 1)
(2E) calculate current roll by knowing 2D value and using asin of R matrix row 3, column 2)
So... I'm only maintaining the bottom row of R matrix.
Somewhere, my logic is flawed! I have a fixture on which I can rotate around earth's yaw axis and also adjust and fix target's roll and pitch angles relative to earth. I am constantly feeding the values back for real-time display at PC.
I fix the pitch at 45 degrees and roll at 0 degrees, for example. Before turning the fixture, I see values at the PC for pitch, roll, and also the 3 values for the bottom row of the R matrix.
Before moving the fixture, I see a nice 45 degree pitch, 0 degree roll, and -0.707, 0, 0.707 displayed at the PC. These drift a bit after 20-30 seconds, but that's ok, for now, since I'm doing nothing to correct for drift yet.
But when I spin the fixture, the values immediately go very wrong.
What flaw(s) are in my approach?
Any and all comments are greatly appreciated.
Replies to This Discussion
-
Permalink Reply by Mike Palmer on -
Brian and Bill -
Like I said, I owe you guys a pizza each! Thanks to your help, "DCM-lite" seems to be working now. The problem was, as I believe you both suggested, a simple sign thing... the polarity of one of my three gyros is different from the setup described in the paper. Even though I thought I'd tested all combinations, I must have missed one.
A follow up on a few notes and a few remaining questions, just for my understanding...
(1) Brian - your interpretation of the graphs I submitted was correct... The movement I'd recorded in the bitmap was of a fixed roll, not a fixed pitch. Sorry.
(2) Brian, yes, there is a bias in the graphs that is subtracted further down.
(3) Brian - I still do not understand the final step in the algorithm you described several days back... where you're using (in the comments) acos( Down Vector[] ) to get pitch and roll in earth frame.
I do not understand your use of acos(). I am using -asin( Down Vector[0]) since Down Vector[0] = -sin(theta) from page 10 of Bill's paper. Please to clarify.
(4) Bill - per your suggestion, in my algorithm's current form, I now do the normalization, but not the orthagonalize-correction. In DCM-lite, I realize that since I'm only maintaining bottom row, it's not possible to do the orthagonalize-correction. But... gradual loss of orthagonality is still occurring in DCM-lite, correct? It's just that DCM-lite cannot address it. Please to confirm (or deny!)
(5) Bill - you stated earlier: "In your experiment, a pitch is a rotation around the Y axis. In terms of DCM, it changes only the first and last elements of the bottom row." I don't understand your statement... theta is involved in all 3 elements of the row. Please to explain.
So.. it seems to work pretty well now (huge thanks again)... it's refreshing to me to see that there are people out there that freely (well, for price of a future pizza, anyway!) give thorough assistance to those struggling with particular issues...
I do have a few issues left I was hoping to get some feedback on...
(A) Gyro bias calibration techniques. Any suggestions on how you deal with these or references to further information? The gyros I'm using are ST LISY300AL parts, which yield 3.3 mV / (degree/sec), then have some gain via external op-amp gain circuits before connecting to micro ADC input pins. By the time it reaches the micro, they're approximately 4 ADC points per degree/sec.
I have filtering on the circuit board, but it seems that even when not moving, I still get fluctuations in a few ADC points from gyro readings. I can dampen the analog input readings via averaging, which helps somewhat, and also use a "band" approach (discard fluctuations less than 2 ADC points from bias for example). These seem to help but of course have a negative side, too.
Any feedback appreciated.
(B) At risk of being booted from an RC plane forum, I'm actually attempting to use the DCM-lite approach to control attitude of a (real) boat, via these sensors and DC motors which control position of trim tabs along the back of the boat.
Bill's paper describes an approach of using accelermoter feedback to stabilize gyro drift, based on knowing GPS speed and factoring out centrifugal acceleration. I also have GPS available on my platform, and I think this is do-able, even with DCM-lite, but for one thing... a plane may move, on average, in the direction pointed, but this is not so true for a boat, which tends to pitch at 5-12 degrees even though it's moving across a water surface. Sin(12 degrees) is a pretty large number that probably cannot be ignored for long, so I do not think that approach will work for my boat. Comments and/or feedback appreciated.
So ends my dissertation!
Thanks!
-Mike
-
Permalink Reply by William Premerlani on -
Mike,
Its good to hear that you have "DCM lite" working, I am sure there are lots of other applications for it. I will be by your house some day for that slice of pizza.
Regarding your questions:
(4) There are no further "orthogonality issues". You are done. It works. The bottom row of the matrix has three elements. Through normalization, there are only two independent elements. The drift correction (you are going to do the drift correction using the accelerometers, right?) will assure that there is no accumulated error. The only question of orthogonality would be between the bottom row and the first two rows, and since you are not using them, the question is moot. In other words, your computation of the bottom row will be correct, without any long term drift or error accumulation. Its solid.
(5) Regarding what I was saying about your experiment, it was for the specific situation that you had, because the roll angle was zero. You are right, in general if the roll angle is not zero, a pitch will change all three elements.
(A) Regarding gyro offset and drift, the use of the accelerometers for drift compensation will take care of that. There is also an issue of calibration, but since your boat cannot roll or pitch 360 degrees, calibration is not an issue, it is ok if the calibration is a little off. If you want to calibrate your gyros, you can put your board on a swivel chair and rotate it a few times, and use the results to fine-tune the gyro gains.
(B) As it turns out, I have also have successfully used my board to control a 23 foot sailboat, so I have given some thought to some of the issues related to the water surface, so I would be happy to discuss them with you, if you want. What are you trying to do?
Bill
-
Permalink Reply by Mike Palmer on
-
Bill -
I'm trying to control pitch and roll of a boat (fishing boat, speedboat) thru the use of 2 DC motors that adjust plates along the left and right of the stern of the boat. adjusting the plate position (trim tabs) causes different amounts of deflection of water over which the boat is travelling. It also causes the attitude of the boat to change.
Since the boat is typically pitching up as it moves (mostly) horizontally over the water, and since it could also be listing to one side, I think the cross product of velocity and Wgyro (Eqn. 25) requires three multiplications, but the multiplications require I know the pitch and roll angles first. This seems chicken-eggish. What am I missing?
-
Permalink Reply by William Premerlani on
-
Mike,
Actually, equation 25 requires only 2 multiplies.
Now, to your question. Assuming the craft (air, sea, or land) is moving in the direction that it is pointing, the sideways acceleration measured in the frame of reference of the craft is equal to the speed, times the yaw rate, measured in the frame of reference of the craft. This is the familiar acceleration you feel in a car when it is turning. In a car, this acceleration is always sideways, because the car is moving forward. You can compute it correctly without knowing the pitch and roll angles of the car.
There is a vertical acceleration that is the product of the speed times the pitch rate. This is what you feel in a roller-coaster.
It is not entirely chicken and eggish, because the accelerations and the rotations are both measured in the same reference frame, the craft. It all comes out.
But the assumption is made that the craft is moving in the direction that it is pointed. That is approximately true for an aircraft, not so good for a boat, as you point out. So, in a way, you are right, the thing we are trying to correct (pitch and roll angles) depends on an assumption about the pitch and roll angles. It does not require that we actually measure them.
Best regards
Bill
-
Permalink Reply by William Premerlani on
-
Mike,
By the way, one of your big challenges with your trim tabs is going to be to move them quickly.
The sailboat project I am working on uses a DC motor driven jack-screw with a 12 inch stroke, for controlling the rudder. Its rather slow, it takes around a minute to go from end to end. Its find for what we are doing, which is maintaining heading.
For what you want to do, have you made any estimate for how fast the tabs have to move? Have you found an actuator that can provide both the force and the speed that you are going to need?
Bill
-
Permalink Reply by Mike Palmer on
-
Bill-
The actuators for my system are pretty fast, so I think not a problem... 3-4 seconds, full scale movement.
Question on the A centrifugal calculation in Eqn. 25... how is an acceleration m/(s*s) computed from rad/sec * m/s?
-
Permalink Reply by William Premerlani on
-
Mike,
Regarding the computation of acceleration, equation 25, the units come out just right.
As you point out, acceleration is in meters per second per second.
Rotation rate is in radians per second. Radians is a dimensionless angle measurement.
Velocity is in meters per second.
So, rotation rate time velocity has units of radians-meters per second per second, which is the same thing as meters per second per second.
Technically speaking, the units of rotation are simple per second.
There is a converion in my software, though, because acceleration measured by the accelerometers is measured in terms of g's, so I do have to do that conversion.
Bill
-
Permalink Reply by Brian Wolfe on -
Hey Mike,
Congratulations on getting the DCM lite running. That's great.
Refereeing to your question (3) above, If you look at my post I wasn't suggesting that you use ACOS to find your angles, I was letting you know what each of the lower DCM elements were. The name DCM is an abbreviation for Direction Cosine Matrix. It's called that because each element in the matrix (assuming you don't violate all the rules such as orthogonality, normalization ect..) is the cosine of the angle between one of the axis in the reference frame to one of the axis in the body frame. The last row of the DCM relates the Z axis of the reference frame to each of the three axis of the body frame. The first element in that row is the cosine of the angle between the Z axis of the reference frame and the X axis of the body frame. That should be useful for determining pitch.
As an example, if we are level, the last row of the DCM should be 0,0,1. That's what we initialize it to and it represent a normalized vector pointing straight down in the reference frame(hence the reason I call it Down_Vector). If we take the acos of the first element we get acos(0) = 90. That's correct. When we are level the angle between the reference frame z axis and the body frame x axis is 90 degrees. If we want to call that term pitch though 90 degrees when we're level isn't very intuitive so we subtract that value from 90 degrees to give us an angle of zero. So now we have Pitch = 90 - acos(Down_Vector[0]). If we remember our trig identities, we realize that 90-acos(angle) = asin(angle) so that's what we use for this element.
Sorry for the long explanation to tell you that Cos(angle) = sin(90-angle), but hopefully going thru this will give you a better idea what the terms in the DCM mean and how they can be used. I wanted to also mention (and maybe you already know this from reading Bill's paper) that if your going to close a loop to correct for pitch and/or roll you can use the DCM terms directly without converting to an angle first. For instance, since the first element in the Down_Vector is the cosine of the angle between Ref_Z and Body_X it's zero when the platform is level. That represents zero error. If you pitch up you get a positive value and if you pitch down you get a negative value. So we can use this term directly as the error term in our control loop(modifying with appropriate gain and signs). It won't take much noodling to realize that it's not linear, and that could be an issue in some situations, but it is pretty linear around 0 and particularly in the case of a boat that's where you'll be operating (hopefully).
You mentioned you were concerned about some noise. The gyros will put out a certain amount of noise even when they're perfectly stationary. If you look in the data sheet for the Gyro you'll see the noise spec. You should check to make sure the noise your seeing is on the order of what the data sheet calls out. If your getting more noise it could indicate a problem with the circuit/layout. If it's close your probably fine. It seems most people want there data to look as smooth as possible. Although this makes for pretty graphs, it can actually cause more problems than the noise in a real system. In general, you can't filter without adding delays or lag in the system. If that lag becomes excessive it can make it very difficult to close a loop and remove higher frequency errors. In the final system, there is often one filter that smooths out much of the noise that ripples through to the output command and that is the vehicle body or airframe itself. If you do have some 50Hz noise rippling through it's unlikely the boat will respond to it since the amount of energy required to move the mass of the boat at 50 Hz is enormous. I'm sort of over simplifying and I'm not saying to ignore the noise all together, I'm just saying it's not important that the data be perfectly smooth and there's just as much risk of over filtering as there is of having excessive noise.
So it looks like your getting about a 0.25 Deg/S resolution with your hardware? You must be using a 10 bit converter? Where do you have your filter bandwidth set at?
Brian
- ‹ Previous
- 1
- 2
- Next ›
© 2010 Created by Chris Anderson.
