Touch Sensor Madgwick on LSM9DS0
I am trying to implement Madgwick sensor fusion algorithm from here on an LSM9DS0 sensor (accelerometer, gyroscope and magnetometer) on an STM Cortex M3 microcontroller.
The raw data from all the sensors seems to be fine.
My problem is that when I hold the sensor with the z-axis horizontal or down (i.e. tilt or pitch angle greater than 90 degrees) the quaternion from the filter becomes really unstable and randomly flips 180 degrees. Rather, q0 and q3 are constantly changing signs, which leads to a rotation of the rotation by 180 degrees.
I tried using constant values instead of the actual sensor output and still got this behavior.
When the z-axis is more or less vertical, the quaternion seems plausible.
I have not seen anything like this in the video examples.
I tried to ignore the magnetometer data and use the 6DOF version of the filter, but it was a disaster; the quaternion just flies and spins uncontrollably.
I need to set the beta parameter quite large (~ 100) because lower values gave a very shaky result with sudden flips. I find this odd since the beta is usually around 0.5-0.05.
The filter refresh rate is 1 kHz.
Can anyone help me?
source to share
It says there is a bug in the madgwick filter code!
The gradient decent step should look like this:
s0= -_2q2*(2*(q1q3 - q0q2) - ax) + _2q1*(2*(q0q1 + q2q3) - ay) + -_4bz*q2*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx) + (-_4bx*q3+_4bz*q1)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my) + _4bx*q2*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
s1= _2q3*(2*(q1q3 - q0q2) - ax) + _2q0*(2*(q0q1 + q2q3) - ay) + -4*q1*(2*(0.5 - q1q1 - q2q2) - az) + _4bz*q3*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx) + (_4bx*q2+_4bz*q0)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my) + (_4bx*q3-_8bz*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
s2= -_2q0*(2*(q1q3 - q0q2) - ax) + _2q3*(2*(q0q1 + q2q3) - ay) + (-4*q2)*(2*(0.5 - q1q1 - q2q2) - az) + (-_8bx*q2-_4bz*q0)*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(_4bx*q1+_4bz*q3)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q0-_8bz*q2)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
s3= _2q1*(2*(q1q3 - q0q2) - ax) + _2q2*(2*(q0q1 + q2q3) - ay)+(-_8bx*q3+_4bz*q1)*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(-_4bx*q0+_4bz*q2)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
and this code on the official site is deprecated and will be replaced soon.
The fix was satisfactory.
My other error was not reading the function prototype correctly. I copied a slightly modified version of the code where the accelerometer and gyroscope values were replaced.
source to share