Comment by Lew Payne on April 5, 2010 at 5:53pm

Bill - Yes, I've read your three papers as well as one of the Mahony papers. I also read your airspeed one, and the lengthy Q&A responses you did in threads/blogs. I like the elegance of the DCM and quaternion approach, along with the problems it solves. I just have no way of estimating nor evaluating if more precise sensors would yield significantly better results. You're in a better position to know that than I am... as I've not yet opened my checkbook to purchase components. But, since I'm working on an STM32-based port, I have the opportunity to start from scratch when it comes to sensors. I'm trying to ascertain "best bang for the buck" or accuracy/cost ratio... within a realistic range (i.e., perceptible accuracy that makes a difference).

I do plan on having a pitot tube, for airspeed. I don't see any other practical way of impaling squirrels while taxiing for takeoff.

Comment by Lew Payne on April 5, 2010 at 5:55pm

BTW - Thanks for the prior response. I'll put the HMC5843 on my list, rather than the 6343. If there are any other sensor changes you'd make, knowing what you know today, if you were starting from scratch... I'd sure love to hear about them.

Comment by Alex Zarenin on May 16, 2010 at 10:18pm

Dear Bill,

I have been reading and re-reading your very informative document "DCM IMU Theory : first draft" - I would like to use your algorithm with my board that uses Sparkfun's 6 DOF IMU Razor and 3-axis magnetometer to derive the attitude angles for the quadrocopter.

However I am stil fighting with understanding how to get the attitude angles from the DCM. For example, if I start with the body frame aligned with the Earth frame and apply very small rotations I get the following rotation matrix:

Small rotation: Yaw=0.05, Pitch=0.06, Roll=0.07
1.0000 -0.0500 0.0600
0.0500 1.0000 -0.0700
-0.0600 0.0700 1.0000

which after normalization looks like:

Axis of Body frame in the Earth frame
Xb/e Yb/e Zb/e
0.9971 -0.0478 0.0597 // Xe/b - X axis of Earth frame seen from the Body frame
0.0519 0.9962 -0.0696 // Ye/b - Y axis of Earth frame seen from the Body frame
-0.0561 0.0725 0.9957 // Ze/b - Z axis of Earth frame seen from the Body frame

The document indicates that "you can find pitch attitude by taking the dot product of the roll axis of the aircraft with the ground vertical" - would you please illustrate on the above example what does that mean.

Greatly appreciate your help,
--Alex

Comment by William Premerlani on May 17, 2010 at 4:56am

Hi Alex,
The roll axis of the aircraft in earth coordinates is the first column of the matrix, { 0.9971 , 0.0519 , -0.0561 }.
The ground down vertical is { 0 , 0 , 1 }, in earth coordinates.
Taking the dot product, we get -0.0561. This means that in the down Z direction, there is a -0.0561 pitch, or in the up Z direction, there is a 0.0561 pitch.
Best regards,
Bill

Comment by Alex Zarenin on June 28, 2010 at 10:08pm

Dear Bill,

Slowly but surely I am working on my implementation of your DCM algorithm using my custom board. To simplify development I am writing a C# code on the PC using data wirelessly streamed from the board as I move it around :)

The gyro part of the algorithm works fine, but, as expected, the error slowly creeps in. Moreover, the gyro gains do differ from the nominal, so with the integration etc. I hardly get 90 degrees rotation in DCM after rotating board 90 degrees, so it is time now to cancel the drift using acceleration measurement. With regard to this I would like to ask you a few questions:

1. Does the acceleration vector need to be normalized, that is brought to a unit length before taking the cross product with the Earth vertical?
2. Is it possible to avoid normalization by adjusting values of Kp and Ki in the feedback loop?
3. How to pick a good initial estimate of the Kp and Ki coefficients to start playing around?

In a steady state (on the table) my accelerometer (after ADC) produces a vector fluctuating around {0, 0, 3300} with 3,300 reflecting Earth gravity; however as I move the board individual measurements do not always (actually, never) produce a vector of Sqrt(X^2 + Y^2 + Z^2) = 3300 – thus the questions 1 and 2 above. With the question 3 I am just trying to avoid doing a lot of unnecessary test runs.

As usually, would greatly appreciate your suggestions.
Thank you,
--Alex

Comment by William Premerlani on July 1, 2010 at 5:05pm

Hi Alex,

Here are the answers to your questions:

1. Does the acceleration vector need to be normalized, that is brought to a unit length before taking the cross product with the Earth vertical?

No, the acceleration vector does not need to be normalized. There is no point to do that. Just scale the gains to take into account the nominal value of gravity. If you compensate for acceleration, there should not be much variation in the final estimate of gravity. In any case, the PI feedback controller will work over a very wide range of gains.

2. Is it possible to avoid normalization by adjusting values of Kp and Ki in the feedback loop?

Yes.

3. How to pick a good initial estimate of the Kp and Ki coefficients to start playing around?

What we did was to analyze the closed loop transfer function. It is a second order transfer function. A good place to start is by deciding where you want the two poles of the transfer function.

The denominator of the transfer function is s*s+Kp*s+Ki.

My preference for the transfer function is to make the demoninator equal to (s+1/tau)**2, where tau is the desired time constant of the response to a disturbance. This produces a critically damped response. We started with tau = 10 seconds.

So, Kp = 2/tau, and Ki = (1/tau)**2

With tau = 10, Kp = 0.2, and Ki = 0.01

That is a good starting point. Then you can adjust Kp and Ki to suit you.

Best regards,
Bill

Comment by Alex Zarenin on July 1, 2010 at 8:17pm

Hi Bill,

Thank you very much for the clarification! Based upon your suggestion for Kp and Ki and given that the gravity vector translates into 3,300 units, to preserve tau = 10, I would have to set:
Kp = 0.2 / 3,300 = 6.06e-5
and Ki = 0.01 / 3,300 = 3.03e-6

Finally, your implementation of data fusion utilizes GPS and accelerometer, with GPS being relatively slow (1 to 5 Hz) in updating the measurements. I am targeting the heli-type application – actually the current board that I am working on has 3 gyros, 3-axis accelerometer and 3-axis magnetometer and will output (over UART) all the components of the attitude data for the second board, which will control motors and do the navigation.
Given this setup, do you see any downside to reducing tau to, let’s say, 1 second?

Thank you,
Alex

Comment by William Premerlani on July 2, 2010 at 7:12am

Hi Alex,

Keep in mind that the values of Kp and Ki that I gave you are floating point values. If you are using integer arithmetic, you will have to convert them into whatever base you are using.

I am glad to hear you say that you are going to use a magnetometer, that is a great idea with a heli-type application.

Regarding the time constant tau, I do not recommend reducing it to 1 second, it will cause too much of the transient errors of the accelerometers to pass through into the controls. 5 seconds is about the shortest that would recommend. But of course you can try different values and see what you like.

Best regards,
Bill

Comment by Jared Rought on July 2, 2010 at 10:07am

I have a couple questions regarding the formation of the DCM.

1) How is the Rmatrix initially formed? I have looked through several implementations that use the accelerometers to acquire the initial heading. Is this the approach that is used?

2) I am a little confused as to what role the integration plays in forming Rmatrix. To update the Rmatrix you are just crossing the Rmatrix with the antisymmetric matrix {1, -omegaZ*t, omegaY*t; omegaZ*t, 1, -omegaX*t; -omegaY*t, omegaX*t,1} , correct? If so , where does the integration fit in?

Thank you in advance for any help that anyone offers

Jared

Comment by William Premerlani on July 2, 2010 at 6:11pm

Hi Jared,

1. We assume that the plane is level and pointing to the north at power up. So the initial value of the Rmatrix is
{ 1 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 1 } If you look at the code, you will see an initializer with this value. If this assumption is wrong, it does not matter, because the roll-pitch-yaw drift compensation will "lock" onto the accelerometers and yaw information (GPS or magnetometer). The accelerometer and magnetometer lock occur on the ground. The GPS lock occurs after takeoff.

2. The integration is done by combining two matrices into one matrix. The actual integration equation with the two matrices is:

RmatrixNew = RmatrixOld + RmatrixOld* {0, -omegaZ*t, omegaY*t; omegaZ*t, 0, -omegaX*t; -omegaY*t, omegaX*t,0}

The second term is the integration, it adds the change to the old value to get the new value. However, I decided to put the diagonal elements to work to eliminate the separate addition step, and to let the matrix multiplication routine do it for me. MicroChip provides an efficient matrix multiplication routine. So, it is convenient to combine the two terms in the above equation:

RmatrixNew=RmatrixOld* {1, -omegaZ*t, omegaY*t; omegaZ*t, 1, -omegaX*t; -omegaY*t, omegaX*t,1}

Best regards,
Bill

Comment

• ‹ Previous
• 1
• …
• 4
• 5
• 6
• 7
• 8
• Next ›
• Page