I've been struggling for some time now on how to correctly calculate the yaw angle from an IMU, but can't get it to work. I'm using the LSM9DS1, if that matters.
I already have proper values for roll and pitch. The value for yaw is also more or less correct until I start tilting the device. Therefore I have to implement some kind of tilt compensation.
I calculate the euler angles as follows:
double weight = 0.05f;
private void calculateEulerAngles() {
// Measured angle by the accelerometer
double rollXLMeasured = Math.atan2(this.accelerometer.getX(), this.accelerometer.getZ()) / 2 / Math.PI * 360;
double pitchXLMeasured = Math.atan2(this.accelerometer.getY() / 9.81f, this.accelerometer.getZ() / 9.81f) / 2 / Math.PI * 360;
// Adding a low pass filter
double rollXLFiltered = (1 - this.weight) * rollXLFilteredOld + this.weight * rollXLMeasured;
double pitchXLFiltered = (1 - this.weight) * pitchXLFilteredOld + this.weight * pitchXLMeasured;
this.rollXLFilteredOld = rollXLFiltered;
this.pitchXLFilteredOld = pitchXLFiltered;
// Calculating deltaTime
long time = System.nanoTime();
int difference = (int) ((time - this.deltaTime) / 1000000000);
this.deltaTime = time;
// Adding a complementary filter
this.roll = ((1 - this.weight) * (this.roll + this.gyroscope.getY() * difference)) + (this.weight * rollXLMeasured);
this.pitch = ((1 - this.weight) * (this.pitch - this.gyroscope.getX() * difference)) + (this.weight * pitchXLMeasured);
// Calculating yaw using the magnetometer and applying a low pass filter
double rollRadians = this.roll / 360 * (2 * Math.PI);
double pitchRadians = this.pitch / 360 * (2 * Math.PI);
double magnetometerXCompensated = (-this.magnetometer.getX() * Math.cos(rollRadians)) - (this.magnetometer.getY() * Math.sin(pitchRadians) *
Math.sin(rollRadians)) + (this.magnetometer.getZ() * Math.cos(pitchRadians) * Math.sin(rollRadians));
double magnetometerYCompensated = (this.magnetometer.getY() * Math.cos(pitchRadians)) + (this.magnetometer.getZ() * Math.sin(pitchRadians));
double yawMeasured = Math.atan2(magnetometerYCompensated, magnetometerXCompensated) / (2 * Math.PI) * 360;
double yawFiltered = (1 - this.weight) * yawFilteredOld + this.weight * yawMeasured;
this.yawFilteredOld = yawFiltered;
this.yaw = yawFiltered;
// Print roll, pitch and yaw for debug purposes
System.out.println(this.roll + ", " + this.pitch + ", " + this.yaw);
}
I don't include the whole code I use, since I think it's clear what the above code does and this is the part thats essential to the problem, I suppose.
And again, I get correct values, just not for yaw when I tilt the device. So there has to be an error concerning the math.
Do I have to do my calculations with the raw values, like the data thats inside the IMU, or use already processed data? For example this.accelerometer.getX() actually returns x_raw * 0.061f / 1000 * 9.81f with x_raw being the value stored inside the IMU and 0.061f being some coeffiecent. I basically copied the calculation from the Adafruit library, I'm not 100% sure why you have to multiply/divide those values though.
Also, you might have noticed that when I calculate the magnetometerXCompensated value I invert the x-axis. I do this, because the magnetometer axis aren't aligned with the acceleromter/gyroscope axis, so in order to align them I have to flip the x-axis.
Does anyone have an idea on how to solve this? I'm really tired of it not working properly, since I tried for quite a while now to solve it, but I'm not getting the results I wanted.
