#!/usr/bin/env python # coding: utf-8 # # Inertial Measurement Unit (IMU) # # Kevin J. Walchko, 12 July 2017 # # --- # # IMUs are key sensors in Inertial Navigation Systems (INS). INS is key for aircraft, ships, cruise missiles, ICBMs, etc to travel long distances and arrive at a location where we want them. Although the mathematical equations behind an INS is a little complex, the sensors feeding an INS need to be calibrated in order to get good results. Today, a lot of devices have built into them IMU's. The IMU we will use is actually one for a cell phone and costs around $15. # # When I was a grad student at UF, I was developing an INS for a robotic system. I was given a MEMS IMU which had a worse performance than the one we are using today. My IMU cost around $5000 back in the mid-to-late 1990's. # # ## References # # - [Wikipedia MEMS](https://en.wikipedia.org/wiki/Microelectromechanical_systems) # - [Simple Calibration Routine](https://github.com/kriswiner/MPU6050/wiki/Simple-and-Effective-Magnetometer-Calibration) # - [Calibration Routine](http://www.sensorsmag.com/components/compensating-for-tilt-hard-iron-and-soft-iron-effects) # - [Hard/Soft Iron Effects](https://www.phidgets.com/docs/Magnetometer_Primer) # - [Wikipedia: Earth's magnetic field](https://en.wikipedia.org/wiki/Earth%27s_magnetic_field) # - [NASA: Earth's Pole Reversal](https://www.nasa.gov/topics/earth/features/2012-poleReversal.html) # - [Wikipedia: Tesla (unit)](https://en.wikipedia.org/wiki/Tesla_(unit)) # - [C version](https://github.com/TobiasSimon/MadgwickTests/blob/master/MadgwickAHRS.c) # # ## Setup # In[1]: get_ipython().run_line_magic('matplotlib', 'inline') # In[2]: from __future__ import print_function from __future__ import division import numpy as np from the_collector import BagReader from mpl_toolkits.mplot3d import Axes3D from matplotlib import pyplot as plt from math import sin, cos, atan2, pi, sqrt, asin from math import radians as deg2rad from math import degrees as rad2deg # # NXP IMU # # ![](pics/imu.jpg) # # Our inertial measurement unit (IMU) contains 2 main chips: # # ### FXOS8700 3-Axis Accelerometer/Magnetometer # # - ±2 g/±4 g/±8 g adjustable acceleration range # - ±1200 µT magnetic sensor range # - Output data rates (ODR) from 1.563 Hz to 800 Hz # - 14-bit ADC resolution for acceleration measurements # - 16-bit ADC resolution for magnetic measurements # # ### FXAS21002 3-Axis Gyroscope # # - ±250/500/1000/2000°/s configurable range # - Output Data Rates (ODR) from 12.5 to 800 Hz # - 16-bit digital output resolution # # # Micoelectromechanical Systems (MEMS) # # MEMS is the technology of microscopic devices, particularly those with moving parts. It merges at the nano-scale into nanoelectromechanical systems (NEMS) and nanotechnology. MEMS are also referred to as micromachines in Japan, or micro systems technology (MST) in Europe. Basically, using technology for microprocessor production, companies are able to produce microscopic mechanical devices. # # Our Inertial Measurement Unit is capable of measuring acceleration and the magnetic field of the Earth. Shown below, NXP produced a small mechanical device. Basically, as gravity pulls on the proof mass, the capaticence of C1 and C2 changes. When properly calibrated, the sensor is able to determine the amount of gravity from the capacitence change. # # ![How it works.](pics/mems-works.jpg) # # Below shows what it looks like under an electron microscope. # # ![Viewed under an electron microscope.](pics/mems-microscope.jpg) # # Here we will take sensor measurements and determine sensor biases to properly calibrate the sensor. # # # Accelerometers # # For the next series of images, imagine the a gross simpification of the above discussion. A proof mass in magically suspended inside a box. The walls of the box are able to measure the amount of force applied to them. The proof mass will only displace from the center under the influence of an external acceleration. # # ![](pics/right_hand_axes.jpg) # # Given a righthand coordinate system (e.g., standard cartesian coordinate system), the sensors measure acceleration as follows: # # ![](pics/center.png) # # No gravity or acceleration acting on the device. Probably this is in space far from any celestial bodies) or the device is in free fall (terminal velocity). # # ![](pics/x-acceleration.png) # # Now the device is accelerating in the posative x-direction with no gravity present. Notice how the proof mass is being pulled by gravity and pressing against the negative x-axis? Thus the IMU reads the reaction (equal and opposite) of what is happening. # # ![](pics/gravity.png) # # Just like before, there is only one acceleration acting on the device, gravity, and we measure an acceleration in the -Z direction. Typically people talk in terms of g's, this is mainly to stay away from the annoying issue of are we talking SI units ($9.81 m/sec^2$) or imperial units ($32 ft/sec^2$). # # ![](pics/xz-acceleration.png) # # Finally, we have oriented our divice $45^\circ$ up, and gravity is forcing the mass equally on the -x and -z axes. # # ## Accel Calibration # # Manufactures try to produce sensors that perform well, but when you are making millions of them and lowest cost is the is a critical factor in determining if someone will buy it, your sensor won't be perfect. Typically, you want to run the accels through a variety of static tests, where you orient them # # # Gyroscopes # # MEMS gyroscopes contain a pair of masses that are driven to oscillate with equal amplitude but in opposite directions. When rotated, the Coriolis force creates an orthogonal vibration that can be sensed by a variety of mechanisms. # # $$ # F = 2 M v \times \Omega # $$ # # where $F$ is the force, $M$ is the mass, $v$ is the velocity of the mass, and $\Omega$ is the angular velocity. # # # Magnetometers # # ## Earth's Magnetic Field # # The intensity of the field is often measured in gauss (G), but is generally reported in nanoteslas (nT), with 1 G = 100,000 nT. A nanotesla is also referred to as a gamma ($\gamma$). The tesla is the SI unit of the Magnetic field, B. The Earth's field ranges between approximately 25,000 and 65,000 nT (0.25–0.65 G or 25-65 $\mu$T). By comparison, a strong refrigerator magnet has a field of about 10,000,000 nanoteslas (100 G). # # | Prefix | Symbol | Decimal | # |--------|--------|-----------| # | milli | m | $10^{-3}$ | # | micro | $\mu$ | $10^{-6}$ | # | nano | n | $10^{-9}$ | # # ## Geographical Variation of the Field # # ![](pics/world_field_map.jpg) # # # ## Temperal Variation of the Field # # The Earth's magnetic field is always changing and often change every 200k - 300k years. # # ![](pics/earth_magnetic_field.gif) # # ## Noise # # ### Hard Iron Interference # # So called hard iron interference is caused by static magnetic fields associated with the enviornment. For example, this could include any minor (or major) magnetism in the metal chassis or frame of a vehicle, any actual magnets such as speakers, etc... This interference pattern is unique to the environment but is constant. If you have your compass in an enclosure that is held together with metal screws even these relatively small amounts of ferromagnetic material can cause issues. If we consider the magnetic data circle, hard iron interference has the effect of shifting the entire circle away from the origin by some amount. The amount is dependent on any number of different factors and can be very large. The important part is that this shift is the same for all points in time so it can be calibrated out very easily with a numeric offset which is taken care of by the calibration process # # # To compensate and recenter, for each axis (x,y,z), we will calculate the mean offset ($\alpha$): # # $$ # \alpha_x = \frac{x_{max} + x_{min}}{2} \\ # mag_{corrected} = mag_{raw} - \alpha_x # $$ # # ### Soft Iron Interference # # Soft iron interference is caused by distortion of the Earth's magnetic field due to materials in the environment. Think of it like electricity - the magnetic field is looking for the easiest path to get to where it is going. Since magnetic fields can flow more easily through ferromagnetic materials than air, more of the field will flow through the ferromagnetic material than you would expect if it were just air. This distortion effect causes the magnetic field lines to be bent sometimes quite a bit. Note that unlike hard iron interference which is the result of materials which actually have a magnetic field of their own, soft iron interference is caused by non-magnetic materials distorting the Earth's magnetic field. This type of interference has a squishing effect on the magnetic data circle turning it into more of an ellipsoid shape. The distortion in this case depends on the direction that the compass is facing. Because of this, the distortion cannot be calibrated out with a simple offset, more complicated math will still let the compass account for this type of interference though. # # Cell Phones # # The coordinate systems for cell phones are strange. They don't follow normal aerospace coordinate system definitions for doing INS. This is probably because CompSci types don't understand the math and only think about GUI programming where the axes are typically oriented different. # # Some IMUs, if you spend a little more money, come with Kalman Filters built into them. Understanding how they are setup can be confusing for someone with an aerospace/INS background and not an Andriod/Windows background. Any ways, typical definitions for cell phones IMUs are: # # ![](pics/axis_device.png) # # ![](pics/Yaw-Pitch-Roll-Slate-Win8.png) # # Our IMU does not have this issue, but in the future, as cell phone/laptop/tablet IMU become cheaper and more common, it is good information to understand becuase you may be using one on your robot. # In[3]: def normalize(x, y, z): """Return a unit vector""" norm = sqrt(x * x + y * y + z * z) if norm > 0.0: inorm = 1/norm x *= inorm y *= inorm z *= inorm else: raise Exception('division by zero: {} {} {}'.format(x, y, z)) return (x, y, z) # In[30]: def plotArray(g, dt=None, title=None): """ Plots the x, y, and z components of a sensor. In: title - what you want to name something [[x,y,z],[x,y,z],[x,y,z], ...] Out: None """ x = [] y = [] z = [] for d in g: x.append(d[0]) y.append(d[1]) z.append(d[2]) plt.subplot(3,1,1) plt.plot(x) plt.ylabel('x') plt.grid(True) if title: plt.title(title) plt.subplot(3,1,2) plt.plot(y) plt.ylabel('y') plt.grid(True) plt.subplot(3,1,3) plt.plot(z) plt.ylabel('z') plt.grid(True) # In[5]: def getOrientation(accel, mag, deg=True): ax, ay, az = normalize(*accel) mx, my, mz = normalize(*mag) roll = atan2(ay, az) pitch = atan2(-ax, ay*sin(roll)+az*cos(roll)) heading = atan2( mz*sin(roll) - my*cos(roll), mx*cos(pitch) + my*sin(pitch)*sin(roll) + mz*sin(pitch)*cos(roll) ) if deg: roll *= 180/pi pitch *= 180/pi heading *= 180/pi heading = heading if heading >= 0.0 else 360 + heading heading = heading if heading <= 360 else heading - 360 else: heading = heading if heading >= 0.0 else 2*pi + heading heading = heading if heading <= 2*pi else heading - 2*pi return (roll, pitch, heading) # In[6]: def find_calibration(mag): """ Go through the raw data and find the max/min for x, y, z """ max_m = [-1000]*3 min_m = [1000]*3 for m in mag: for i in range(3): max_m[i] = m[i] if m[i] > max_m[i] else max_m[i] min_m[i] = m[i] if m[i] < min_m[i] else min_m[i] bias = [0]*3 for i in range(3): bias[i] = (max_m[i] + min_m[i])/2 return bias def apply_calibration(data, bias): """ Given the data and the bias, correct the data """ c_data = [] for d in data: t = [] for i in [0,1,2]: t.append(d[i]-bias[i]) c_data.append(t) return c_data # In[37]: def split_xyz(data): """ Break out the x, y, and z into it's own array for plotting """ xx = [] yy = [] zz = [] for v in data: xx.append(v[0]) yy.append(v[1]) zz.append(v[2]) return xx, yy, zz def plotMagnetometer3D(data, title=None): x,y,z = split_xyz(data) fig = plt.figure() ax = fig.gca(projection='3d') ax.plot(x, y, z, '.b'); ax.set_xlabel('$\mu$T') ax.set_ylabel('$\mu$T') ax.set_zlabel('$\mu$T') if title: plt.title(title); def plotMagnetometer(data, title=None): x,y,z = split_xyz(data) plt.plot(x,y,'.b', x,z,'.r', z,y, '.g') plt.xlabel('$\mu$T') plt.ylabel('$\mu$T') plt.grid(True); plt.legend(['x', 'y', 'z']) if title: plt.title(title); # # Run Raw Compass Performance # # First lets tumble around the imu and grab lots of data in ALL orientations. # In[11]: bag = BagReader() bag.use_compression = True cal = bag.load('imu-1-2.json') # In[24]: def split(data): ret = [] rdt = [] start = data[0][1] for d, ts in data: ret.append(d) rdt.append(ts - start) return ret, rdt # In[25]: accel, adt = split(cal['accel']) mag, mdt = split(cal['mag']) gyro, gdt = split(cal['gyro']) # In[26]: plotArray(accel, 'Accel [g]') # In[ ]: # In[27]: plotArray(mag, 'Mag [uT]') # In[28]: plotArray(gyro, 'Gyros [dps]') # In[38]: # now, ideally these should be an ellipsoid centered around 0.0 # but they aren't ... need to fix the bias (offset) plotMagnetometer(mag, 'raw mag') plotMagnetometer3D(mag, 'raw mag') # In[35]: # so let's find the bias needed to correct the imu bias = find_calibration(mag) print('bias', bias) # In[36]: # now the data should be nicely centered around (0,0,0) cm = apply_calibration(mag, bias) plotMagnetometer(cm, 'corrected mag') plotMagnetometer3D(cm, 'corrected mag') # Now using this bias, we should get better performance. # # Check Calibration # In[77]: # read in new data and break it out cal_check = read('check_cal.dat', ['imu']) a, m, g, ts = breakout_imu(cal_check) # apply correction in previous step cm = apply_calibration(m, bias) plotMagnetometer(cm) # In[79]: # Now let's run through the data and correct it roll = [] pitch = [] heading = [] for accel, mag in zip(a, cm): r,p,h = getOrientation(accel, mag) roll.append(r) pitch.append(p) heading.append(h) x_scale = [x-ts[0] for x in ts] print('timestep', ts[1] - ts[0]) plt.subplot(2,2,1) plt.plot(x_scale, roll) plt.grid(True) plt.title('Roll') plt.subplot(2,2,2) plt.plot(x_scale, pitch) plt.grid(True) plt.title('Pitch') plt.subplot(2,2,3) plt.plot(x_scale, heading) plt.grid(True) plt.title('Heading'); # Now, this data was acquired with the imu starting off flat and slow rotated 360 degrees with stops around 90, 180, 270 from the starting position. At the end, I started wobbling/nutating (don't know how else to describe it) so you see the roll and pitch jump up ... there was some inadvertant change to heading during the end. # # Now looking at the results, you can see I didn't start at 0 deg and there is approximately 4 movements about 90 degrees appart ... which is what I did. # # ----------- # # Creative Commons License
This work is licensed under a Creative Commons Attribution-ShareAlike 4.0 International License.