How would i implement position and heading readings into my SLAM robot?

I have already posted this question on stack exchange yesterday but haven't gotten a response yet so i'm trying my luck here.

I am trying to build a low-cost SLAM system with an MPU-6050 and GY-271 (magnetometer). Currently, i have a robot with an Arduino that collects the sensor data and a Raspberry Pi that (hopefully) will do the SLAM calculations.

I want my robot to be able to use all three sensor readings (along with a PML) in SLAM to create a 2D map of the environment. However, considering that i want a 2D map, i will not require all the axis readings correct?

I read another post where one of the answers said that only the yaw from the gyroscope, and the x and y from the accelerometer would be needed.

  1. My question is, how would i implement this into my SLAM robot? Would a complementary filter work??

  2. Would i also need to use all the axis (x, y, and z) readings from the magnetometer? Or just one or two axis?

Here is my code if you want to have a look at it.

#include <Wire.h>

long accelX, accelY, accelZ;
float gForceX, gForceY, gForceZ;

long gyroX, gyroY, gyroZ;
float rotX, rotY, rotZ;

long magX, magY, magZ;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  setupMPU();

  //  //Find the starting angle.
  //  accelData();
  //  gyroData();
  //  magData();
  //  printData();
  //  delay(1000);
}

void loop() {
  accelData();
  gyroData();
  magData();
  printData();
  delay(100);
}

void setupMPU() {
  //Initialize MPU-6050.
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission();

  //Gyroscope Range Configuration
  Wire.beginTransmission(0x68);
  Wire.write(0x1B);
  Wire.write(0);
  Wire.endTransmission();

  //Accelerometer Range Configuration
  Wire.beginTransmission(0x68);
  Wire.write(0x1C);
  Wire.write(0);
  Wire.endTransmission();

  //Magnetometer Mode Selection
  Wire.beginTransmission(0x1E);
  Wire.write(0x02);
  Wire.write(0);
  Wire.endTransmission();
}

void accelData() {
  Wire.beginTransmission(0x68);
  Wire.write(0x3B);
  Wire.endTransmission();

  Wire.requestFrom(0x68, 6);
  while (Wire.available() < 6);
  accelX = Wire.read() << 8 | Wire.read();
  accelY = Wire.read() << 8 | Wire.read();
  accelZ = Wire.read() << 8 | Wire.read();

  gForceX = accelX / 16384.0;
  gForceY = accelY / 16384.0;
  gForceZ = accelZ / 16384.0;
}

void gyroData() {
  Wire.beginTransmission(0x68);
  Wire.write(0x43);
  Wire.endTransmission();

  Wire.requestFrom(0x68, 6);
  while (Wire.available() < 6);
  gyroX = Wire.read() << 8 | Wire.read();
  gyroY = Wire.read() << 8 | Wire.read();
  gyroZ = Wire.read() << 8 | Wire.read();

  rotX = gyroX / 131.0;
  rotY = gyroY / 131.0;
  rotZ = gyroZ / 131.0;
}

void magData() {
  Wire.beginTransmission(0x1E);
  Wire.write(0x03);
  Wire.endTransmission();

  Wire.requestFrom(0x1E, 6);
  while (Wire.available() < 6);
  magX = Wire.read() << 8 | Wire.read();
  magZ = Wire.read() << 8 | Wire.read();
  magY = Wire.read() << 8 | Wire.read();
}

void printData() {
  Serial.print("Gyro (degrees)");
  Serial.print(" X=");
  Serial.print(rotX);
  Serial.print(" Y=");
  Serial.print(rotY);
  Serial.print(" Z=");
  Serial.print(rotZ);
  Serial.print(" Accelerometer (g)");
  Serial.print(" X=");
  Serial.print(gForceX);
  Serial.print(" Y=");
  Serial.print(gForceY);
  Serial.print(" Z=");
  Serial.print(gForceZ);
  Serial.print(" Magnetometer");
  Serial.print(" X=");
  Serial.print(magX);
  Serial.print(" Y=");
  Serial.print(magY);
  Serial.print(" Z=");
  Serial.println(magZ);
}

However, considering that i want a 2D map, i will not require all the axis readings correct?

If your robot really moves in a 2D plane, the acceleration in one axis (assuming you mount the accelerometer correctly) will be constant. Gravity still sucks. You can ignore that one axis.

  1. My question is, how would i implement this into my SLAM robot?

What is "this" that you wish to implement?

Would a complementary filter work??

As in, every time the Pi got an X acceleration that was greater than some value, is sent back "great job!" and every time it got a Y acceleration less than some value, it sent back "Keep up the great work'?

What, exactly, do you want to filter?

Hey Paul,

I realize that the question wasn't very clear (sorry about that!) so let me put it another way.

My goal here is to implement an AHRS for use in a 2D SLAM system. I want to use the magnetometer and gyroscope to calculate the robot's heading. I also want to use the accelerometer and wheel encoders to calculate the robot's velocity and change in position. Then with that i can hopefully find the change in the x position and y position for the robot. The only problem is, i don't know how i would combine the magnetometer and gyroscope readings to calculate the heading and the accelerometer and wheel encoder readings to calculate the velocity and change in position.

I hope that was i bit clearer. :slight_smile:

don't know how i would combine the magnetometer and gyroscope readings to calculate the heading and the accelerometer and wheel encoder readings to calculate the velocity and change in position.

Before you combine readings, get each separate "reading" working first. Learn how to calculate heading from the magnetometer; learn how to estimate position from the encoders. Both are quite complicated.

You can read here why the accelerometer and gyro are very unlikely to be useful for position and velocity estimations.

You can read here why the accelerometer and gyro are very unlikely to be useful for position and velocity estimations.

Then you can think about how you might move through a room full of furniture, in a blackout, wearing a blindfold. Slowly, one step at a time. Feel around. Nothing there? Good. This space is clear. Move another step. Repeat. Sooner or later, your sore toes will know where every piece of furniture is.