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

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 ?

• Welcome to Robotics:SE. What code are you running? Can you edit your question to add the code (and ideally your wiring diagram)? Commented Nov 27, 2018 at 20:36
• yeah sure, its simple i2c protocol wire library with stm32f103 (blue pill) connected to mpu6050. and the code is very basic right now just reading the registers of mpu6050 to get accelerometer data. and then acceleration to displacement Commented Nov 28, 2018 at 18:22

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.

• okey thanks but its not working out since jts only giving me displacement at the specific time not the entire path can you workout me on this ??? Commented Nov 30, 2018 at 7:37
• @Robokishan - The current position is generally the only thing you get from an IMU. If you want to record the path, then you need to record the path. Something like pathHistory(:, end+1) = position; or pathHistory.emplace_back(position);, etc. You could preallocate the pathHistory variable, but then you'd need to know how long you're going to record the path.
– Chuck
Commented Nov 30, 2018 at 13:31
• okay got it thank you seems like its the only option Commented Dec 2, 2018 at 16:48
• I am trying to follow step 4 but are confused. How do I apply step 4? I am trying to implement this in C++. Please help Commented Mar 19, 2019 at 17:10
• @BrandonGorman - Step 4 requires the output of Step 3, which is the orientation of IMU. Do you have the orientation? Is your question how to convert Euler angles (or quaternions) to a rotation matrix?
– Chuck
Commented Mar 20, 2019 at 20:49

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.