what is the algorithm to get position (Linear displacement and linear velocity ) using IMU (Like MPU6050)?

Question

So I have been working on a drone project for a very long time, now I decided to work on Kalman filter which is used widely nowadays like Ardupilot. I looked at the source code so basically understand that using double integration we can get linear displacement using IMU and GPS sensor fusion with Kalman filter. But when I run simple code for sample time 0.1s, I didn't get expected result. I run the code for 1 meter and distance I get was like sometimes 768cm or sometimes 2meters or even 100m so am little nervous about the algorithm.

What is the proper way to get linear displacement using IMU ?

Answer 1

I thought for sure that there would have been a duplicate question somewhere on the site that answers this question, but I can't find one, so here's a quick description of the method.

1. Put your IMU in a known starting position and orientation (position + orientation = "pose").
2. Capture IMU accelerometer and gyroscope readings.
3. Use numeric integration on the gyroscope output (angle += gyroReading*deltaTime) to get the current orientation of the IMU.
4. Use the current orientation of the IMU to construct a rotation matrix that will transform the accelerometer readings from the IMU "body frame" of reference to the "world frame" of reference.
5. Use numeric integration on the transformed accelerometer output (speed += xfmAccelerometerReading*deltaTime) to get the current speed of the IMU in the world frame.
6. Use numeric integration on the world-frame speed (position += speed*deltaTime, or position += speed*deltaTime + 0.5*xfmAccelerometerReading*deltaTime*deltaTime) to get the current position of the IMU in the world frame.
7. GOTO: 2.

This is the most simplistic way of using an IMU output to get position. All sensors have a bias, though, so when you integrate the output you're left with a drift on the speed, position, and orientation estimates (important because they are estimates and not measurements).

You can use the gravitational "down" vector (the only sustainable long-term acceleration) to correct any drift on your x/y rotations. You can use a magnetometer/compass to correct any drift on your z rotation. You can use GPS to correct position drift.

I am a huge fan of the Madgwick filter and plug it every chance I get - it's free, open-source, outperforms the Kalman filter, and it's probably already written for you. It will take accelerometer, gyro, and magnetometer readings and give you the outputs.

Answer 2

I can't comment in this forum yet, so I have to post an answer, but I am looking at the same problem. The MPU9250 DMP output includes a quaternion and a fused quaternion. I can't find an exact description of this, but I am guessing this is the rotation from body frame to world frame. (Quaternions do rotations like a rotation matrix, only the numerical accuracy is better and they don't have gimbal lock.)

I've also noticed that the IMU data takes a while to stabilize. I start my robot stable and pointing due north and the magnetometers report it is pointing south at first. Within about 10 seconds the raw magnetometer readings swing north. The fused compass reading can take 60 seconds or more to fully agree. I was very confused until I figured this out, but now I can get reliable compass readings. I still have not got good world frame position output and will look at the Madgwick filter. Thanks for that pointer.

The integrations are easy, but you need to subtract gravity. Ultimately that will degrade accuracy because you can never orient the gravity vector perfectly and error from that will accumulate. An IMU cannot keep accurate position for long, but it should be accurate for some period of time.