6
\$\begingroup\$

I am trying to correctly track orientation of a sensor with accelerometer, gyroscope, and magnometer, using the open source Madgwick algorithm. The algorithm returns quaternions which represent the rotation between the internal and external frames. However, when using this code the quaternions returned seem to be suffering from drift and even after digging through multiple times I can not find what may be the cause. On the top is what is returned, and the bottom is the approximately correct values (from an experiment with a different sampling frequency): Quaternions. Could this be caused by too low of a sampling frequency? The internal Madgwick report shows a lower bound of 10hz, and this sample was taken at 5hz. However it is not clear to me how that issue would produce these results, but that is currently my best guess. I think the issue could also be the linear acceleration from gravity, but my understanding is that gravity's influence can be removed once the quaternions have been calculated - so if that is indeed the issue how can I remove linear acceleration to calculate them in the first place?

Edit: The accelerometer and gyro values are not suffering from drift, and this effect only emerges with the calculated quaternions.

\$\endgroup\$
  • \$\begingroup\$ The drift is caused by sensors as a fundamental error, and you can do virtually nothing with it. \$\endgroup\$ – Eugene Sh. Aug 18 '15 at 20:58
  • \$\begingroup\$ The accelerometer and gyroscope data themselves are not suffering from drift, and look accurate. Only the calculated quaternions show this effect. \$\endgroup\$ – user2605159 Aug 18 '15 at 21:00
  • \$\begingroup\$ Do you lowpass your input signals? \$\endgroup\$ – mng Aug 19 '15 at 0:20
  • \$\begingroup\$ @mng yes, the quaternion arrays are proportionally smoothed but none of the drift has been corrected \$\endgroup\$ – user2605159 Aug 19 '15 at 2:50
  • \$\begingroup\$ i am having some issues in trying to implement the same algorithm in a microcontroller. Maybe we can share ideas, i believe the drift thing is because the sampling rate, i mean how often your code performs. There's a graph in Sebastian Madgwick report in which he compares the drift against the sampling rate (It is in page 23). Apparently your sampling rate is to low. I'd like to share some ideas with you \$\endgroup\$ – Axel Gonzalez Nov 3 '17 at 17:25
2
\$\begingroup\$

The problem is basic physics of how the accellerometers and gyros work, and the fact that these signals need to be integrated to get position.

No accellerometer or electronic gyro puts out exactly 0 with no accelleration or circular motion. These sensors may have little drift (sometimes), but do have offset. The drift comes from integrating the offset.

Gyro data only requires one level of integration, and can be partially corrected by sensing the gravity vector and the magnetic vector. However, this doesn't cover all degrees of freedom.

Accellerometer data requires double integration to get position, and there is no reference to correct accumulating errors against.

Basically, cheap MEMS accellerometers and gyros aren't much good past a few seconds from a know position and orientation, and there is nothing you can do about it given the acc and gyro data alone.

\$\endgroup\$
  • \$\begingroup\$ How do examples of accurate orientation tracking like the original link video and others work? Are you saying a 5hz sampling rate sensor is too "cheap"? The sensors themselves are noisy but the Madgwick algorithm does use sensor fusion to correct for drift in the individual measurement instruments. Put simply: how are these video examples of accurate training being created if "there is nothing you can do about it given the acc and gyro data alone"? \$\endgroup\$ – user2605159 Aug 18 '15 at 22:44
  • 1
    \$\begingroup\$ @user: Not gonna watch videos. I've done some inertial navigation with MEMS sensors in the microcontroller, and 5 Hz sounds quite slow to me. However, that seems to have nothing to do with the drift you are seeing. I was tracking head motion during a golf swing, and after a few seconds the integrated data was pretty much useless. Fortunately the data was good enough for the duration of a swing. You can't get around the 0 offset and the resulting drift from its integration. \$\endgroup\$ – Olin Lathrop Aug 18 '15 at 22:54
  • \$\begingroup\$ I appreciate your advice. However the videos show a sensor being rotated arbitrarily and tracked correctly for minutes in real time using the exact code I've linked to. I'm aware of the issues of compounding noise due to integration, but it seems to have been overcome using this algorithm. Positional tracking has too-large errors but I believe those who have solved for orientation using these 3 sensors (as all smartphones have) successfully use the magnometer and the gravity vector as calibration. \$\endgroup\$ – user2605159 Aug 18 '15 at 22:59
  • \$\begingroup\$ I agree with Olin on this one. Go and ask on the robotics stack exchange robotics.stackexchange.com and you'll get very similar answers. \$\endgroup\$ – KyranF Aug 18 '15 at 23:40
  • 1
    \$\begingroup\$ I have posted to the robotics community here. I do not understand the implication that this is impossible when there are many examples of it working as I intend mine to work, with the same raw MEMS imput. \$\endgroup\$ – user2605159 Aug 19 '15 at 0:02
2
\$\begingroup\$

@Olin Lathrop's answer is wrong. Unfortunately, I don't have the reputation to either downvote or comment on his answer. It's curious that he chose to claim that what is being achieved in the paper you link isn't physically possible...

As madgwick's paper demonstrates it is indeed possible to maintain stable orientation estimates over long durations using only a 9 DOF MEMS MARG. His algorithm was also benchmarked on a bunch of different hardware by Kris Winer. He, in particular, demonstrates virtually drift-free tracking over a half hour period with both time periods of lying still and being moved around. The relative error of the absolute orientation is less than a degree over this time period. Impressively this is achieved without resorting to a full Kalman filter and requires only scalar operations instead of needing the computationally expensive matrix inversion required by a Kalman filter.

Looking at it through the lens of observability the problem of orientation estimation is a problem with six coupled states. 3 states for the orientation itself and 3 states for the derivatives of the orientation (ie the angular rates)

Obviously, the gyroscopes allow for direct measurement of the latter three states. This leaves the issue of the three orientation states. Assuming that there is a static gravitational field and magnetic field that are not aligned, we can fix two of the three directions. The structure of the coordinate system imposes constraints on the relationship between the three directions allowing the third to be fixed simply by construction. As such the system is observable, which is to say no, unlike @Olin Lathrop's claim, the "physics" explicitly allows you to determine the complete orientation even without depending on any integration. As such his objection regarding integration error is completely unfounded.

With that being said integration is still a useful process as it allows sensor fusion to be performed which is to say that you are essentially performing a best fit between the implied angular rate incremental positions and the incremental directly measured absolute orientations. Seeing as these are likely not correlated in that they are measuring three completely separate physical phenomena they allow you to significantly improve the accuracy and robustness of your orientation estimation.

This brings us to the issue you raised regarding the origin of the lower frequency bound on the tracking accuracy. Not being the author of the algorithm and only ever having used it as a component in a bigger solution where its performance was adequate I haven't personally dug through every line of every equation in the paper but that being said the temporal nature of the problem does indeed point to something related to timesteps.

The numerical integration itself might contribute but is likely not the main driver as its performance should be reasonably robust against larger timesteps. I would look at the single step gradient descent (second paragraph on page 7 of the linked paper) that is being used as the optimization algorithm to fit the sensor observations to the assumed fields. At higher frequencies, this simplification is likely sufficient as there isn't much time for the two to diverge and you can arrive at a reasonable estimate after only one iteration. As you slow down the algorithm this likely isn't the case and you would need to iterate until you get a reasonable error term.

In summary, you can definitely track orientation using a 9DOF MEMS sensor. I'm not clear why you are attempting to run this algorithm as slowly as you are but there are some spots in the algorithm to check if you want to improve the lower bound but be aware that they will come with significant overhead which means that if the reason you are having this issue is because it is the best your hardware is able to achieve (maybe due to other background tasks) then I would recommend offloading background tasks or upgrading hardware to improve your rate rather than trying to tweak the algorithm. Generally speaking, AHRS and IMU systems should be run at the highest rate you possibly can on dedicated hardware as its performance tends to have fairly critical implications for the rest of your system.

\$\endgroup\$
  • \$\begingroup\$ Welcome to EE.SE! Note that it is typically good etiquette here to provide your answer as a stand-alone. The way that you have phrased your post invites discussion, which is not the intent of SE. \$\endgroup\$ – Daniel Nov 1 '18 at 19:32

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.