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.
In [1]:
%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
Our inertial measurement unit (IMU) contains 2 main chips:
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.
Below shows what it looks like under an electron microscope.
Here we will take sensor measurements and determine sensor biases to properly calibrate the sensor.
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.
Given a righthand coordinate system (e.g., standard cartesian coordinate system), the sensors measure acceleration as follows:
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).
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.
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$).
Finally, we have oriented our divice $45^\circ$ up, and gravity is forcing the mass equally on the -x and -z axes.
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
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.
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}$ |
The Earth's magnetic field is always changing and often change every 200k - 300k years.
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 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.
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:
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 [2]:
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);
In [4]:
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.
In [3]:
# apply correction in previous step
cm = apply_calibration(mag, 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.
This work is licensed under a Creative Commons Attribution-ShareAlike 4.0 International License.