T3

### robust estimator of the direction cosine matrix

#### Replies

• Thanks exactly what I did. Just wanted to make sure I did it correctly b/c this is all new to me. I'm a software engineer and i'm trying to learn how to build simulations so i'm taking an online course where nothing is explained. This is a huge learning curve for me. Again thank you so much. All of your help is greatly appreciated.
• how do i convert acceleration and velocity from body frame to ECEF frame with gravity?

given: long, lat, alt, bearing angle, time, and body attitude
• Hi Bill,

With yours and Louis' previous help in posts, I now have the a matrixnav DCM ported to my ARM7. It seems to work, so long as I don't set any KPPitchRoll and KPitchRoll gains. I now have the gyros giving a good approximation of the change in attitude in roll and pitch (I am not dealing with yaw to start). The problem is that the DCM does not return to position close to that prior to any rotations. For example, If I place the gyros and accel on desk at 0 deg, then move 45 deg pitch angle, hold a second, then return to 0 deg on desk and let go, the DCM gives me approx 45 deg (so my gyro gain should be close), but then returns to something like 10 or 15 degrees pitch when it should be closer to 0 deg pitch.

When I then try and set some KP and KI gains, what happens is that the PID adjustment then has a runaway... as I can't see any limit set to the KI gains as one would normally have in a PID algorithm. I was thinking the the accels would be used to counter gyro drift and also assist in the correction of gyro errors accumulated.

The gplane variable is set to be [0 0 1] (x=0, y=0, z=1) to start, then updated with accel readings (adc units scaled g's, so should be 0 0 1 with unit standing still) to which are adjusted by the zero offset calibration on startup. As rmat is normalized prior to the roll_pitch_drift function, I normalize my gplane vector as well, before I do the crossvector of gplane and rmat.

Does the gplane variable related to x y z accel? which then correspond to same x y z gyros? Is errorRP vector also error in x y z?
I don't get why it should run away when I set PID's...

I continue to reread the technical papers on this algorithm... I hope it will sink in soon...

Thanks
Sean
• Hi,

Thank you for the interesting algorithm, and for all the detailed information given in the comments !

I think the compensation of the centrifugal force is very promising, and I wanted to understand a little bit better how you implemented it with the GPS velocity and to get your thoughts.

In particular, how is the GPS velocity used to compensate the centrifugal force ? My impression (is it correct ?) after very quickly looking at the code, is that the velocity is simply assumed to be along the x body axis.

Do you think there could be an algorithm to obtain a better estimate of the velocity vector ? (For example, in Mahony's paper there is an angle-of-attack model, etc...). Because the better the velocity vector is approximated, the better the centrifugal force compensation is.

For example, if the GPS velocity vector is used (as opposed to only the norm), it can be transformed to the body frame if the yaw is known (I assume the yaw can be inferred from the GPS heading). But is the GPS velocity vector reactive enough during turns, I don't know.
A problem also, is probably in case of side wind : in this case the yaw is not really the GPS heading anymore, and it is probably harder to get right (I guess magnetometers are needed here - same as when the estimator is used on a helicopter). So in case of wind, the GPS speed cannot be transformed to the body frame accurately I suppose.

Best,

• I need help building a transformation matrix from NED to ENU. I have to multiply roll * pitch * yaw. I've been trying to figure this out but this isn't my area of expertise. Can someone please assist me with this problem?

Michelle
• Hi Jose, I am interested in taking a look at your Matlab files.

UFO-MAN
• I am trying to understand this attitude estimation method,
i am several days with a problem and i would like to consult.

I have implemented your DCM update in MATLAB.

omegacorr = omegacorrPAcc + omegacorrIAcc + omegacorrPMag + omegacorrIMag;
omegatotal = gyro1 + omegacorr;
theta = omegatotal;
rup = [ rmax -theta(3) theta(2)
theta(3) rmax -theta(1)
-theta(2) theta(1) rmax];
rmat = (rmat*rup);

I am making a comparison with euler integration:

i_roll = i_roll + gyro(1);
i_pitch = i_pitch + gyro(2);
i_yaw = i_yaw + gyro(3);

there are one first stuff than i can not understand
i am scaling gyro measurement (in rad/sec) * 0.00075!!!!

result seems to be correct

but this "integrated" DCM is very strange, it is only correct to North direction!!!
when i turn to south then roll value is exacly inverse values!

Any idea?
• Bill/Louis,
The info on the matrix elements is great. I now have it responding to my inputs once I adjusted the gyro gain etc. Now I need to work on the p and i gains for pitch & roll. Without compensation for pitch roll by accel, the euler angles representation by phi the and psi drifts as expected.
If the elements are used for demo and control without conversion to quarterions or Euler angles, how would I be able to correct a magnetometer for pitch and roll? I see Louis has mag being used so I must read the paper again ... Hopefully it sinks in and becomes more evident to me. Thx all info to date. Seano
• Thanks Louis,
I have the problem of the jumping from -pi to +pi
I will think this through as I continue my debugging of the code.
Thx
Sean
• Simon, the ADIS16405 looks like a good device. But.... have you checked the price? Take a look here: http://search.digikey.com/scripts/DkSearch/dksus.dll?Detail&nam...
Digikey lists it for USD 707,- which I think is very expensive.
Datasheet here by the way: http://www.analog.com/static/imported-files/data_sheets/ADIS16405.pdf

UFO-MAN

### Activity

How to use the new @donkey_car graphical UI to edit driving data for better training https://www.youtube.com/watch?v=J5-zHNeNebQ
yesterday
RT @SmallpixelCar: Wrote a program to find the light positions at @circuitlaunch. Here is the hypothesis of the light locations updating ba…
Saturday
RT @SmallpixelCar: Broke my @HokuyoUsa Lidar today. Luckily the non-cone localization, based on @a1k0n LightSLAM idea, works. It will help…
Thursday
@gclue_akira CC @NVIDIAEmbedded
Nov 23
RT @luxonis: OAK-D PoE Autonomous Vehicle (Courtesy of zonyl in our Discord: https://discord.gg/EPsZHkg9Nx) https://t.co/PNDewvJdrb
Nov 23
RT @f1tenth: It is getting dark and rainy on the F1TENTH racetrack in the @LGSVLSimulator. Testing out the new flood lights for the racetra…
Nov 23
RT @JoeSpeeds: Live Now! Alex of @IndyAChallenge winning @TU_Muenchen team talking about their racing strategy and open source @OpenRobotic…
Nov 20
RT @DAVGtech: Live NOW! Alexander Wischnewski of Indy Autonomous Challenge winning TUM team talking racing @diyrobocars @Heavy02011 @Ottawa…
Nov 20
Incredible training performance with Donkeycar https://www.youtube.com/watch?v=9yy7ASttw04
Nov 9
RT @JoeSpeeds: Sat Nov 6 Virtual DonkeyCar (and other cars, too) Race. So bring any car? @diyrobocars @IndyAChallenge https://t.co/nZQTff5…
Oct 31
RT @JoeSpeeds: @chr1sa awesomely scary to see in person as our \$1M robot almost clipped the walls as it spun at 140mph. But it was also awe…
Oct 29
RT @chr1sa: Hey, @a1k0n's amazing "localize by the ceiling lights" @diyrobocars made @hackaday! It's consistently been the fastest in our…
Oct 25