问题
I'm trying to implement Madgwick sensor fusion algorithm from here on LSM9DS0 sensor (accelerometer, gyroscope and magnetometer) on STM Cortex M3 microcontroller.
Raw data from all sensors seems to be fine.
My problem is: when I hold sensor with it's z-axis horizontal or downwards (i.e. roll or pitch angle is more than 90 degrees) - quaternion from filter becomes really unstable and randomly flips 180 degrees. More correctly, q0 and q3 are constantly changing signs, resulting in 180 degree flip of rotation.
I tried using constant values instead of real sensor output and still got this behavior.
When z-axis is more or less vertical, quaternion seems plausible.
I haven't seen anything like this on example videos.
I tried to ignore magnetometer data and use 6DOF version of filter but it was a disaster; quaternion is just flying around and spinning uncontrollibly.
I have to set beta-parameter quite large (~100) because lesser values have given very shaky result with sudden flips. I do find this strange since usually beta is about 0.5-0.05.
Filter update frequency is 1KHz.
Can anyone please help me?
回答1:
Here is said that there is a mistake in code of madgwick filter!
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 that code on official site is outdated and soon will be replaced.
Correcting this produced a satisfying result.
My other mistake was not reading function prototype properly. I copied slightly changed version of the code, where accelerometer and gyroscope values were interchanged.
来源：https://stackoverflow.com/questions/25902186/madgwick-sensor-fusion-on-lsm9ds0