# 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)? – sempaiscuba Nov 27 '18 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 – Robokishan Nov 28 '18 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 ??? – Robokishan Nov 30 '18 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 Nov 30 '18 at 13:31
• okay got it thank you seems like its the only option – Robokishan Dec 2 '18 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 – Brandon Gorman Mar 19 '19 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 Mar 20 '19 at 20:49