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: 24614

Reply to This

Replies to This Discussion

Any idea how this compares in performance with the DCM, which is the computationally inexpensive algorithm we already use on that hardware? The paper doesn't discuss it.
Chris and Bill,

I am familiar with the author of the paper. While he was finishing up his work, and completing the list of references for his paper, he stumbled across the DCM work, and contacted me. Up until that time, he was not aware of ArduImu, or the work of the UAV DevBoard team or Robert Mahoney.

He and I exchanged a few emails and concluded that his algorithm and DCM used approximately the same amount of CPU loading, and that performance was comparable.

I am not sure what he means when he says "Unlike an IMU, the inclusion of the magnetometer mean that the filter is not subject to any accumulating errors."

The DCM algorithm is not subject to accumulating any errors. You can use acclerometers and GPS or magnetometer to keep it from doing that.

Best regards,
Bill Premerlani
Hi Bill, having quickly looked over the paper, I'm trying to understand the jist of the difference between his approach and standard DCM. Differences in DCM matrix or quaternion aside, in the end, it appears that the rate of change of the vectors is what he is tracking? Is that right, the derivative of the vectors? Does your DCM work not the rate of change but the differences in actual vector directions?
Hi Michael,

I have not looked over the paper, it is on my "list" of things to do. I appreciate the fact that you have looked at it.

Do you think it is recommended reading? Could you tell me in a nutshell what the author's approach is?

What the DCM algorithm does, in a nutshell, is to start by directly integrating the nonlinear differential equations that describe the kinematics of rotation, using only the gyro information and the direction cosine matrix, and then lock onto reference vectors, such as accelerometers, magnetometers, or GPS heading information, with a PI feedback controller, to prevent accumulation of errors.

Hi Bill, here's my best take on it, the filter also computes the orientation by integrated the angular gyro rates whose biases have been nulled out by gravity (accelerometer) and mangetic vectors. It seems to me that the method of getting the vectors to match seem different. Instead of PI feedback it uses a gradient descent algorithm to align the rotation vectors. It seems that one of the apparent unique features of this approach centers around the magnetic sensor. Inclination errors (y) of the mag sensor are nulled out by using the magnetic estimated orientation and normalized earth x and z readings (I think I got that right) In any event magnetic errors are supposed to influence heading only.

I don't know if it is recommended reading...but for you..I believe you can distill it to mere mortal speak better than anyone!
I am the author of the paper and thought I could clear a few things up.

RE: "I am not sure what he means when he says "Unlike an IMU..."
I mean this literally: an IMU (Inertial Measurement Unit) consists of only inertial sensors and so may only estimate an orientation according to gravity and angular rate; therefore the 3D orientation (considered as a single state) will be subject to an accumulating error. If you wish to consider the an orientation as decoupled Euler angles then you could more accurately say that only the yaw is subject to the accumulating error. If you add a magnetometer, GPS, or anything other than an ‘inertial’ sensor then it is no longer an IMU; it is an IMU + something.

RE: “Any idea how this compares in performance with the DCM”
I had done as many tests as I could to try and show that my algorithm performs better than Mahoney’s or vice versa. I could not. I can only conclude that performance is equal. This is more obvious once one considers how the algorithms actually work (next point).

RE: “difference between his approach”
Mayhony’s [quaternion version], Bachman’s, and my algorithm are the 3 most efficient I know of, and all work in fundamentally the same. They are essentially a complementary (A.K.A. composite) filter. The difference is in the calculation of the feedback error. Bachman’s computes an error using a Gauss-Newton approach, I compute an error using a Normalised Gradient decent approach, and Mayhoy computes an error using a simple dot product. As a result, all three algorithms perform almost identically but each is computationally less expensive respectively. There are other subtle differences that may be of use to certain applications or implementations but I will not discuss them here.

My paper does present what I believe to be a notable unique feature (as identified by Michael Zaffuto): the computation of a reference direction of magnetic flux. This (1) eliminates the need for the designer to predefine this, and (2) prevents magnetic distortions (soft-iron errors) from corrupting pitch or roll.

Mayhony’s algorithm is computationally less expensive and more ‘elegant’. In my own work, I now use Mayhony’s for these reason but I combine it with my ‘magnetic distortion compensation’ algorithm for the reasons given above.
Hi Seb,

A few questions, if you don't mind
1) I assume you're using the Quaternion form of Mahony's algorithm, right?
2) Who is Bachman? Do you have a link or reference to his (her?) algorithm?
3) Did you have a chance to look at the filter from Martin & Salaun I referred you to a few weeks ago?

- Roy
1) Yes, I am only concerned with Mahony’s quaternion version. btw. My report contains (Section 2) what I hope to be a useful overview of quaternions in this application.

2) Just Google "Bachmann quaternion". There are numerous publications about/using his algorithm. The intended application is human motion (as is mine) not UAVs. This paper provides a good summery with block diagram (always useful): http://citeseerx.ist.psu.edu/viewdoc/download?doi=

3) I [briefly] read the Martin & Salaun paper you gave me. I am familiar with the principles they employ but cannot say I completely follow the paper.

Short of a full understanding of their method, it may be more useful to simply know how much more/less efficient it is. Unfortunately they do not state an absolute number of scalar arithmetic operations (though I understand that the method is deterministic). They say that the algorithm requires 2.8 ms per update on an 8-bit ATmega128 using floats and running (we can only assume) at 16 MHz. My implementation of Mahony’s filter on a dsPIC (RE: Bills UAV Board) using floats and the C30 compiler requires 10,000 instructions, which = 625us at 16 MHz (i.e. MIPS); if you add on my magnetic distortion compensation it requires 12,000 instructions, which = 750us at 16 MHz. Obviously the differences due to 8-bit vs. 16-bit and Atmel vs. PIC are dramatic but I thought I may as well provide these numbers anyway and interpret them as you will.

FYI: The total number of scalar arithmetic operations required by Mayhony’s filter are 191 and 149 for with and without my magnetic distortion compensation respectively. These quantities can be reduced further; e.g. use auxiliary variables to prevent repeated calculations.
Hi Seb, as you now have experience with both similar filter methods, are you able to comment on the possible differences in gyro tracking with respect to filter tuning. In your paper, Beta and Zeta could be adjusted in real time and their minimum values (best dynamic performance) easily derivable from gryoscope rate/drift errors. Does the gradient method provide for the possibility of a more optimal trajectory to gryo lock under dynamic conditions? Does it open up the possiblity of more deterministic stability if implementing dynamic gain scheduling in the filters?
>> comment on the possible differences... filter tuning
I have found that, in practise, both filters must be tuned ad hoc. Optimal gains are not just dependant on the system but also the application.

On paper, my filter derivation does present the advantage of allowing filter gains to be defined by observable system characteristics (e.g. a quantified gyroscope measurement error). In practise, I do not believe this is of great benefit. For a real-world system the measurement error is not constant and is the compounded effect of many different sources; e.g. thermal, quantisation, calibration, alignment, axis-coupling, etc.

RE: stability
The normalisation of the feedback error is of great benefit to stability. I have tried very low sample rates and very high gains and am simply unable to make my filter become unstable. However, I am not sure that this is unique characteristic of my algorithm (?).

I do not understand what you mean by “...a more optimal trajectory to gryo lock...” (?).

I don't filly understand what they're doing, either. It seems they have extra computations compared to Mahony, so I expect their algorithm would take longer to execute. They also claim to have a magnetic distortion compensation, so that magnetic anomalies will only affect the yaw axis. Finally, they use a "trick" to enforce the normalization of the quaternion. Instead of dividing it by the magnitude (which involves taking a square root), they feedback the magnitude's deviation from one (which only involves multiplying and adding). The same trick could be applied to any of the algorithms to avoid the relatively expensive square root calculation. (Although I thought the point of "invariance" was that such compensation was not needed? I have been communicating with the authors of that paper (sporadically) via email. Perhaps they can eventually give us further insights to the workings of their algorithm)

- Roy
Hi Seb, The apparent unconditional stability of the feedback and insensitivity to the tuning parameters is what I find attractive. The influence of the intertial vectors on the estimated orientation can be mimimized easily and safely during large accelerations or extended periods of centripetal acceleration. The speed of convergence with applied higher Beta gains following a problematic period provides a potentiallly quicker vector realignment. The ability to easily adjust this 'step size' in the gradient descent iteration is the optimal trajectory of vector realignment along the normalized gradient of the error surface I was referring to.

This general ability to adjust the filter time constant in your filter seems safe. I'm not sure that the PI feedback parameters could be so easily adjusted real time as safely. This was the root of my question I suppose. Can the PI feedback filter be adjusted in real time with same stability your filter exhibits? I believe the PI filter time constant is presently a fixed 15 seconds and limits 'reacquistion' following long duration problematic time periods. I know your filter can be made to require in 5 seconds, with high Beta, and then Beta can be set to a lower value for normal operation.

Am I on the right track here?

Reply to Discussion



Season Two of the Trust Time Trial (T3) Contest 
A list of all T3 contests is here. The current round, the Vertical Horizontal one, is here

© 2020   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service