 # Magnetometer Tilt Compensation for Yaw Axis Using HMC5883L and BMA180

Hi, i want to make yaw Axis for my 3 Axis Gimbal project, but i really confused to solve the magnetometer tilt compensation. I use HMC5883L magnetometer and BMA 180 Accelerometer. From the reference that i found on the internet, i need magnetometer and accelerometer to solve Magnetometer Tilt Compensation. Anda i found this algorithm from https://code.google.com/p/sf9domahrs/:

CMx = mag_x*cos(pitch) + mag_y*sin(roll)sin(pitch) + mag_z*cos(roll)sin(pitch) CMy = mag_y*cos(roll) - mag_z*sin(roll)

MAG_Heading = Atan(CMy/CMx)

So, i applying the algorithm to my sketch:

``````pitchAccelXh = atan2((accelResult - biasAccelY) / 1024, (accelResult - biasAccelZ) / 1024); //Accelerometer Pitch Degrees in radian
rollAccelYh = atan2((accelResult - biasAccelX) / 1024, (accelResult - biasAccelZ) / 1024); //Accelerometer Roll Degrees in radian

float cos_roll= cos(pitchAccelXh);
float sin_roll = sin(pitchAccelXh);
float cos_pitch = cos(rollAccelYh);
float sin_pitch = sin(rollAccelYh);

float mag_X = scaled.XAxis;  //magnetometer X Axis
float mag_Y = scaled.YAxis;  //magnetometer Y Axis
float mag_Z = scaled.ZAxis;  //magnetometer Z Axis

// The tilt compensation algorithem.
Yh = mag_Y * cos_roll - mag_Z * sin_roll;
Xh = mag_X * cos_pitch + mag_Y * sin_roll * sin_pitch + mag_Z * cos_roll * sin_pitch;

realHeading = atan2(Yh, Xh) * (360.0 / (2*PI));
``````

But it didn't works, it is still like just use magnetometer without accelerometer, but different in data value. Anybody know about the right algorithm for Magnetometer Tilt Compensation?

Thank You..

I know this one, https://www.loveelectronics.co.uk/Tutorials/13/tilt-compensated-compass-arduino-tutorial But I have not used it for my HMC5883L yet.

I have try those one, but not working. It is working for you?

Any Idea?

I don't use that concept at all, and I think the logic and methodology for it is spurious.

The basic feature of the geomagnetic field is that it isn't parallel to the ground, as you would think when you have a magnetic compass sitting on a map on a table.

It is a vector field shooting down into the ground at a diagonal angle, which is more or less constant over short distances and which you can easily find out what it is for your region.

The 3-axis magnetometer is going to give you a vector in 3D which is going to show you the direction of that field. Since you already know the direction of the field, you can determine the orientation of the magnetometer, with the exception of rotation about the vector direction.

If you also have an accelerometer, and you assume your device is stationary or moving at a constant velocity ( so it has no actual acceleration ), you can fully determine the orientation of your device.

I don't use that concept at all, and I think the logic and methodology for it is spurious.

The basic feature of the geomagnetic field is that it isn't parallel to the ground, as you would think when you have a magnetic compass sitting on a map on a table.

It is a vector field shooting down into the ground at a diagonal angle, which is more or less constant over short distances and which you can easily find out what it is for your region.

The 3-axis magnetometer is going to give you a vector in 3D which is going to show you the direction of that field. Since you already know the direction of the field, you can determine the orientation of the magnetometer, with the exception of rotation about the vector direction.

If you also have an accelerometer, and you assume your device is stationary or moving at a constant velocity ( so it has no actual acceleration ), you can fully determine the orientation of your device.

So, how i can solve tilt compensation for yaw Axis? By the way, this is my complete code:

``````// Reference the I2C Library
#include
// Reference the HMC5883L Compass Library
#include

// Store our compass as a variable.
HMC5883L compass;
// Record any errors that may occur in the compass.
int error = 0;

int accelResult;
float timeStep = 0.02;          //20ms. Need a time step value for integration of gyro angle from angle/sec
float biasAccelX, biasAccelY, biasAccelZ;
float pitchAccel = 0;
float rollAccel = 0;

float Xh;
float Yh;
float realHeading;
float pitchAccelXh;
float rollAccelYh;
float XM;
float YM;
float ZM;

unsigned long timer;

//Penjabaran fungsi writeTo sebagai Fungsi untuk writing byte ke alamat device pada I2C
void writeTo(byte device, byte toAddress, byte val) {
Wire.beginTransmission(device);
Wire.write(toAddress);
Wire.write(val);
Wire.endTransmission();
}

//Penjabaran fungsi readFrom sebagai Fungsi untuk membaca num bytes dari alamat pada device I2C
void readFrom(byte device, byte fromAddress, int num, byte result[]) {
Wire.beginTransmission(device);
Wire.write(fromAddress);
Wire.endTransmission();
Wire.requestFrom((int)device, num);
int i = 0;
while(Wire.available()) {
result[i] = Wire.read();
i++;
}
}

//Fungsi untuk mmebaca nilai Accelerometer
void getAccelerometerReadings(int accelResult[]) {
byte buffer;
readFrom(0x40,0x02,6,buffer);
accelResult = (((int)buffer) << 8 ) | buffer; //Yes, byte order different from gyros'
accelResult = (((int)buffer) << 8 ) | buffer;
accelResult = (((int)buffer) << 8 ) | buffer;
}

// Out setup routine, here we will configure the microcontroller and compass.
void setup()
{

int totalAccelXValues = 0;
int totalAccelYValues = 0;
int totalAccelZValues = 0;
int i;

// Initialize the serial port.
Serial.begin(115200);

Serial.println("Starting the I2C interface.");
Wire.begin(); // Start the I2C interface.

Serial.println("Constructing new HMC5883L");
compass = HMC5883L(); // Construct a new HMC5883 compass.

Serial.println("Setting scale to +/- 1.3 Ga");
error = compass.SetScale(1.3); // Set the scale of the compass.
if(error != 0) // If there is an error, print it out.
Serial.println(compass.GetErrorText(error));

Serial.println("Setting measurement mode to continous.");
error = compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous
if(error != 0) // If there is an error, print it out.
Serial.println(compass.GetErrorText(error));

writeTo(0x40,0x10,0xB6); //Soft_reset accelerometer BMA180
writeTo(0x40,0x0D,0x10); //set fungsi ee_w

// Determine zero bias for all axes of both sensors by averaging 50 measurements
delay(100); //wait for gyro to "spin" up
for (i = 0; i < 50; i += 1) {
getAccelerometerReadings(accelResult);
totalAccelXValues += accelResult;
totalAccelYValues += accelResult;
totalAccelZValues += accelResult;
delay(50);
}

biasAccelX = totalAccelXValues / 50;
biasAccelY = totalAccelYValues / 50;
biasAccelZ = (totalAccelZValues / 50) - 256; //Don't compensate gravity away! We would all (float)!

}

// Our main program loop.
void loop()
{
// Retrive the raw values from the compass (not scaled).
MagnetometerRaw raw = compass.ReadRawAxis();
// Retrived the scaled values from the compass (scaled to the configured scale).
MagnetometerScaled scaled = compass.ReadScaledAxis();

// Values are accessed like so:
int MilliGauss_OnThe_XAxis = scaled.XAxis;// (or YAxis, or ZAxis)

// Calculate heading when the magnetometer is level, then correct for signs of axis.
float heading = atan2(scaled.YAxis, scaled.XAxis);

// Once you have your heading, you must then add your 'Declination Angle', which is the 'Error' of the magnetic field in your location.
// Find yours here: http://www.magnetic-declination.com/
// Mine is: 2Ã¯Â¿Â½ 37' W, which is 2.617 Degrees, or (which we need) 0.0456752665 radians, I will use 0.0457
// If you cannot find your Declination, comment out these two lines, your compass will be slightly off.
float declinationAngle = 0.55;
heading += declinationAngle;

// Correct for when signs are reversed.
if(heading < 0)
heading += 2*PI;

// Check for wrap due to addition of declination.
if(heading > 2*PI)
heading -= 2*PI;

// Convert radians to degrees for readability.
float headingDegrees = heading * 180/M_PI;

// Output the data via the serial port.
//Output(raw, scaled, heading, headingDegrees);

// Normally we would delay the application by 66ms to allow the loop
// to run at 15Hz (default bandwidth for the HMC5883L).
// However since we have a long serial out (104ms at 9600) we will let
// it run at its natural speed.
// delay(66);

timer = millis(); //get a start value to determine the time the loop takes
getAccelerometerReadings(accelResult);

pitchAccel = atan2((accelResult - biasAccelY) / 1024, (accelResult - biasAccelZ) / 1024) * 360.0 / (2*PI);
rollAccel = atan2((accelResult - biasAccelX) / 1024, (accelResult - biasAccelZ) / 1024) * 360.0 / (2*PI);

//---------------Tilt Compensated Begin----------------------------------------

pitchAccelXh = atan2((accelResult - biasAccelY) / 1024, (accelResult - biasAccelZ) / 1024);
rollAccelYh = atan2((accelResult - biasAccelX) / 1024, (accelResult - biasAccelZ) / 1024);

//pitchAccelXh = constrain(pitchAccelXh, -1.57, 1.57);
//pitchAccelXh = map(pitchAccelXh, -3.14, 3.14, 0, 6.28);

//rollAccelYh = constrain(rollAccelYh, -1.57, 1.57);
//rollAccelYh = map(rollAccelYh, -3.14, 3.14, 0, 6.28);

float cos_roll= cos(rollAccelYh);
float sin_roll = sin(rollAccelYh);
float cos_pitch = cos(pitchAccelXh);
float sin_pitch = sin(pitchAccelXh);

XM = scaled.XAxis;
YM = scaled.YAxis;
ZM = scaled.ZAxis;

// The tilt compensation algorithem.
Yh = YM * cos_roll - ZM * sin_roll;
Xh = XM * cos_pitch + YM * sin_roll * sin_pitch + ZM * cos_roll * sin_pitch;

realHeading = atan2(-Yh, Xh) * 360.0 / (2*PI);

//--------------------Tilt Compensated End--------------------------------------

Serial.print(pitchAccel);
Serial.print("  pitch \t");
Serial.print(rollAccel);
Serial.print("  roll \t");

//Serial.print(pitchAccelXh);
//Serial.print("  pitchXH ");
//Serial.print(rollAccelYh);
//Serial.print("  rollYH");

//Serial.print(raw.XAxis);
//Serial.print("RawX:\t");
//Serial.print("   ");
//Serial.print(raw.YAxis);
//Serial.print("   ");
//Serial.print(raw.ZAxis);
//Serial.print("   \tScaled:\t");

//Serial.print(scaled.XAxis);
//Serial.print("  X ");
//Serial.print(scaled.YAxis);
//Serial.print("  Y ");
//Serial.print(scaled.ZAxis);
//Serial.print("  Z ");

Serial.print(realHeading);
Serial.print(" DegreeReal  \t");

Serial.print(headingDegrees);
Serial.println(" Degrees   \t");

delay(200);

//timer = millis() - timer;          //how long did the loop take?
//timer = (timeStep * 1000) - timer; //how much time to add to the loop to make it last time step msec
//delay(timer);                                    //make one loop last time step msec

}
``````

It depends what you mean by "tilt compensation for your compass".

If you literally intend to use it as a compass ( for orienteering or something ), then you need to "compensate" for the fact that you are not holding your compass flat, steady, and parallel to the ground.

If you intend to use it as an aid to orientation calculation for some moving device, that's a rather different question. The best method comes down to what sort of device you are trying to orient ( a car ? a robot on the ground ? a copter ? An acrobatic aircraft ? ), and what other devices you are using it with.

I notice in your code there, there is nothing about what you are comparing the magnetic reading TO. Do you know what the magnetic field in your area is like ?

So, how i can solve tilt compensation for yaw Axis?

I am not even sure what you think this sentence means.

A magnetic device is going to clarify the orientation of your device in relation to rotating around the yaw axis. It's nothing to do with tilting the yaw axis.

I am not even sure what you think this sentence means.

A magnetic device is going to clarify the orientation of your device in relation to rotating around the yaw axis. It's nothing to do with tilting the yaw axis.

Sorry, i mean "tilt compensation for magnetometer"

If you literally intend to use it as a compass ( for orienteering or something ), then you need to "compensate" for the fact that you are not holding your compass flat, steady, and parallel to the ground.

If you intend to use it as an aid to orientation calculation for some moving device, that's a rather different question. The best method comes down to what sort of device you are trying to orient ( a car ? a robot on the ground ? a copter ? An acrobatic aircraft ? ), and what other devices you are using it with.

That's it, i am going to use this magnetometer for Multicopter to know where we are heading, so the magnetometer is not always flat, steady, and parallel to the ground. And the problem is the magnetometer need to be held flat to function properly. If we tilt it (pitch or roll) to certain angle (for example to 45 degrees) the reading will be more inaccurate the further the compass is tilted. So I need to keep the accuracy of magnetometer even we tilt it.

I am looking for some references on the internet and found some useful sites like this (Maybe it will help explain my problem more clearly): https://www.loveelectronics.co.uk/Tutorials/13/tilt-compensated-compass-arduino-tutorial http://diy.powet.eu/2011/03/19/tilt-compensation-azimuth-pitch-le-roll/?lang=en https://gist.github.com/timtrueman/322555 http://n0m1.com/2012/02/27/6dof-arduino-compass-accelerometer/

They are says we need both accelerometer and magnetometer to solve magnetometer inaccurate problem when it tilted, and use an algorithm. But, i still not yet success to solve this problem.

And the problem is the magnetometer need to be held flat to function properly. If we tilt it (pitch or roll) to certain angle (for example to 45 degrees) the reading will be more inaccurate the further the compass is tilted. So I need to keep the accuracy of magnetometer even we tilt it.

Thats not really correct. That is spurious wooly thinking which comes from the days of 2-axis magnetometers, maybe. A 3-axis magnetometer DOES NOT need to be held flat to function properly. It is always going to correctly show you the direction of the geomagnetic field ( or any other magnetic field which is present ), relative to its own current orientation. Apart from scaling and offset calibration for the different axial directions ( which will be an issue whether you are holding it flat and level, or not ), the assertion that the reading will be more inaccurate the further the compass is tilted, is simply not correct.

If you know the orientation of the device, you can always determine the projection of the magnetic field vector into a plane parallel to the ground. If you don't know the orientation of the device, you can compare the measured magnetic field vector to the expected magnetic field vector in your region, to determine the correct which needs to be made to the modelled orientation of the device.

The direction of the magnetism is flat at the equator, and pointing down at the poles. http://geokov.com/education/magnetic-declination-inclination.aspx http://en.wikipedia.org/wiki/Magnetic_dip This site tells how much the inclination is, http://magnetic-declination.com/ (if you type your location, click on the location marker). This site tells even more, http://www.geomag.bgs.ac.uk/data_service/models_compass/wmm_calc.html (click on the map to place the marker).

The direction of the magnetism is flat at the equator, and pointing down at the poles. http://geokov.com/education/magnetic-declination-inclination.aspx http://en.wikipedia.org/wiki/Magnetic_dip This site tells how much the inclination is, http://magnetic-declination.com/ (if you type your location, click on the location marker). This site tells even more, http://www.geomag.bgs.ac.uk/data_service/models_compass/wmm_calc.html (click on the map to place the marker).

Ok thanks Erdin..

Thats not really correct. That is spurious wooly thinking which comes from the days of 2-axis magnetometers, maybe. A 3-axis magnetometer DOES NOT need to be held flat to function properly. It is always going to correctly show you the direction of the geomagnetic field ( or any other magnetic field which is present ), relative to its own current orientation. Apart from scaling and offset calibration for the different axial directions ( which will be an issue whether you are holding it flat and level, or not ), the assertion that the reading will be more inaccurate the further the compass is tilted, is simply not correct.

If you know the orientation of the device, you can always determine the projection of the magnetic field vector into a plane parallel to the ground. If you don't know the orientation of the device, you can compare the measured magnetic field vector to the expected magnetic field vector in your region, to determine the correct which needs to be made to the modelled orientation of the device.

Ok, so the 3 axis magnetometer give us magnetic field in three axis, X, Y and Z. From my program that i have posted before, I successfully get magnetic field value in all 3 axis vector. Then, i want to know where we are heading (degree from north pole) by using this data . And according to this site: https://www.loveelectronics.co.uk/Tutorials/8/hmc5883l-tutorial-and-arduino-library

To Calculate heading of magnetometer, we can use this formulation: heading = arc tan (magnetic filed on Y Axis/magnetic filed on X Axis)

And finally i get the heading degrees, pretty nice when magnetometer keep flat. But, when i tilt the magnetometer (Pitch or Roll), the degrees value is change. And so far, I still not found a way to maintain the value of degree when the magnetometer tilted. Is there something wrong with my calculations?

Oh, and i found this video: http://www.youtube.com/watch?v=fOSTOnQzZCI

i look up for his code and he use something called Quaternion..

Yeah, quaternions are the way to go.

The problem starts, when you assume that you can calculate the direction of the magnetic field simply by taking the arctan of the Y axis and X ais readings of the magnetometer. That assumption is misguided. And once you have made that assumption, you have all kinds of work-arounds to “compensate” for the fact that you have made a poor assumption.

If you assume that your vehicle has very small actual real acceleration, then you can estimate the orientation of your device by observing the apparent direction of the gravitational “down” direction using your accelerometer.

You can then estimate the rotation matrix which transforms the actual orientation of your device into a reference frame which is parallel to the ground. You can then transform the apparent magnetic field vector into the reference frame. You can compare that to the assumed actual magnetic field vector, to estimate the discrepancy between the reference direction in the horizontal direction of your calculated reference frame, and the actual “true” reference horizontal direction of the reference frame.

This is conceptually quite simple. Further complications emerge because different people use different conventions for which way the reference axes run.

Yeah, quaternions are the way to go.

The problem starts, when you assume that you can calculate the direction of the magnetic field simply by taking the arctan of the Y axis and X ais readings of the magnetometer. That assumption is misguided. And once you have made that assumption, you have all kinds of work-arounds to "compensate" for the fact that you have made a poor assumption.

If you assume that your vehicle has very small actual real acceleration, then you can estimate the orientation of your device by observing the apparent direction of the gravitational "down" direction using your accelerometer.

You can then estimate the rotation matrix which transforms the actual orientation of your device into a reference frame which is parallel to the ground. You can then transform the apparent magnetic field vector into the reference frame. You can compare that to the assumed actual magnetic field vector, to estimate the discrepancy between the reference direction in the horizontal direction of your calculated reference frame, and the actual "true" reference horizontal direction of the reference frame.

This is conceptually quite simple. Further complications emerge because different people use different conventions for which way the reference axes run.

Well, thank you about the Explanation michinyon, but i little bit confusing, can you explain it again with some mathematical form, formulation or algorithm?

michinyon: Yeah, quaternions are the way to go.

The problem starts, when you assume that you can calculate the direction of the magnetic field simply by taking the arctan of the Y axis and X ais readings of the magnetometer. That assumption is misguided. And once you have made that assumption, you have all kinds of work-arounds to "compensate" for the fact that you have made a poor assumption.

If you assume that your vehicle has very small actual real acceleration, then you can estimate the orientation of your device by observing the apparent direction of the gravitational "down" direction using your accelerometer.

You can then estimate the rotation matrix which transforms the actual orientation of your device into a reference frame which is parallel to the ground. You can then transform the apparent magnetic field vector into the reference frame. You can compare that to the assumed actual magnetic field vector, to estimate the discrepancy between the reference direction in the horizontal direction of your calculated reference frame, and the actual "true" reference horizontal direction of the reference frame.

This is conceptually quite simple. Further complications emerge because different people use different conventions for which way the reference axes run.

Hi, michinyon. I found Quaternion based algorithm from here: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/

It's open source, and i tried to use that algorithm on my code. This is my coomplete Code: https://github.com/mamette/quaternion/blob/master/AHRS.ino

I got the Pitch, Roll and Yaw value, but they are irregularly. Is anything wrong with my code?

And i have some question: 1. When we insert the Accelerometer value to Quaternion, the Accelerometer value must be in "g" scale or just raw value on each axis? 2. When we insert the Gyroscope value to Quaternion, the Gyroscope value must be in rad/s, it is right? 3. When we insert the Magnetometer value to Quaternion, just insert Magnetometer raw value on each axis, it is right?

Thank You..

Have you calibrated the Mag to get the offsets? Remember, in a quadcopter, the mag is between 4 motors, which is going to affect the output.

You will need to pass rad/s for gyro (from memory I'm pretty sure that's what I do for Madgwick) and raw (non G) values for accel.

I've personally never had much success with mags on my quad - they seem to cause drift which affects pitch and roll.

You first need calibration (either using the device calibration method, or, by rotating it in a complete circle to ascertain the full field strengths).

You can then use the X, Y, and Z readings to compute the location of "north" relative to whichever way you're pointing (keeping in mind that "north" changes as much as 30 degrees depending where on earth you are (called declination - see http://www.magnetic-declination.com/), and that it's not horizontal to the ground either (confusingly, called "inclination" - see https://physics.stackexchange.com/questions/283761/if-you-hold-a-compass-needle-vertical-does-it-point-down-or-up-differently-on-wh/283782 )

If you picture the X, Y, and Z readings as meaning "where on an imaginary circle is the direction of magnetism - where those 3 circles are on 3 different planes, you can compute the logical position of a vector pointing towards magnetic north in 3 dimensions.

The bad news - there's no simple way to do what you want accurately, and definitely not one that's going to work without trig math on all 3 readings (you can't just use X and/or Y alone).

The good news - you probably don't really need to know an actual "heading" - most applications just need some local relative direction as compared with something you already know.

Beware that strong magnets (such as ones in all modern motors) seriously mess with readings from a long way away (10 or more foot even) - grab an app from your phone store that shows your phone magnetometer readings, and wave a magnet around to see this easily!

This page has detailed with code purportedly working (although not accommodating declination/inclination AFAIK): http://mythopoeic.org/magnetometer-real-data/

Fun fact: pigeons can literally "see" magnetism... but only if the sky is blue (no joke; the blue is part of their eyes' mechanism to see this)