T3

robust estimator of the direction cosine matrix

Replies

• HI,

I'm using the DCM code as provided with the CK devices "mongoose" as the starting basis for an opens-source wifi AHRS for real aircraft. (That means some of the difficulties of UAVs don't occur: no continuous high-speed rotations for a Cessna 182, or else you've got other problems...)

Comparing the Mayhony algorithm with the Madgwick algorithm they're very similar except in the function for the error vector.

One uses a linear combination of the magnetic and gravitational cross products, and the other uses the first iteration of a steepest descent estimation, based on the Jacobian of an error function yada yada yada.

Is there any significant difference in the performance?

I also think there are two simplifications that can be made in the algorithm:

Don't compute the Z (third) row of the R matrix in Eqn. 17 - just set it to the cross product of the X and Y rows. That removes any requirement ever to renormalize the third row.

• Hello William and everybody else.

I've been reading everything i could find about IMUs. Thanks to a basic Robotics course, I have a decent understanding of reference frames, rotations, concatenations and kinematics and dynamics.

I've been trying to go at it alone, and so far I've been unsuccessful. My algo works, but picks up way too much noise. And that's ok because there is practically no filter.

I've gone directly with quaternions, and I got decent results but sensors are very very noisy.

Now what I couldn't understand in everything I've read is how measures get fused together.

I may be asking a lot, but would you make a scheme of the algorithm? A process chart, just like a matlab simulink worksheet would really help me understand. Trying to follow it from code is cumbersome, although I'm very familiar with C (been programming in C for 10 yrs, PICs for half that).

So far I get the accelerations and the rotations, I calculate the coriolis correction vector as speeds * angular_speeds (integration of speeds is another matter altogether!), and get instant acceleration vector. Then from the angular speed vector I calculate the intermidiate quaternion and operate a quat multiplication between the old quaternion and the intermidiate to obtain the updated one (angles are obviously scaled by sampling_interval). Using basic rotations (planar) I'm able to see it works, but as I said it picks up way too much noise and drift. Even if yaw drift can't be compensated, I expected a better behaviour.

The approach I used is to run some specialized PIDs for about 30s to zero out the Gyros and the ACCs before starting the real algorithm. Then the ACCs' PIDs are stopped as ACCs do not have drift, while the Gyros' PIDs remaing active with very low dynamics and what is essentially an hystereis to follow the slowly moving drift. This appears to work quite well, fact is that each sensor axis has its own quirks: not two ACCs signals have the same FS or center. Let alone Gyros.

What I've been able to discern is that most of the working methods do not involve direct filtering of the signals before it enters the algorithm as I do, but I see citations that you use PIDs somewhere.

What do you use PIDs for?

And how filtering is ultimately obtained?

Can yuo give me a noise figure for your sensor? To estimate if mine are unsuitable for the task, or is it just plain me (which in part surely is).

Thank you

Claudio

P.S. I've received suggestion of filtering the Gyros with an HP filter. I tried but the spikes such filtering gave were huge. Besides I can't exlude a-priori that the device will eventually maintain some attitude different than leveled, in such case the signal filtered by an HP filter would eventually drop to zero thus yeilding wrong results.

P.P.S. will you eventually design a 9dof module? Among the low cost IMUs I found, yours is the one that suits me the most as it includes a very powerful DSP and one I know how to program. I've seen the Sparkfun Ultimate IMU but I haven't been anywhere near an ARM core, so I wouldn't know where to start. And that's a huge con for me right now.

• Hello everyone

I'm new to this hobby, and kind of jumping straight into the deep end, but I have some questions about what a section of your code does. I'm not very fluent in the C family (yet), but I haven't been able to find what the 'union longww' means. And how does it relate to the VariableName.WW?

Thanks!

•

Hello everyone,

Thanks to all contributors for the amazing work.

I'm using the DEV Board (RED old one) in IMU configuration (No GPS/Magnetometer yet) using a fairly new firmware (Mag Demo) in the attempt to stabilize a VTOL in hover mode.

Note that I'm not really in a helicopter setup (I have electrical turbines everywhere so very high frequency vibrations just like in a plane - it's something like a Harrier jet)

I've tried to obtain the absolute acceleration of the model regardless of it's attitude (taking out the gravity component from the accelerometer sensor by simple rmat operations - code bellow) and sending the X and Y axis absolute accelerations to PID controllers similar to the head-locking GY401 hoping that the hover will be as precise as the GY401 tail locking in helis... .

The code for obtaining the absolute accelerations I've used is:

// gravity measured in the frame of reference of the plane
fractional gravity_p_ref[] = { 0 , 0 , 0 } ;

// gravity component in the plane reference
fractional abs_acceleration_p[3] = { 0 , 0 , 0 } ;
...
// get the gravity component in the plane frame of reference:
MatrixMultiply( 3 , 3 , 1 , gravity_p_ref , rmat_transpose , gravity ) ;
// substract the gravity component from the accelerometer readings:
VectorSubtract( 3 , abs_acceleration_p , gplane , gravity_p_ref ) ;
// project the absolute acceleration of the plane frame of reference to the the ground frame of reference:
MatrixMultiply( 3 , 3 , 1 , absolute_acceleration , rmat, abs_acceleration_p) ;

and it seems to be working: I've played with the board and, regardless of the attitude, the absolute_acceleration shows something only if the position of the board changes, not the attitude.

The problem is that the absolute acceleration values should became 0 as soon as the board gets steady but it's not the case... it converges very slowly and aventually never gets to 0  at all. Maybe some errors accumulate, I don't get it.

As for the speed of convergence, it's a killer for the control PIDs, especially in the "head-locking"="position locking" in my case configuration.

The question: is there a way to tune/setup the DCM algorithm for this particular case where tha acelerometers mesures mostly the gravity with only few acceleration spikes (from controlling elements)?

May it also be imperative to use higher quality sensors (thinking of SCA3000 as accelerometer i.e. and lower drift gyros)?

I will also try to rely only on gyro sensors to calculate the attitude and implicitly the gravity component of accelerometers (disable the accelerometer correction) for the short time of hovering but I'm sure that the gyro drift will be annoying.

BTW, I've noticed an even higher convergence time in attitude estimation in EKF based AHRS modules (PNI SpacePoint) ~ tens of seconds :( - I didn't have the "budget" to try the Micro Strain or other commercial ones ... It would be nice to be able to make it with UAV Dev Board :)

Thanks and Regards,

Dragos

• Hi Bill,

I am a mechanical engineering student who is going to design an autopilot system with the UAV DevBoard. After reading your Direction Cosine Matrix documentations I have some questions:

- Do you completely ignore the dynamics of the system in the whole autopilot? So it is not neccesary to make any dynamic model of the airplane?

- Should I use the GPS signal or the Rmatrix to determine the position of the plane and to compare it with a desired trajectory?

Thanks for the support!

Piet
• Thanks to Bill et. al. I am finally contributing, not just withdrawing!

http://www.diydrones.com/forum/topics/propeller-dcm-code?xg_source=...

One hot-off-the-press propeller spin based DCM algorithm coming up! Admittedly it is quite simplistic compared to the matrixpilot, but I hope it will give some people the first step towards something great!

Enjoy and thanks again!
• Hi! I had this under control. But then I stopped thinking about it for a while and no I dont understand any more :( Could any one help my understand way you take the cross product betwene Z in the earth frame of reference and the acc vector to find the error in the body frame of reffrence??
• Dear Bill:
The altitude holding algorithm of MatrixNavRv2 make me very confused. MatrixNavRv2 can do a accurate altitude holding. But I don't know the MarixNav how to know current altitude? The DCM algorithm seems not provide a way to do a altitude measure.
Best Regards
Jack Chen
• hi everyone,

first of all thanks for your excellent share.
i implemented (tried at least) dcm algorithm in C#.
connected my 6 dof IMU to PC via serial port and read raw data.
and tried a wide range of PI gains but i never achieved a good result.
is it because of gains?

one more thing is i confused with accelerometer signs. should down to earth be positive or negative?
thank you all.

best regards.
evren.
• refer to Louis, (1-based indexing)
phi = atan2(-DCM23, DCM33)
theta = atan2(DCM13, sqrt(1-(DCM13)^2))
psi=atan2( -DCM12, DCM11)

and the firmware on ArduIMU_pack,
theta=asin(DCM[3,1])

---
the angle of phi & psi is varied from 0 to 180 then -180 to 0, that is good to be used in labview cube, the cube could rotate 360o

problem is on theta which is varied from -90 to 90, it doesnt represent a 360o rotation in labview's cube, it screw the rotation visualization..

any thought ?

---
another question about Gyro_Gain and Ki Kp value, how to find it's best value ?