5

I am trying to correctly track orientation of a sensor with accelerometer and gyroscope using the open source Madgwick algorithm. The algorithm returns quaternions which represent the rotation between the internal and external frames and I use it to determine gravity vector to correct accelerometers.

The algorithm works well when movements are slow and static, but when I try to excite the sensors with fast and periodic movements the quaternions always end up drifting. You can see below the representative figure of this drift when a back and forth movement from top to bottom is performed, the drift is observed on gravity vectors calculated with the quaternions obtained by the fusion algorithm.

(Blue = X axis, Red = Y axis and Yellow = Z axis)

plot of gravity drift

The IMU is a low-cost one : Invensense ICM20948, my sample rate is good (about 300 Hz 100 Hz), I have smooth quaternions, and set parameters algorithm to have good result with slow and static movements.

So my question is : Do you think it's possible to have good result with Madgwick algorithm when this kind of movements are performed? (fast and periodic).

Dave Tweed
  • 168,369
  • 17
  • 228
  • 393
tristan
  • 51
  • 1
  • 3
    It sounds like it's time for you to read up on [coning and sculling errors](http://files.microstrain.com/TN-I0019_3DM-GX3-25__Coning_And_Sculling.pdf), which arise in strapdown inertial navigation systems that are subjected to vibration with a sample rate that isn't fast enough. There are a lot of academic papers on this topic, but I'm having trouble finding a good introductory description. – Dave Tweed Sep 16 '20 at 11:33
  • 2
    Based on a quick scan of [the original paper](http://www.x-io.co.uk/res/doc/madgwick_internal_report.pdf), it doesn't appear that it would be easy to extend Madgwick in this way. You'll probably need to think about moving on to something more sophisticated, like EKF. That's what one of my clients used in his precision GPS/INS system. – Dave Tweed Sep 16 '20 at 11:56
  • I guess, the dynamic response can't be higher than sampling time, also it is not guaranteed that samples are synchronous. – Marko Buršič Sep 16 '20 at 12:16
  • Thanks for your answers! I'm not sure problem is from the too less sampling rate because the frequency of the dynamic response is about 5Hz while the sampling rate is 100 Hz (I said 300 Hz in my first message but it was a mistake). By the way do you have some documentation about EKF ? – tristan Sep 16 '20 at 13:32
  • 1
    Sorry, EKF is short for [Extended Kalman Filter](https://en.wikipedia.org/wiki/Extended_Kalman_filter). It's mentioned in the paper, so I assumed you might be familiar with the abbreviation. – Dave Tweed Sep 16 '20 at 18:09
  • Okay, thank you. Do you think there are open source implementations of EKF for IMU on the internet? – tristan Sep 17 '20 at 09:30

0 Answers0