I found this video a few months ago and asked the author for a copy of his paper as soon as it was available.  Just received it today.  Here are links to both.

PDF report with source code:




Intro to report:

This report presents a novel orientation filter applicable to IMUs consisting of tri-axis gyroscopes and accelerometers, and MARG sensor
arrays that also include tri-axis magnetometers. The MARG implementation
incorporates magnetic distortion and gyroscope bias drift compensation.
The filter uses a quaternion representation, allowing accelerometer and
magnetometer data to be used in an analytically derived and optimised
gradient-descent algorithm to compute the direction of the gyroscope
measurement error as a quaternion derivative. The benefits of the filter

(1) computationally inexpensive; requiring 109 (IMU) or 277 (MARG) scalar arithmetic operations each filter update,

(2) effective at low sampling rates;e.g.10Hz,and

(3) contains 1 (IMU) or 2 (MARG) adjustable parameters defined by observable system

Description from YouTube video:

A real-time demonstration of an efficient orientation filter capable of providing an estimate of the sensor arrays orientation
relative to the earth through the fusion of tri-axis gyroscope, tri-axis
accelerometer and tri-axis magnetometer data. Unlike an IMU, the
inclusion of the magnetometer mean that the filter is not subject to any
accumulating errors. The filter also incorporates magnetic distortion
compensation to overcome soft-iron disturbances and gyroscope bias drift
compensation. The algorithm is an alternative to more computationally
expensive Kalman based solutions that are commonly used in this
application. The total computation requirement of this filter is 278
scalar arithmetic operations per sample.

Hardware used in video: Sparkfun 6DOF IMU Razor (ADXL335, LPR530 and LPY530) with gyroscope RC HP filters
removed, Sparkfun HMC5843 breakout board (low ESR cap replacement),
x-io Board with .NET interface library

Views: 24242

Reply to This

Replies to This Discussion

Roy, It sounds like you have a better understanding of this paper than me. I cannot help but feel it could all be explained a little bit better. Either way, it does not present the mathematical elegance of which I am so fond of in Moyhony’s approach. RE: “magnetic...” Very interesting, this is something many authors fail to address. RE: “normlaisation” Again, I am very interested in this; more so if the technique may be applied to the normalisation of the accelerometer and magnetometer measurements and so eliminate the need for all sqrt operations. I am glad that you are in contact with the authors, I would be very grateful if you could keep me updated.
Hi Roy and Seb,

We avoid taking the square root in MatrixPilot's implementation of the DCM algorithm, and in our control algorithms. In most cases, it is not necessary.

In the normalization step of the DCM algorithm, we use a Taylor's expansion and a feedback loop to drive the magnitudes of the rows and columns of the matrix to 1.0. It is computationally simple.

We use CORDIC arithmetic in doing conversions from rectangular coordinates to polar, yielding magnitude and angle in one fell swoop, without needing to do square root.

Total CPU loading for the DCM algorithm, using fixed point, is 2%

Best regards,

I will keep you updated. The authors are very slow to respond. They promised to send me their code, but I haven't seen it yet (one of the authors is getting married, so that may explain some of the delay). You can try emailing them yourself. I just used the address that was in their paper.

- Roy
I think so. If I rephrase...
The proportional gain (beta) may be adjusted in real-time to change your 'confidence' in accel+mag measurements; e.g. during centripetal periods, beta = low so that gyroscopes are 'trusted' more, and during stationary periods, beta = high and rapid convergence achieved.
This would of course be the case for my, Mayhony's and Bachmann's filter. When you throw in the integral gain, the system becomes 2nd order and so no longer inherently stable.
i want to test your algorithm. i have a question about magnetometer.
did you converted the output values into Tesla (others) ?

good work and good luck for your thesis ;)

The units for both accelerometer and magnetometer do not matter. Both sensor measurements are normalised and compared against a normalised reference direction. Obviously they must be calibrated to some units.

btw I’ve just uploaded my implementation of Mayhony's DCM filter incorporating my magnetic distortion compensation.

could give me the parameters of initialisation of HCM5843 (gain register).

i have another question, which method did you used to calibrate gyroscope and magnetometer?

void initHMC5843(void) {
sendI2C(0x3C); // HMC5843 write address
sendI2C(0x00); // HMC5843 Configuration Register A
sendI2C(0b00011000); // 50Hz measurement update
sendI2C(0b00100000); // +/- 1 Ga range
sendI2C(0x00); // continuous-conversion mode.
sendI2C(0x3C); // HMC5843 write address
sendI2C(0x03); // HMC5843 1st output data register

I think you contacted me via email about calibration. I cannot explain the methods here, but as I said in the email, I will try to publish documentation/source-code/video in the next few weeks.

Dear Seb Madgwick,

I'm happy to see your code working on 9DoF-Razor IMU from Sparkfun, but have some problems. I have the following problems.

1.In normalization , when norm =0, there will be "divide by zero" problem. if we place a if statement{ if (norm>0) } at acceleration normalization upto end of the filterupdate code, there is drift( fluctuation) can be seen in graphical 3D object, though the +ve rotations about all axes are ok. 

2. I use the sensors reading code same, as it is provided by Sparkfun. offset reading from gyro's , raw readings from accelerometer, compass. Should there be any changes ?

3. Sensors axis sign {w_x,w_y,w_z, a_x,a_y,a_z,m_x,m_y,m_z}={ 1, -1, -1, 1,1,1, -1, 1, 1} . Board axes are as it is from sparkfun.

Present,I'm working on implementation of good Quaternion based algorithm for my research work since the DCM may be very good for UAV's, but may be a problem for motion tracking in CG & Virtual Reality applications.

( This statement is from book, "Strapdown INS Technology by D.H.Titterton & J.L.Weston )

If someone here would like to have a look at my arduino code for 9DOF-Razor board for possible mistakes, i can email if you provide email ID.



1) The code and algorithm are rather old now and have evolved a great deal since that initial publication.  I intend to publish a latest version but it is currently a low priority.

2) The algorithm needs calibrated measurements.  Gyroscope must be in rad/s, accelerometer and gyroscope can be in any calibrated units; e.g. ‘g’, m/s/s, Gauss, Tesla.


Anyone here suggest a good practical, Quaternion based Orientation algorithm which can give the same performance like DCM algorithm which is on 9DoF-Razor board.
I'm really in need of Quaternions for my work to go forward.
I would like to stick to only one approach(algorithm) to tune it perfectly.
RE: Seb Madgwick,
I Think Mahony Quaternion is good( source download AHRS.zip by Seb Madgwick).
I followed the comments provided in the AHRS.c file regarding sensor data to experiment it on 9DoF-Razor baord,It works, but i still have problems in tuning Kp,Ki. Present i'm using Kp=.02,Ki=.00002
Would you tell me how to avoid the problem when norm=0, I do the following.
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
safe_a[0]= ax; safe_a[1]= ay; safe_a[2]= az;
ax = safe_a[0]; ay =safe_a[1] ; az = safe_a[2] ;
I do the same as above wherever normalization is required. will it be a problem ?
Any help in this regard until this code works on 9DoF-board is appreciated.
Leave Ki as 0 and start with a Kp value of 5.  You will want to reduce your Kp value from this by 10 or even 100 times when tuning.  The lowest value of Kp you can use is dependent on: gyroscope bias calibration errors, and gyroscope sensitivity calibration errors and expected angular dynamics of application (coupled characteristics).

This is an example of how vectors are normalised in the latest code.

/* Normalise accelerometer measurement */
    norm = sqrt(ax*ax + ay*ay + az*az);
    if(norm == 0) {        // handle NaN
    else {
        ax = ax / norm;
        ay = ay / norm;
        az = az / norm;    

Reply to Discussion


© 2018   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service