Update

Adding blog links to provide more information about the math behind the code.
IMU Intro - It gives an introduction into IMU working and the math behind calibration and basic idea behind finding roll, pitch and yaw.
Sensor Fusion - This blog goes into math behind kalman filter, Madgwick filter and how they are applied here.
Hands-on Intro - A general overview of getting started.

imusensor

The repo provides a bridge between MPU9250 and raspberry pi. It also lists various caliberation code and filters for getting an accurate orientation from MPU9250 This repo mostly concentrates on the problem of connecting IMU(MPU9250) to raspberry pi through I2C communication.

Pre-requisites

Some of the requirements are to enable I2C in rpi. Installing I2C tools and smbus

sudo apt-get install i2c-tools
sudo pip install smbus

Connect the MPU9250 with rpi using the below connections

Rpi pin	MPU9250 pins
pin 3 ->	SDA pin
pin 5 ->	SCL pin
pin 6 ->	Ground(GND)
pin 1 ->	VCC

After you have made the connections, type the following command -

sudo i2cdetect -y 1

If you see 68 in the output, then that means the sensor is connected to the rpi and 68 is the address of the sensor.

Basic Usage

The below code is a basic starter for the library

import os
import sys
import time
import smbus

from imusensor.MPU9250 import MPU9250

address = 0x68
bus = smbus.SMBus(1)
imu = MPU9250.MPU9250(bus, address)
imu.begin()
# imu.caliberateGyro()
# imu.caliberateAccelerometer()
# or load your own caliberation file
#imu.loadCalibDataFromFile("/home/pi/calib_real_bolder.json")

while True:
	imu.readSensor()
	imu.computeOrientation()

	print ("roll: {0} ; pitch : {1} ; yaw : {2}".format(imu.roll, imu.pitch, imu.yaw))
	time.sleep(0.1)

Other Functionalities

Setting Accelerometer Range

The accelerometer in MPU9250 has the following ranges of +-2g, +-4g, +-8g and +-16g
You can set this setting by the below command

imu.setAccelRange("AccelRangeSelect2G")

Simiarly for 4g use "AccelRangeSelect4G" and follow similary for 8g and 16g ranges.

Setting Gyroscope Range

Gyroscope sensor in MPU9250 has the following ranges +-250DPS, +-500DPS, +-1000DPS and +-2000DPS
You can set this setting by the below command

imu.setGyroRange("GyroRangeSelect250DPS")

Simiarly for 500DPS use "GyroRangeSelect500DPS" and follow similary for 1000DPS and 2000DPS ranges.
Note: DPS means degrees per second

Setting internal low pass filter frequency

The sensor has an internal low pass filter to remove some basic noise in the values generated by accelerometer and gyrscope.
Use the following command

imu.setLowPassFilterFrequency("AccelLowPassFilter184")

frequency	str
5Hz	AccelLowPassFilter5
10Hz	AccelLowPassFilter10
20Hz	AccelLowPassFilter20
41Hz	AccelLowPassFilter41
92Hz	AccelLowPassFilter92
184Hz	AccelLowPassFilter184

Gyroscope Caliberation

Though most sensors are caliberated during manufacturing, however, there still could be a need for caliberation due to various cahnges like being soldered to a breakout board. Gyroscope normally comes with a bias. This can be found by averaging the values when it is kept still and then subtract those values to get the appropriate values.

imu.caliberateGyro()

This will calculate the bias and it is stored in imu.GyroBias You can also set its value, but make sure you give 3x1 numpy array.

Accelerometer Caliberation

This caliberation includes an extra parameter called scale apart from bias. Use the below command

imu.caliberateAccelerometer()

The above function will store the scale and bias in the following variables imu.Accels and imu.AccelBias respectively.

Magnometer Caliberation

This has two types of caliberation

imu.caliberateMagApprox() : As the name suggests, this is a near approximation of scale and bias parameters. It saves time however, might not be always accurate. In this the scale and bias are stored in imu.Mags and imu.MagBias respectively.
imu.caliberateMagPrecise() : It tries to fit the data to an ellipsoid and is more complicated and time consuming. It gives a 3x3 symmetric transformation matrix(imu.Magtransform) instead of a common 3x1 scale values. The bias variable is imu.MagBias
For more details on this, have a look at mag_caliberation folder in examples.

IMU Orientation

The computed orientation is in terms of eurler angles. roll for x axis, pitch for y axis and yaw for z axis. We use NED format which basically means, the sensor's x-axis is aligned with north, sensor's y-axis is aligned with east and sensor's x-axis is aligned with down. imu.computeOrientation() The roll, pitch and yaw can be accessed by imu.roll, imu.pitch and imu.yaw. Note: The euler angles will only make sense when all the sensors are properly caliberated.

Filters for sensorfusion

Orientation from accelerometer and magnetometer are noisy, while estimating orientation from gyroscope is noise free but accumulates drift over time. We will combining both of these to obtain more stable orientation. There are multiple ways to do it and we have given two options of kalman and madgwick. You are free to write your own algorithms.

Kalman

It uses gyroscope to estimate the new state. Accelerometer and magnetometer provide the new measured state. The kalman filter aims to find a corrected state from the above two by assuming that both are forms of gaussian distributions. look at kalmanExample.py in examples

import os
import sys
import time
import smbus
import numpy as np

from imusensor.MPU9250 import MPU9250
from imusensor.filters import kalman 

address = 0x68
bus = smbus.SMBus(1)
imu = MPU9250.MPU9250(bus, address)
imu.begin()
# imu.caliberateAccelerometer()
# print ("Acceleration calib successful")
# imu.caliberateMag()
# print ("Mag calib successful")
# or load your caliberation file
# imu.loadCalibDataFromFile("/home/pi/calib_real_bolder.json")

sensorfusion = kalman.Kalman()

imu.readSensor()
imu.computeOrientation()
sensorfusion.roll = imu.roll
sensorfusion.pitch = imu.pitch
sensorfusion.yaw = imu.yaw

count = 0
currTime = time.time()
while True:
	imu.readSensor()
	imu.computeOrientation()
	newTime = time.time()
	dt = newTime - currTime
	currTime = newTime

	sensorfusion.computeAndUpdateRollPitchYaw(imu.AccelVals[0], imu.AccelVals[1], imu.AccelVals[2], imu.GyroVals[0], imu.GyroVals[1], imu.GyroVals[2],\
												imu.MagVals[0], imu.MagVals[1], imu.MagVals[2], dt)

	print("Kalmanroll:{0} KalmanPitch:{1} KalmanYaw:{2} ".format(sensorfusion.roll, sensorfusion.pitch, sensorfusion.yaw))

	time.sleep(0.01)

Madgwick

This is slightly better than kalman and more smooth in giving out the orientation. However, for this to work properly, the sensor fusion needs to run at least 10 times faster frequency than the sensor sampling frequency. look at madgwickExample.py in examples

import os
import sys
import time
import smbus


from imusensor.MPU9250 import MPU9250
from imusensor.filters import madgwick

sensorfusion = madgwick.Madgwick(0.5)

address = 0x68
bus = smbus.SMBus(1)
imu = MPU9250.MPU9250(bus, address)
imu.begin()

# imu.caliberateGyro()
# imu.caliberateAccelerometer()
# or load your own caliberation file
#imu.loadCalibDataFromFile("/home/pi/calib_real4.json")

currTime = time.time()
print_count = 0
while True:
	imu.readSensor()
	for i in range(10):
		newTime = time.time()
		dt = newTime - currTime
		currTime = newTime

		sensorfusion.updateRollPitchYaw(imu.AccelVals[0], imu.AccelVals[1], imu.AccelVals[2], imu.GyroVals[0], \
									imu.GyroVals[1], imu.GyroVals[2], imu.MagVals[0], imu.MagVals[1], imu.MagVals[2], dt)

	if print_count == 2:
		print ("mad roll: {0} ; mad pitch : {1} ; mad yaw : {2}".format(sensorfusion.roll, sensorfusion.pitch, sensorfusion.yaw))
		print_count = 0

	print_count = print_count + 1
	time.sleep(0.01)

For the detailed explanation -> link

Filter comparison

We have also done a small filter comparison of all the filters. This data can be streamed to your computer using zmq and also you can visualize the imu orientation using pygame_viz.py in examples/filters_comparison.

Acknowledgments

Most of the documentation for interfacing MPU9250 with arduino is present. Our work has been inspired by the following works.

bolderflight/MPU9250: This is a nice library for interfacing MPU9250 with arduino.
kriswiner/MPU9250: This is a library for getting some accurate orientation from MPU9250. The author has answered a lot of questions in the issues and most of them are very enlightening for anybody working with IMUs. Highly recommend it.
TKJElectronics/KalmanFilter : This is an implementation of second order kalman filter for IMU when using with arduino.

niru-5/imusensor