I am working on interfacing a pair of LSM9DS0 IMUs with some Teensy code to reconstruct the orientation of two connected rigid bodies (a user's forearm and palm, in this case). The previous thread here, "Quaternian[sic] is changing very slowly" was invaluable in getting my setup running.
My problem: the quaternions update very slowly, no matter how high I set the beta parameter in Madgwick's algorihm. Also, I get acceptable results only if I invert the magnetometer's Z value before passing it into Madgwick. I've never seen this mentioned anywhere else online in the context of LSM9DS0 + Madgwick, so I'm very confused and wondering what I may have done wrong!
My setup: two LSM9DS0s on Adafruit breakout boards daisy-chained via SPI. My code uses the Sparkfun LSM9DS0 driver in SPI mode along with a slightly modified version of Nicholas Robinson's LSM9DS0 AHRS. My modified version incorporates onehorse's uint32_t fix, and allows the developer to pass in a SFE_LSM9DS0 object to the constructor, since I need to run two instances at once (one per IMU). I am polling at a regular interval (~10ms) rather than using interrupt-driven processing, to reduce the number of wires between the devices. This may be a mistake and I'm willing to reconsider if it ends up being part of my problem.
I started out using onehorse's Madgwick-for-teensy implementation, but realized that my environment had a high level of soft iron distortion (metallic objects all around) so ended up shifting my Madgwick to the PC so that I could do all the soft iron correction ellipsoid fitting work there, before integrating with Madgwick. I'm using x-io's C# Madgwick implementation. Again, if there's a compelling reason to eg: do the Madgwick on the Teensy and send the center offset and calibration matrix to the Teensy at startup, I'm open to that, but I don't think it's my problem? Regardless, I'm using usb serial to ship all values (accelerometer, gyro and mag xyz) to the PC using a binary protocol, sending everything as 32-bit floats.
Even ignoring mag calibration artifacts (and testing in the middle of a large room with ferrous objects pushed to the walls), I was seeing some really weird things!
Whenever I pitched or rolled the IMUs across the vertical (ie: from positive to negative pitch, or vice-versa), the reported yaw of the quaternion generated by Madgwick would flip by 180! I looked at the LSM9DS0 datasheet (pg 11 / Sec. 1.2) and realized that the magnetometer Z is flipped relative to the accelerometer output. I tried inverting the Z axis before passing it into Madgwick and this weird flip-flopping behavior was corrected! Okay, awesome, but why has nobody else in the history of the Internet had this problem? And what other weirdness is it hiding?
I'm sampling at ~100Hz (teensy is reporting ~0.0102 sec between samples right now), and I have tried adjusting my Madgwick gyro measurement error everywhere from 180 deg/s to 0.1 deg/s (beta = sqrt(4f/3f) * PI * (error / 180f); ) -- low beta results in VERY slow quaternion update; high beta somewhat faster but eventually noisy.
Any idea what I should be looking for here? And how might my inverted Z be related to everything else?
I can post video of my system, and selected source code; will just take a minute to capture/process/upload..
My problem: the quaternions update very slowly, no matter how high I set the beta parameter in Madgwick's algorihm. Also, I get acceptable results only if I invert the magnetometer's Z value before passing it into Madgwick. I've never seen this mentioned anywhere else online in the context of LSM9DS0 + Madgwick, so I'm very confused and wondering what I may have done wrong!
My setup: two LSM9DS0s on Adafruit breakout boards daisy-chained via SPI. My code uses the Sparkfun LSM9DS0 driver in SPI mode along with a slightly modified version of Nicholas Robinson's LSM9DS0 AHRS. My modified version incorporates onehorse's uint32_t fix, and allows the developer to pass in a SFE_LSM9DS0 object to the constructor, since I need to run two instances at once (one per IMU). I am polling at a regular interval (~10ms) rather than using interrupt-driven processing, to reduce the number of wires between the devices. This may be a mistake and I'm willing to reconsider if it ends up being part of my problem.
I started out using onehorse's Madgwick-for-teensy implementation, but realized that my environment had a high level of soft iron distortion (metallic objects all around) so ended up shifting my Madgwick to the PC so that I could do all the soft iron correction ellipsoid fitting work there, before integrating with Madgwick. I'm using x-io's C# Madgwick implementation. Again, if there's a compelling reason to eg: do the Madgwick on the Teensy and send the center offset and calibration matrix to the Teensy at startup, I'm open to that, but I don't think it's my problem? Regardless, I'm using usb serial to ship all values (accelerometer, gyro and mag xyz) to the PC using a binary protocol, sending everything as 32-bit floats.
Even ignoring mag calibration artifacts (and testing in the middle of a large room with ferrous objects pushed to the walls), I was seeing some really weird things!
Whenever I pitched or rolled the IMUs across the vertical (ie: from positive to negative pitch, or vice-versa), the reported yaw of the quaternion generated by Madgwick would flip by 180! I looked at the LSM9DS0 datasheet (pg 11 / Sec. 1.2) and realized that the magnetometer Z is flipped relative to the accelerometer output. I tried inverting the Z axis before passing it into Madgwick and this weird flip-flopping behavior was corrected! Okay, awesome, but why has nobody else in the history of the Internet had this problem? And what other weirdness is it hiding?
I'm sampling at ~100Hz (teensy is reporting ~0.0102 sec between samples right now), and I have tried adjusting my Madgwick gyro measurement error everywhere from 180 deg/s to 0.1 deg/s (beta = sqrt(4f/3f) * PI * (error / 180f); ) -- low beta results in VERY slow quaternion update; high beta somewhat faster but eventually noisy.
Any idea what I should be looking for here? And how might my inverted Z be related to everything else?
I can post video of my system, and selected source code; will just take a minute to capture/process/upload..
Last edited: