Package: imusensor
imusensorThe 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-requisitesSome of the requirements are to enable I2C in rpi.
Installing I2C tools and smbussudoapt-getinstalli2c-tools
sudopipinstallsmbusConnect the MPU9250 with rpi using the below connectionsRpi pinMPU9250 pinspin 3 ->SDA pinpin 5 ->SCL pinpin 6 ->Ground(GND)pin 1 ->VCCAfter you have made the connections, type the following command -sudoi2cdetect-y1If 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 UsageThe below code is a basic starter for the libraryimportosimportsysimporttimeimportsmbusfromimusensor.MPU9250importMPU9250address=0x68bus=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")whileTrue:imu.readSensor()imu.computeOrientation()print("roll:{0}; pitch :{1}; yaw :{2}".format(imu.roll,imu.pitch,imu.yaw))time.sleep(0.1)Other FunctionalitiesSetting Accelerometer RangeThe accelerometer in MPU9250 has the following ranges of +-2g, +-4g, +-8g and +-16gYou can set this setting by the below commandimu.setAccelRange("AccelRangeSelect2G")Simiarly for 4g use "AccelRangeSelect4G" and follow similary for 8g and 16g ranges.Setting Gyroscope RangeGyroscope sensor in MPU9250 has the following ranges +-250DPS, +-500DPS, +-1000DPS and +-2000DPSYou can set this setting by the below commandimu.setGyroRange("GyroRangeSelect250DPS")Simiarly for 500DPS use "GyroRangeSelect500DPS" and follow similary for 1000DPS and 2000DPS ranges.Note:DPS means degrees per secondSetting internal low pass filter frequencyThe sensor has an internal low pass filter to remove some basic noise in the values generated by accelerometer and gyrscope.Use the following commandimu.setLowPassFilterFrequency(AccelLowPassFilter184)frequencystr5HzAccelLowPassFilter510HzAccelLowPassFilter1020HzAccelLowPassFilter2041HzAccelLowPassFilter4192HzAccelLowPassFilter92184HzAccelLowPassFilter184Gyroscope CaliberationThough 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 inimu.GyroBiasYou can also set its value, but make sure you give 3x1 numpy array.Accelerometer CaliberationThis caliberation includes an extra parameter called scale apart from bias. Use the below commandimu.caliberateAccelerometer()The above function will store the scale and bias in the following variablesimu.Accelsandimu.AccelBiasrespectively.Magnometer CaliberationThis has two types of caliberationimu.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 inimu.Magsandimu.MagBiasrespectively.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 isimu.MagBiasFor more details on this, have a look at mag_caliberation folder in examples.IMU OrientationThe 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 byimu.roll,imu.pitchandimu.yaw.Note:The euler angles will only make sense when all the sensors are properly caliberated.Filters for sensorfusionOrientation 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.KalmanIt 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 examplesimportosimportsysimporttimeimportsmbusimportnumpyasnpfromimusensor.MPU9250importMPU9250fromimusensor.filtersimportkalmanaddress=0x68bus=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.rollsensorfusion.pitch=imu.pitchsensorfusion.yaw=imu.yawcount=0currTime=time.time()whileTrue:imu.readSensor()imu.computeOrientation()newTime=time.time()dt=newTime-currTimecurrTime=newTimesensorfusion.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)MadgwickThis 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 examplesimportosimportsysimporttimeimportsmbusfromimusensor.MPU9250importMPU9250fromimusensor.filtersimportmadgwicksensorfusion=madgwick.Madgwick(0.5)address=0x68bus=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=0whileTrue:imu.readSensor()foriinrange(10):newTime=time.time()dt=newTime-currTimecurrTime=newTimesensorfusion.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)ifprint_count==2:print("mad roll:{0}; mad pitch :{1}; mad yaw :{2}".format(sensorfusion.roll,sensorfusion.pitch,sensorfusion.yaw))print_count=0print_count=print_count+1time.sleep(0.01)For the detailed explanation ->linkFilter comparisonWe 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.AcknowledgmentsMost 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.