Guide to gyro and accelerometer with Arduino including Kalman filtering

@Jase_MK
Sorry, but you misunderstand the whole point of a Kalman filter or similar. The whole point of a Kalman filter or similar is to filter out any noise i.e. acceleration from a car etc. So if you tune your filter properly it will filter out all the noise from the accelerometer! To get the total amount of g-force, you just have to calculate the length of the force vector, as I did in the code.

Regards
Lauszus

OK, thanks. So using the Kalman filter, or similar, I can use my IMU readings to calculate angular position (pitch, roll), even if the car is accelerating about all over the place. I think I need to read the thread again from the start and try out some code and see what happens.

Thanks

@Jase_MK
Yes exactly :slight_smile:
Okay, do that - I think the best way to "learn" how it works is to see the effect of the filter yourself.
Try to upload the Arduino code provided and use the Processing program I created to see a visual representation of the data.

Regards
Lauszus

Will do. My IMU is in the post so I shall get playing asap.

Thanks again

@Jase_MK
Okay. Your welcome :slight_smile:
Just write me again if you need further assistance!

Regards
Lauszus

Hello,

Pretty new to the forum and to using the Arduino Uno with the SLM303.

I'm trying to build a tilt compensated compass with an nmea string as out. This string is going to be input for my boat network so my tiller pilot will be able to better steer a course. Quite a project....

The code I have so far:

#include <Wire.h>
#include <LSM303.h>

LSM303 compass;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  compass.init();
  compass.enableDefault();
  
  // Calibration values. Use the Calibrate example program to get the values for
  // your compass.
  compass.m_min.x = -397; compass.m_min.y = -284; compass.m_min.z = -464;
  compass.m_max.x = 150; compass.m_max.y = 287; compass.m_max.z = 227;
  
  /* Output van Calibrate
  M min X: -397 Y: -284 Z: -464 
  M max X: 150 Y: 287 Z: 227
  */
}

/* Oorspronkelijke code
void loop() {
  compass.read();
  int heading = compass.heading((LSM303::vector){0,-1,0});
  Serial.println(heading);
  delay(500);
} 
*/

/*----------------------------------------------------------------------------
        1   2   3 4   5 6
        |   |   | |   | |
 $--HDG,x.x,x.x,a,x.x,a*hh<CR><LF>
------------------------------------------------------------------------------

Field Number: 
1. Magnetic Sensor heading in degrees
2. Magnetic Deviation, degrees
3. Magnetic Deviation direction, E = Easterly, W = Westerly
4. Magnetic Variation degrees
5. Magnetic Variation direction, E = Easterly, W = Westerly
6. Checksum */

char nmeastr[17]; // HCHDG,000.00,,,,*
int checksum;
char hs[6]; // 000.00

void loop() {
  strcpy(nmeastr,"HCHDG,");
  
  //dtostrf(FLOAT,WIDTH,PRECSISION,BUFFER);
  compass.read();
  dtostrf(compass.heading(), 5, 0, hs);
  //Serial.print("hs ");
  //Serial.print(hs);
  //Serial.println();  
  
  strcat(nmeastr,hs);
  strcat(nmeastr,",,,,");
  
  //add a checksum
  checksum=0;
  for (int n=0; n < strlen(nmeastr); n++) {
    checksum ^= nmeastr[n];
  }
  
  Serial.print("$");
  Serial.print(nmeastr);
  Serial.print("*");
  Serial.print(checksum, HEX);
  Serial.println();
  
  /* 5hz == 200 */
  delay(500);
}

When laying flat on the table, it produces:

$HCHDG, 207,,,,*59
$HCHDG, 211,,,,*5E
$HCHDG, 213,,,,*5C
$HCHDG, 209,,,,*57
$HCHDG, 213,,,,*5C
$HCHDG, 211,,,,*5E
$HCHDG, 210,,,,*5F
$HCHDG, 215,,,,*5A
$HCHDG, 212,,,,*5D
$HCHDG, 207,,,,*59
$HCHDG, 208,,,,*56
$HCHDG, 208,,,,*56
$HCHDG, 208,,,,*56
$HCHDG, 211,,,,*5E
$HCHDG, 210,,,,*5F
$HCHDG, 210,,,,*5F
$HCHDG, 207,,,,*59
$HCHDG, 217,,,,*58
$HCHDG, 215,,,,*5A
$HCHDG, 211,,,,*5E
$HCHDG, 211,,,,*5E
$HCHDG, 209,,,,*57
$HCHDG, 212,,,,*5D
$HCHDG, 210,,,,*5F
$HCHDG, 209,,,,*57
$HCHDG, 210,,,,*5F
$HCHDG, 211,,,,*5E
$HCHDG, 212,,,,*5D
$HCHDG, 211,,,,*5E
$HCHDG, 210,,,,*5F

As you can see, not a stable course to steer :wink: The video shows the issue I'm facing when it comes to stable ocurse

I know I should do some filtering, and this is where I got to this topic and also got stuck on how to get the kalman filter to this code. I'd like some help on how I can put the filter in place so the course gets more stable.

Many thanks in advance for your reply!

Hi,
I don't have any experience with magnetometers - I know that Sparkfun called it a compass sensor, but it's really a magnetometer.

Have you check out this application note:

Also check out this library I found:

I don't know if that is the library you are already using? But it look like it? Have you tried to run the example sketch (https://github.com/pololu/LSM303/blob/master/LSM303/examples/Heading/Heading.ino) and see how that went?

Remember that any magnetic field will affect your reading, so keep away all electronics - my friend had a big problem with his until he realized that his laptop's harddrive affected his magnetometer a lot!

Sorry that I can't help you no more - I have no experience with magnetometers. If it were me would properly try the example sketch provided in the library and then read the application note to understand what was going on :slight_smile:
I don't think you can use the Kalman filter I provided. You need another type of filter - try google for "ahrs magnetometer" and see what you can find.

Regards
Lauszus

Thanks for your explaination Lauszus.

The code from GitHub - pololu/lsm303-arduino: Arduino library for Pololu LSM303 boards is the code I use for the compass function. The output shown is the result of a minor addition I did to create nmea output.

I'll google "ahrs magnetometer" to see if it can help. Part of the learning curve :wink:

George

Okay. I will try to talk with my teammate tomorrow as he is currently building a quadrocopter, so he has some experience with magnetometers, and then ask him for good links and so on.

Regards
Lauszus

Thanks Lauszus, that woud be great!

Been searching using the keywords you suggest, however I did not find a solution that would fit my needs. Sites I found suggest to use some sort of Kalman filter. Will also do more digging.

Cheers, George

Dear George.

This is Thomas, Lauszus' teammate. As Lauszus sure enough introduced me I am building a Quadrocopter. To do this you need a 9DOF sensor, which means a 3-angled accelerometer, gyro and magnetometer.
To combine all the sensor data I use a DCM algorithm - Direct Cosine Matrix - which then gives me the orientation of the board in x, y and z rotational degrees.

With the orientation I am then also able to get the tilt compensated heading by combining the magnetometer data and the orientation values.

On the Arduino, which I am not using for this project though, it can be done with the following code: https://dev.qu.tu-berlin.de/projects/sf-razor-9dof-ahrs/wiki/Tutorial
If that doesn't work out for you because of it being to advanced then have a look at the smaller version of the code here: Google Code Archive - Long-term storage for Google Code Project Hosting.

Let me know if you can get it working.

Best Regards
Thomas Jespersen

Dear Thomas, Lauszus,

Thanks for the time you took to reply.

I read the article and searched for a calibration programm related to the LSM303. I found this program:

#include <Wire.h>
#include <LSM303.h>

LSM303 compass;
LSM303::vector running_min = {2047, 2047, 2047}, running_max = {-2048, -2048, -2048};

// temp
float x_min; float x_max; float x_cur;
float y_min; float y_max; float y_cur;
float z_min; float z_max; float z_cur;

void setup() {
  
  Serial.begin(9600);
  Wire.begin();
  compass.init();
  compass.enableDefault();
 
}

void loop() {  
  compass.read();
  
  running_min.x = min(running_min.x, compass.m.x);
  running_min.y = min(running_min.y, compass.m.y);
  running_min.z = min(running_min.z, compass.m.z);

  running_max.x = max(running_max.x, compass.m.x);
  running_max.y = max(running_max.y, compass.m.y);
  running_max.z = max(running_max.z, compass.m.z);
  
  Serial.print("M min ");
  Serial.print("X: ");
  Serial.print((int)running_min.x);
  Serial.print(" Y: ");
  Serial.print((int)running_min.y);
  Serial.print(" Z: ");
  Serial.print((int)running_min.z);

  Serial.print(" M max ");  
  Serial.print("X: ");
  Serial.print((int)running_max.x);
  Serial.print(" Y: ");
  Serial.print((int)running_max.y);
  Serial.print(" Z: ");
  Serial.println((int)running_max.z);
  
  delay(100);
}

After running the programm and looking at the serial output, I started moving the board in all sorts of direction, and after some time I got a stable output:

/* Output of Calibrate.ino
M min X: -548 Y: -378 Z: -547 M max X: 248 Y: 413 Z: 314
M min X: -548 Y: -378 Z: -547 M max X: 248 Y: 413 Z: 314
M min X: -548 Y: -378 Z: -547 M max X: 248 Y: 413 Z: 314
*/

These values I entered in the programm "Heading.ino"

#include <Wire.h>
#include <LSM303.h>

LSM303 compass;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  compass.init();
  compass.enableDefault();
  
  // Calibration values. Use the Calibrate example program to get the values for
  // your compass.
  compass.m_min.x = -548; compass.m_min.y = -378; compass.m_min.z = -547;
  compass.m_max.x = 248; compass.m_max.y = 413; compass.m_max.z = 314;
  
  /* Output van Calibrate
  M min X: -548 Y: -378 Z: -547 M max X: 248 Y: 413 Z: 314
  M min X: -548 Y: -378 Z: -547 M max X: 248 Y: 413 Z: 314
  M min X: -548 Y: -378 Z: -547 M max X: 248 Y: 413 Z: 314
  */
}

/* Oorspronkelijke code
void loop() {
  compass.read();
  int heading = compass.heading((LSM303::vector){0,-1,0});
  Serial.println(heading);
  delay(500);
} 
*/

/*----------------------------------------------------------------------------
        1   2   3 4   5 6
        |   |   | |   | |
 $--HDG,x.x,x.x,a,x.x,a*hh<CR><LF>
------------------------------------------------------------------------------

Field Number: 
1. Magnetic Sensor heading in degrees
2. Magnetic Deviation, degrees
3. Magnetic Deviation direction, E = Easterly, W = Westerly
4. Magnetic Variation degrees
5. Magnetic Variation direction, E = Easterly, W = Westerly
6. Checksum */

char nmeastr[17]; // HCHDG,000.00,,,,*
int checksum;
char hs[6]; // 000.00

void loop() {
  strcpy(nmeastr,"HCHDG,");
  
  //dtostrf(FLOAT,WIDTH,PRECSISION,BUFFER);
  compass.read();
  dtostrf(compass.heading((LSM303::vector){0,-1,0}), 5, 0, hs);
  //Serial.print("hs ");
  //Serial.print(hs);
  //Serial.println();  
  
  strcat(nmeastr,hs);
  strcat(nmeastr,",,,,");
  
  //add a checksum
  checksum=0;
  for (int n=0; n < strlen(nmeastr); n++) {
    checksum ^= nmeastr[n];
  }
  
  Serial.print("$");
  Serial.print(nmeastr);
  Serial.print("*");
  Serial.print(checksum, HEX);
  Serial.println();
  
  /* 5hz == 200 */
  delay(500);
}

While running this programm, serial output showed the heading as NMEA-string. Having it away from all sorts of electroncs and put a normal (old fashion :wink: ) compass pointing in the same directoin, and laying flat on the floor,, the output showed:

$HCHDG, 0,,,,*5C
$HCHDG, 2,,,,*5E
$HCHDG, 5,,,,*59
$HCHDG, 1,,,,*5D
$HCHDG, 3,,,,*5F
$HCHDG, 1,,,,*5D
$HCHDG, 2,,,,*5E
$HCHDG, 4,,,,*58
$HCHDG, 1,,,,*5D
$HCHDG, 2,,,,*5E
$HCHDG, 2,,,,*5E
$HCHDG, 0,,,,*5C
$HCHDG, 3,,,,*5F
$HCHDG, 1,,,,*5D
$HCHDG, 2,,,,*5E
$HCHDG, 3,,,,*5F
$HCHDG, 3,,,,*5F
$HCHDG, 3,,,,*5F
$HCHDG, 3,,,,*5F
$HCHDG, 2,,,,*5E
$HCHDG, 4,,,,*58
$HCHDG, 4,,,,*58
$HCHDG, 5,,,,*59

Allthough not very accurated, it points north with some variance. Not too bad, but room for improvement, I think.

As soon as I start turning the lsm303 up and down , still pointing in the same direction, I would expect to see the same heading however the output shows:

$HCHDG, 14,,,,*49
$HCHDG, 14,,,,*49
$HCHDG, 16,,,,*4B
$HCHDG, 12,,,,*4F
$HCHDG, 10,,,,*4D
$HCHDG, 6,,,,*5A
$HCHDG, 5,,,,*59
$HCHDG, 6,,,,*5A
$HCHDG, 10,,,,*4D
$HCHDG, 7,,,,*5B
$HCHDG, 9,,,,*55
$HCHDG, 8,,,,*54
$HCHDG, 9,,,,*55
$HCHDG, 12,,,,*4F
$HCHDG, 13,,,,*4E
$HCHDG, 11,,,,*4C
$HCHDG, 10,,,,*4D
$HCHDG, 11,,,,*4C

IMHO, this heading info is too inaccurate to use as a compass. The autopilot will move the boat from left to right, like a drunken sailor :wink:

I'm not sure wether this is part of the lsm303 (in that case it is not a sufficiënt solution for a compass for an auto pilot) or filtering or something else is required to get a more stable output.

Many thanks, George

Lauszus:
You can not use the kalman filter with only one input. Sorry but you need a gyro too, that is why it called "sensor fusion" :slight_smile:
You could perhaps use some other kind of smoothing algorithm.

  • Lauszus

This is not a true statement. I'm surprised no one picked up on it. A Kalman filter is a useful tool often used to to do sensor fusion. It is not a synonym for sensor fusion. It was originally invented for single measurement systems. The value of a Kalman filter over a regular filter is that it is focused on predicting a future state rather than just estimating the current state.

@zencuke
You are totally right, sorry if it has caused anybody problems!
You could actually try to use the Kalman code provided in the sketch and the just set the gyrorate at 0. I havn't tried it, but in theory it could be done, but you will probably have tune the noise covariances for it to work properly.

Thanks for your feedback!

Hello,

Please I have some questions about this gyro+acc

  1. my arduino (ARDUINO UNO R3) works with a SPI communication (i think so :s) , and i think the gyro+acc board works with a I2C communication protocol. Do you think my arduino can fit with it ? Or do you think it can work with a I2C communication like maybe i'm wrong ?

  2. I wanted to buy this IMU Analog Combo Board Razor - 6DOF Ultra-Thin IMU (IMU Analog Combo Board Razor - 6DOF Ultra-Thin IMU - SEN-10010 - SparkFun Electronics) but in the link, it is said that the IMU is replaced by another one (IMU Digital Combo Board - 6 Degrees of Freedom ITG3200/ADXL345). I think it is quite the same but i want to be sure before buying it. Do you think your different equations to translate acc and gyro data in degrees is gonna work on the new board ?

  3. I Also saw in the sensor's datasheet of the new board that the sensivity is 14.375 LSB/º/s... It's not the same unit as the sensivity of the old one (3.33mV/º/s)... Do you have an idea of what i got to do ?

Thanks

  1. my arduino (ARDUINO UNO R3) works with a SPI communication (i think so :s) , and i think the gyro+acc board works with a I2C communication protocol. Do you think my arduino can fit with it ? Or do you think it can work with a I2C communication like maybe i'm wrong ?

The Arduino supports both SPI and I2C at the same time. By default you don't use the SPI for your Arduino, it is programmed via UART using the bootloader. See the last section at the following page: http://arduino.cc/en/Guide/Environment#uploading. You only use SPI for programming if you are using a programmer (see http://arduino.cc/en/Hacking/Programmer).
You can also use SPI to communicate with sensors etc. See SPI - Arduino Reference. A lot of sensors support both SPI and I2C. Since the name I2C is copyright protected by Phillips, the Arduino team is not allowed to call the library I2C, so instead they call it the Wire library. See: http://arduino.cc/it/Reference/Wire.

  1. I wanted to buy this IMU Analog Combo Board Razor - 6DOF Ultra-Thin IMU (IMU Analog Combo Board Razor - 6DOF Ultra-Thin IMU - SEN-10010 - SparkFun Electronics) but in the link, it is said that the IMU is replaced by another one (IMU Digital Combo Board - 6 Degrees of Freedom ITG3200/ADXL345). I think it is quite the same but i want to be sure before buying it. Do you think your different equations to translate acc and gyro data in degrees is gonna work on the new board ?

It will works just as well, there is even more functions when using a digital board - like tap sensing, free fall detection etc.
The reason why I started out by using analog sensors, were because I didn't need the increased functionality that a digital IMU offers, as I wanted to build a balancing robot (GitHub - TKJElectronics/BalancingRobotArduino: This is the Arduino version of the code for my balancing robot/segway) since I only wanted to calculate the angle of the robot, I just bought a analog one. But I also got a cheap digital IMU to mess around with.
It takes a bit more of work to get a digital sensor working, but there is plenty of example code for all kinds of IMU's so even a beginner could get it working in a couple of hours.

  1. I Also saw in the sensor's datasheet of the new board that the sensivity is 14.375 LSB/º/s... It's not the same unit as the sensivity of the old one (3.33mV/º/s)... Do you have an idea of what i got to do ?

Yes, it's pretty simple, just devide the reading (minused the offset) by 14.375 to convert it to º/s.
I will upload a alternative version of the code using a digital IMU instead in a couple of hours to get you started!

Regards
Lauszus

@kevinh4c
The code for a digital IMU can now be found at github: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF/Digital_IMU

Regards
Lauszus

This post really help my understanding. Thank a lot!!! Keep it up!

Thanks So much Lauszus For your HELP
I'll look after all this :slight_smile:

Lauszus:

  1. I Also saw in the sensor's datasheet of the new board that the sensivity is 14.375 LSB/º/s... It's not the same unit as the sensivity of the old one (3.33mV/º/s)... Do you have an idea of what i got to do ?

Yes, it's pretty simple, just devide the reading (minused the offset) by 14.375 to convert it to º/s.
I will upload a alternative version of the code using a digital IMU instead in a couple of hours to get you started!

So i just got to do this operation for the digital GIRO ?

gyroRate = (gyroAdc-gryoZero)/14.375 ?

Thanks