Post blog post for magnetometer calibration

source/_posts/magnetometer_calibration.md

@@ -0,0 +1,232 @@
+---
+title: PSAS Magnetometer Calibration
+author: Nathan
+date: 2019/11/05
+---
+For a number of years I was involved with a university rocketry club called PSAS{% sidenote %}[Portland State Aerospace Society](http://psas.pdx.edu), a student aerospace engineering project at Portland State University. They build ultra-low-cost, open source rockets that feature very sophisticated amateur rocket avionics systems.{% endsidenote %}. One of the things I really liked to do was play with the data from the launches and learn how rockets and flight electronics work.
+
+
+Our rockets carry an instrument on them called an **IMU** (Inertial Measument Unit). An IMU typically measures both acceleration and rotation-rate of an object in all directions so with some clever math you can recreate the exact position, velocity, and orientation of the rocket over time. This is the only way to know where something is in space, and very important for rockets. IMUs have a problem though: they're not very precise.
+
+
+Since our IMU is fixed to the rocket, {% marginnote %}![diagram of the rocket on it's side showing the layout of the internal components](img/L-12_overview.png) Overview of the rocket "LV2.3". The IMU is near the primary flight computer.{% endmarginnote %} which direction is "up" or "left", etc. relative to the Earth changes constantly as the rocket flies about. In order for the data to be useful we need to know which way we are pointed, which is why IMUs alway have some kind of gryoscope to account for rotation. Our particular IMU has rate-gyroscopes that can sense rotation rate, and so we integrate that once to get orientation. Since any integration will give an estimate that drifts from the true value over time, our IMU also includes a 3-axis _magnetometer_ as well.
+
+## 9DOF IMU
+
+This makes what is often what is refered to as a "9DOF" IMU, because it has "nine degrees of freedom". That would be _x, y, z_ accleration, _x, y, z_ rotation-rate, and _x, y, z_ magnetic field. The reason to have a magnetometer is so you can use Earth's own magnetic field as a kind of guide to the orientation of the rocket. This doesn't instantly solve all problems in life, sadly. But it provides a good reference for the rough orientation of the rocket that can be used to produce a real-time estimate of rate-gyroscape drift, or 'bias', as we fly.
+
+The magnetic field sensor in the rocket is sensitive, but because the Earth's field is so weak it's easily overwhelmed by local effects (metal screws, magnetic fields from nearby wires, etc.). In order to get good orientation data we need to undo{% marginnote %}![photo of two men awkwardly holding a large rocket body and an angle](img/L-12_ground_calibration.jpg) Members of the PSAS ground crew lifting and aranging the rocket around as many different orientations as possible before the flight.{% endmarginnote %} these local effects.
+
+
+So a little before the flight we took the nearly complete rocket, powered the electronics up, picked it up and tried to move it around in every direction.
+
+## Magnetometer Calibration
+
+What do we expect good magnetometer data to look like? The Earth's magnetic field shouldn't change much, so it should look like a single vector going through the IMU. If we rotate the rocket one way or another, the angel that the vector goes through will change, but it should stay the same strength. That means that the magnitude of the local magnetic filed should be constant, and it should measure it to be exactly the same as Earth's magnetic field.
+
+
+
+## Earth's Field Strength
+
+
+But what is the strength of Earth's magnetic field? It varies over time and over the surface of the Earth. We know where we launched from{% sidenote %}
+Latitude: `43.79613280°` N
+Longitude: `120.65175340°` W
+Elevation: `1390.0` m Mean Sea Level
+{% endsidenote %} and the date, so we can look up{% sidenote %}[NOAA's magnetic field calculator](https://www.ngdc.noaa.gov/geomag/magfield.shtml)
+Model Used: `WMM2015`
+ {% endsidenote %} what the expected magnetic field should be:
+
+Its direction:
+
+| Declination (+E/-W) | Inclination (+D/-U) | Horizontal Intensity |
+| ------------------: | ------------------: | -------------------: | 
+|     14.7990° ±0.36° |      66.5386° ±0.22 |   20,754.1 nT ±133 nT| 
+
+
+
+And as a vector:
+
+|   North Comp (+N/-S) | East Comp (+E/-W) | Vertical Comp (+D/-U) |
+| -------------------: | ----------------: | --------------------: |
+|  20,065.7 nT ±138 nT | 5,301.2 nT ±89 nT |   47,819.4 nT ±165 nT |
+
+
+And finally, the total strength:
+
+|         Total Field |
+| ------------------: |
+| 52,129.0 nT ±152 nT | 
+
+
+## Calibration Data
+
+That's what we _expect_ to see. What do we actually get?
+
+
+
+
+In the 22.1 minutes that we had the flight computer collecting data in our calibration run we recoreded 1,079,342 data points from the IMU.
+
+
+
+Looking over time at the _x, y, z_ values of the magnetometer and the mangitue compared to the NOAA predicted field we see it vary a lot.
+
+
+<figure class="fullwidth">
+  <img src="post_files/post_9_0.png">
+</figure>
+
+
+This is because we have a couple of problems. One is that the effective _center_ of our magnetometer values are pushed off to one side. And the other is that the values are skewed (or "stretched") off to one side as well. This is somewhat easier to see in 3D:
+
+
+<figure class="">
+  <img src="post_files/post_11_0.png">
+</figure>
+
+
+## Correction
+
+The two parts of the correction are called "Hard Iron" (fixed center offset) and "Soft Iron" (streched sphere) corrections.
+
+### Hard Iron
+
+This is the simpler of the two, one can essentially find the midrange value of across the entire calibration dataset and subtract that offset to move the '0' point back to center.
+
+### Soft Iron
+
+Finding the soft iron correction is a bit trickier because we want to fit an matching elongated ellipsoid to the data, and then once we have an approximation for that ellipsoid apply stretch to the data to undo the elongation and get it back to a sphere. Luckily an algorithm for this has been worked out. For a detailed breakdown see
+
+https://teslabs.com/articles/magnetometer-calibration/
+
+After doing fitting we end up with both a correction matrix and an offset vector. This is both the soft iron and hard iron correction.
+
+To invert the stretch we multiply a vector representing each magnetometer reading (a 'sample', $\vec s$) by the correction matrix (after subtracting the center offset offset).
+
+$$
+\vec s_\textrm{corrected} = \mathbf{A} \cdot (\vec s - \vec b)
+$$
+
+Where
+
+ - $\vec s_\textrm{corrected}$ is a fully corrected sample at time t
+ - $\mathbf{A}$ is our soft iron correction matrix
+ - $\vec s$ is a raw sample from the IMU at time t
+ - $\vec b$ our hard iron offset vector.
+
+When we solve for $\mathbf{A}$ on the calibration data we get the matrix:
+
+
+
+
+
+$$
+\textbf{A} = \left[\begin{array}{ccc}
+ 0.870368 & -0.128543 & -0.283684 \\\\
+ -0.128543 & 1.510386 & -0.046543 \\\\
+ -0.283684 & -0.046543 & 1.440805
+\end{array}\right]
+$$
+
+
+
+
+And a hard iron offset vector:
+
+
+
+
+
+$$
+\vec b = \left[ \begin{array}{ccc}
+-12.019415 & -3.209783 & -1.939041
+\end{array}\right]
+$$
+
+
+
+
+## Python
+
+If we want to apply this correction we can make a convienient function to call on all our samples:
+
+
+
+
+
+``` python
+import numpy as np
+
+MAG_CORRECTION_A = np.array((
+    ( 0.870367858077, -0.128543320363, -0.283683583608),
+    (-0.128543320363,  1.510386103995, -0.046543028701),
+    (-0.283683583608, -0.046543028701,  1.440804950101),
+))
+
+MAG_CORRECTION_b = np.array((-12.019414737824, -3.209782771540, -1.939040882716))
+
+def apply_mag_correction(sample):
+    """Take a sample and correct for hard and soft iron effects
+    based on calibration run before Launch 12.
+    
+    :param sample: Vector (np.array) (x,y,z) instantaintous magnetometer reading
+    :returns: vector (x,y,z) corrected magnetometer reading
+    """
+    
+    return np.dot(MAG_CORRECTION_A, sample + MAG_CORRECTION_b)
+```
+
+
+
+
+## Apply Calibration
+
+After we apply the calibration fix above, do we get a better result?
+
+
+<figure class="fullwidth">
+  <img src="post_files/post_24_0.png">
+</figure>
+
+
+Yes! Quite a bit better. Notice how the magnitue of the vector now stays very close to constant and is very close to the NOAA estimate!
+
+Again in 3D we can see a much closer to spherical data:
+
+
+<figure class="">
+  <img src="post_files/post_26_0.png">
+</figure>
+
+
+## Applying to Flight Data
+
+We can now take our calibration matrix and apply it to real flight data! Here is a 3D look at the [Launch-12](https://github.com/psas/Launch-12) **raw** (uncalibrated) magnetometer data from liftoff to apogee:
+
+
+<figure class="">
+  <img src="post_files/post_29_0.png">
+</figure>
+
+
+The rocket spins during flight, and we see the magnetic field measurement spiral around the plots. We also see the familiar stretch and offset that we saw in the calibration data.
+
+
+### Calibrated Flight Data
+
+So now lets calibrate the flight data!
+
+
+<figure class="fullwidth">
+  <img src="post_files/post_32_0.png">
+</figure>
+
+
+And it looks like the calibration did a reasonable job. The values now come very close to landing on the nominal Earth field sphere. The XY view is still off a little bit but it might just be that we had some bias in the calibration run. It's still a huge improvment to the original dataset and it now usable in IMU reconstructions of the flight of the rocket.
+
+----------------------------------------------
+
+This post is written as a [jupyter notebook](https://jupyter.org/) and all its code and data can be viewed as a stand-alone own project here:
+
+https://git.natronics.org/natronics/psas-magnetometer-calibration