Pages: [1] 2   Go Down
Author Topic: Magnetometer Tilt Compensation for Yaw Axis Using HMC5883L and BMA180  (Read 11426 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Jr. Member
**
Karma: 0
Posts: 72
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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:

Code:
pitchAccelXh = atan2((accelResult[1] - biasAccelY) / 1024, (accelResult[2] - biasAccelZ) / 1024); //Accelerometer Pitch Degrees in radian
  rollAccelYh = atan2((accelResult[0] - biasAccelX) / 1024, (accelResult[2] - 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..
Logged

Offline Offline
Edison Member
*
Karma: 58
Posts: 2078
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

Offline Offline
Jr. Member
**
Karma: 0
Posts: 72
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Offline Offline
Jr. Member
**
Karma: 0
Posts: 72
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Any Idea?
Logged

Offline Offline
Faraday Member
**
Karma: 62
Posts: 3077
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.


Logged

Offline Offline
Jr. Member
**
Karma: 0
Posts: 72
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Quote
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:
Code:
// Reference the I2C Library
#include <Wire.h>
// Reference the HMC5883L Compass Library
#include <HMC5883L.h>

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

int accelResult[3];
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[6];
  readFrom(0x40,0x02,6,buffer);
  accelResult[0] = (((int)buffer[1]) << 8 ) | buffer[0]; //Yes, byte order different from gyros'
  accelResult[1] = (((int)buffer[3]) << 8 ) | buffer[2];
  accelResult[2] = (((int)buffer[5]) << 8 ) | buffer[4];
}


// 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[0];
    totalAccelYValues += accelResult[1];
    totalAccelZValues += accelResult[2];
    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[1] - biasAccelY) / 1024, (accelResult[2] - biasAccelZ) / 1024) * 360.0 / (2*PI);
  rollAccel = atan2((accelResult[0] - biasAccelX) / 1024, (accelResult[2] - biasAccelZ) / 1024) * 360.0 / (2*PI);
 
  //---------------Tilt Compensated Begin----------------------------------------
 
  pitchAccelXh = atan2((accelResult[1] - biasAccelY) / 1024, (accelResult[2] - biasAccelZ) / 1024);
  rollAccelYh = atan2((accelResult[0] - biasAccelX) / 1024, (accelResult[2] - 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
 
}



Logged

Offline Offline
Faraday Member
**
Karma: 62
Posts: 3077
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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 ?
Logged

Offline Offline
Faraday Member
**
Karma: 62
Posts: 3077
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Quote
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.
Logged

Offline Offline
Jr. Member
**
Karma: 0
Posts: 72
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Quote
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"

Quote
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.
« Last Edit: March 09, 2013, 07:27:26 am by mamette » Logged

Offline Offline
Faraday Member
**
Karma: 62
Posts: 3077
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Quote
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.
Logged

Offline Offline
Edison Member
*
Karma: 58
Posts: 2078
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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).
« Last Edit: March 10, 2013, 03:16:41 am by Erdin » Logged

Offline Offline
Jr. Member
**
Karma: 0
Posts: 72
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Quote
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..

Quote
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:


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

Offline Offline
Faraday Member
**
Karma: 62
Posts: 3077
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.


Logged

Offline Offline
Jr. Member
**
Karma: 0
Posts: 72
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Quote
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?
Logged

Offline Offline
Jr. Member
**
Karma: 0
Posts: 72
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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..
« Last Edit: March 17, 2013, 11:59:27 pm by mamette » Logged

Pages: [1] 2   Go Up
Jump to: