Calculating displacement using Accelerometer and Gyroscope (MPU6050)

Question

I am a computer science student and working on an electronics project that requires to calculate yaw, pitch, roll and X,Y,Z displacement. I want to attach an IMU in a gun and track its orientation and displacement. I am able to get Yaw, Pitch and Roll but unfortunately cant understand how to calculate displacement or position of my gun. I am using a 10-DOF GY-87 sensor that contains MPU-6050.

I am getting values in term of g and m/s2 format. From the research that i have studied yet is that i need to get acceleration/time2 and then add all the values. But cant understand what time difference i should use. Refrence: How to calculate distance based on phone acceleration

#include "I2Cdev.h"
#include "MPU6050.h"

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;
//MPU6050 accelgyro(0x69); // <-- use for AD0 high

int16_t ax, ay, az;
float dx, dy, dz = 0;
int16_t gx, gy, gz;





#define LED_PIN 13
bool blinkState = false;

void setup() {
    // join I2C bus (I2Cdev library doesn't do this automatically)
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif

    Serial.begin(38400);

    Serial.println("Initializing I2C devices...");
    accelgyro.initialize();

    Serial.println("Testing device connections...");
    Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

    Serial.println("Updating internal sensor offsets...");

    accelgyro.setXGyroOffset(85);
    accelgyro.setYGyroOffset(1);
    accelgyro.setZGyroOffset(-4);
    accelgyro.setXAccelOffset(-4269);
    accelgyro.setYAccelOffset(-4836);
    accelgyro.setZAccelOffset(1080);

    pinMode(LED_PIN, OUTPUT);
}

void loop() {

        accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

        dx=dx+(float)(((float)ax/(float)16384)*9.8*0.05*0.05);
        dy=dy+(float)(((float)ay/(float)16384)*9.8*0.05*0.05);
        dz=dz+(float)(((float)az/(float)16384)*9.8*0.05*0.05);
        Serial.print(dx); Serial.print("\t");
        Serial.print(dy); Serial.print("\t");
        Serial.print(dz); Serial.print("\t\n");


delay(1000);
    // blink LED to indicate activity
    blinkState = !blinkState;
    digitalWrite(LED_PIN, blinkState);
}

I want to track object as shown in the following youtube videos.

http://www.youtube.com/watch?v=ZYyyaJgKsDg

I would be grateful to you if anyone of you can guide me in this Regards. Thank you

P.S: Sorry for my bad english and use of non-technical terms.

Answer 1

I am afraid that the answer is not one you will want to hear. It is very, very hard to calculate position from a IMU unit. This video from Google is a very good reference for why (go to minute 24 for a detailed explanation). Basically, you need to integrate acceleration twice to get to position. You also need to remove gravity from the acceleration seen by your IMU. If this isn't done perfectly, the errors add up really fast.

The video you referenced used the information that the ball was rolling on the table to inform their model. They could track the orientation of their sensor to know which way the ball was rolling. They used the radius of the ball along with the angular changes from their board to track the ball in x and y. If you picked their ball off the table it wouldn't work at all.

If you need to track something, you should look for some sensor that can give you information on the position of your object (GPS, video analysis). Then you can use a Kalman filter to combine that with the IMU data to get good positional accuracy.

Good luck with your project.