<SYSTEM_TASK:>
Given the following text description, write Python code to implement the functionality described below step by step
<END_TASK>
<USER_TASK:>
Description:
 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
Simple Calibration Routine
Calibration Routine
Hard/Soft Iron Effects
Wikipedia
Step6:  NXP IMU
Our inertial measurement unit (IMU) contains 2 main chips
Step7:  Run Raw Compass Performance
First lets tumble around the imu and grab lots of data in ALL orientations.
Step8:  Now using this bias, we should get better performance.
Check Calibration
<ASSISTANT_TASK:>
Python Code:
%matplotlib inline
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
Explanation: 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
Simple Calibration Routine
Calibration Routine
Hard/Soft Iron Effects
Wikipedia: Earth's magnetic field
NASA: Earth's Pole Reversal
Wikipedia: Tesla (unit)
C version
Setup
End of explanation
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)
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)
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)
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
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);
Explanation: NXP IMU
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.
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.
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.
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.
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
Temperal Variation of the Field
The Earth's magnetic field is always changing and often change every 200k - 300k years.
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:
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.
End of explanation
bag = BagReader()
bag.use_compression = True
cal = bag.load('imu-1-2.json')
def split(data):
    ret = []
    rdt = []
    start = data[0][1]
    for d, ts in data:
        ret.append(d)
        rdt.append(ts - start)
    return ret, rdt
accel, adt = split(cal['accel'])
mag, mdt = split(cal['mag'])
gyro, gdt = split(cal['gyro'])
plotArray(accel, 'Accel [g]')
plotArray(mag, 'Mag [uT]')
plotArray(gyro, 'Gyros [dps]')
# 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')
# so let's find the bias needed to correct the imu
bias = find_calibration(mag)
print('bias', bias)
# now the data should be nicely centered around (0,0,0)
cm = apply_calibration(mag, bias)
plotMagnetometer(cm, 'corrected mag')
plotMagnetometer3D(cm, 'corrected mag')
Explanation: Run Raw Compass Performance
First lets tumble around the imu and grab lots of data in ALL orientations.
End of explanation
# apply correction in previous step
cm = apply_calibration(mag, bias)
plotMagnetometer(cm)
# 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');
Explanation: Now using this bias, we should get better performance.
Check Calibration
End of explanation
<END_TASK>