Instabilities with yaw/pitch/roll values while device is stable

[tshaddix]

Hello, all! I've been trying to implement this filter for the last few days with troublesome results. I know this filter works, given the work by @PaulStoffregen and others who show it working well.

I have convinced myself that there must be something wrong with the scale and/or coordinate system input I'm giving the filter, as the readings from my IMU seem to be correct and stable. If this turns out to be the case, I'll be happy to put in the work to add comments or wiki pages describing the fix.

Background Info

• IMU: Sparkfun 9DOF LSM9DS1 (https://www.sparkfun.com/products/13284)
• Algorithm: AHRS (update() call)

Here is a printout of the raw readings from my IMU given in x, y, z while sitting on a table:

A: 0.02,-0.02,0.03
G: -0.02,0.03,0.07
M: -0.01,0.04,-0.36
A: 0.02,-0.02,0.02
G: 0.19,0.20,-0.07
M: -0.02,0.04,-0.37
A: 0.03,-0.02,0.01
G: 0.03,0.20,-0.10
M: -0.02,0.04,-0.36
A: 0.02,-0.02,0.03
G: -0.06,0.27,-0.03
M: -0.02,0.04,-0.36
A: 0.01,-0.01,0.03
G: 0.01,0.34,0.18

And here are the values from the filter (the code looks very much like the example included in this repo):

219.,-31.51
219.74,-36.38,-30.93
219.58,-36.56,-30.66
219.71,-36.74,-30.88
219.89,-36.54,-31.17
180.00,-0.36,-0.28
179.99,-0.75,-0.53
180.00,-1.11,-0.80
180.00,-1.42,-1.13
180.01,-1.80,-1.37
180.03,-2.17,-1.64
180.04,-2.54,-1.90
180.05,-2.88,-2.20
180.06,-3.25,-2.46
180.08,-3.62,-2.73
180.09,-4.01,-2.96
180.11,-4.35,-3.25
180.14,-4.69,-3.54
180.16,-5.08,-3.78
180.18,-5.50,-3.96
180.20,-5.89,-4.19
180.22,-6.26,-4.46
180.27,-6.58,-4.76
180.27,-7.03,-4.75
180.31,-7.41,-5.00
180.36,-7.78,-5.27
180.40,-8.13,-5.55
180.46,-8.53,-5.76
180.50,-8.91,-5.98
180.57,-9.26,-6.29
180.63,-9.62,-6.55
180.69,-9.96,-6.83
180.74,-10.33,-7.07
180.78,-10.71,-7.30
180.79,-11.15,-7.26
180.86,-11.49,-7.55
180.94,-11.83,-7.84
180.99,-12.22,-8.04
181.06,-12.53,-8.35
181.12,-12.92,-8.56
181.21,-13.29,-8.81
181.26,-13.67,-9.01
181.35,-14.03,-9.27
181.42,-14.42,-9.46
181.51,-14.78,-9.70
181.60,-15.11,-9.98
181.69,-15.49,-10.21
181.78,-15.86,-10.43
181.88,-16.22,-10.68
181.98,-16.55,-10.95
182.07,-16.93,-11.16
182.17,-17.28,-11.41
182.27,-17.64,-11.63
182.37,-18.02,-11.84
182.50,-18.38,-12.08
182.61,-18.75,-12.31
182.76,-19.05,-12.63
182.89,-19.38,-12.90
183.01,-19.72,-13.16
183.15,-20.03,-13.45
183.25,-20.41,-13.61

# ... A linear growth continues

213.06,-37.41,-30.32
213.31,-37.53,-30.66
213.55,-37.66,-30.96
213.67,-37.58,-30.89
213.66,-37.86,-30.77
213.94,-37.53,-31.05
214.24,-37.47,-31.44
214.44,-37.11,-31.58
214.21,-37.18,-31.03
214.02,-37.39,-30.57
214.19,-36.98,-30.69
214.45,-36.87,-30.92
214.65,-37.01,-31.14
214.73,-37.23,-31.13
214.81,-37.05,-31.04
214.98,-37.12,-31.13
215.06,-37.33,-31.13
215.27,-37.43,-31.33
215.51,-37.56,-31.62
215.62,-37.67,-31.67
215.80,-37.68,-31.78
215.95,-37.77,-31.90
216.23,-37.47,-32.29
216.46,-37.55,-32.57
216.67,-37.68,-32.85
216.70,-37.78,-32.66
216.51,-37.51,-32.22
216.60,-37.71,-32.21
216.46,-37.47,-31.79
216.71,-37.56,-32.10
216.99,-37.50,-32.44
217.14,-37.17,-32.50
216.90,-37.29,-32.00
216.61,-37.27,-31.43
216.67,-37.43,-31.40
216.79,-37.21,-31.46
216.81,-37.46,-31.42
216.85,-37.32,-31.30
217.02,-37.34,-31.46
217.18,-37.27,-31.59
217.31,-37.39,-31.72
217.15,-37.22,-31.30
216.93,-36.94,-30.86
216.97,-36.48,-30.93
217.25,-36.57,-31.36

As mentioned, this is with the IMU laying flat on the table.

Potential Problem Areas

This filter is pretty straight-forward in implementation, so I can really only see two areas where things can be going wrong: The coordinate system and the scaling.

The coordinate system for my IMU is a bit wonky, so I normalized it to a right-handed coordinate system. Here is the coordinate system out of the box:

(Note the polarity marker is rotated for the mag diagram)

Here is my normalization and scaling code (and the filter call):

imu.readGyro();
imu.readAccel();
imu.readMag();

float gx, gy, gz, ax, ay, az, mx, my, mz;

// Use a right-handed coordinate system

gx = imu.calcGyro(imu.gy);
gy = imu.calcGyro(imu.gx);
gz = imu.calcGyro(imu.gz);

ax = imu.calcAccel(imu.ay);
ay = imu.calcAccel(imu.ax);
az = imu.calcAccel(imu.az);

mx = imu.calcMag(imu.my);
my = -imu.calcMag(imu.mx);
mz = imu.calcMag(imu.mz);

// update the filter, which computes orientation
filter.update(gx, gy, gz, ax, ay, az, mx, my, mz);

// print the heading, pitch and roll
roll = filter.getRoll();
pitch = filter.getPitch();
yaw = filter.getYaw();

// Print values here

The calcX calls are part of the IMU library, described in short:

calcAccel(), calcGyro(), and calcMag()

The library keeps track of each sensor’s scale, and it implements these helper functions to make translating between the raw ADC readings of the sensor to actual units easy.

calcAccel(), calcGyro(), and calcMag() all take a single parameter – a signed 16-bit integer – and convert to their respective units. They all return a float value, which you can do with as you please.

I tried to keep this short, and I appreciate any time anyone can spend on it. I'm also including the full sketch for context if needed.

[lixuwork]

I am working on this now. I get a problem that the data of any axis being perpendicular to the ground keep rotating. Have ever encountered this problem?

[tshaddix]

Hi, @lixuwork. Sounds very similar to my issue.

[PaulStoffregen]

Gimbal Lock?

[tshaddix]

Hi @PaulStoffregen, I appreciate the help.

I just re-read over gimbal lock to make sure I'm familiar with the general concepts. Would this problem continue to persist over different trials and tests?

I've plotted out the raw data from each of the sensors and they all seem to be working as expected in terms of scale and orientation. Rotating/accelerating about each axis produces expected results from the sensor data.

[PaulStoffregen]

Sadly, I can't look at this right now. Crazy busy with another urgent (and very late) project.

But at first glance, it sounds like gimbal lock, so maybe just a pointer to that well-known limitation of roll-pitch-yaw will help? That's all I can do at this moment.

[tshaddix]

@PaulStoffregen I appreciate the time you've spent! I'll dive in a bit more. Thanks!

[Neutree]

gyro has error ? Is it calibrated? @ @tshaddix

[marooncn]

Hi, @PaulStoffregen @tshaddix , I just meet the same problem. I use this filter to get the stable quaternion, but the result shows q0 keeps up and q1, q2, and q3 keeps down no matter what value the parameter "delta" is . When I convert the quaternion to euler angles(Raw, pitch, yaw), they keep up just like your data. Do you solve this problem?

[tshaddix]

Hi @marooncn . I ended up just going with an IMU that does all of the sensor fusion for you after I had so much trouble getting things to work out myself. Sorry - wish I could be more helpful!

[HelenXR]

@tshaddix Have you solved your problem yet? I think these code is wrong

mx = imu.calcMag(imu.my);
my = -imu.calcMag(imu.mx);
mz = imu.calcMag(imu.mz);

below is correction

mx = -imu.calcMag(imu.mx);
my = -imu.calcMag(imu.my);
mz = imu.calcMag(imu.mz);

[tshaddix]

Hi @HelenXR , thanks for following up on this. I never did get this working. Did you try this code on the same IMU type? I remember I had swapped those axises because of a mismatched coordinate system with the IMU (mentioned in the "Potential Problem Areas" section of this issue).

If that is the solution for this IMU, then that's wonderful :)