Front website https://natronics.org
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

232 lines
9.4 KiB

5 years ago
5 years ago
5 years ago
  1. ---
  2. title: PSAS Magnetometer Calibration
  3. author: Nathan
  4. date: 2019/11/05
  5. ---
  6. 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.
  7. 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.
  8. 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 always 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.
  9. ## 9DOF IMU
  10. 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.
  11. 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.
  12. 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.
  13. ## Magnetometer Calibration
  14. 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 angle that the vector goes through will change, but it should stay the same strength. That means that the magnitude of the local magnetic field should be constant, and it should measure it to be exactly the same as Earth's magnetic field.
  15. ## Earth's Field Strength
  16. 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 %}
  17. Latitude: `43.79613280°` N
  18. Longitude: `120.65175340°` W
  19. Elevation: `1390.0` m Mean Sea Level
  20. {% endsidenote %} and the date, so we can look up{% sidenote %}[NOAA's magnetic field calculator](https://www.ngdc.noaa.gov/geomag/magfield.shtml)
  21. Model Used: `WMM2015`
  22. {% endsidenote %} what the expected magnetic field should be:
  23. Its direction:
  24. | Declination (+E/-W) | Inclination (+D/-U) | Horizontal Intensity |
  25. | ------------------: | ------------------: | -------------------: |
  26. | 14.7990° ±0.36° | 66.5386° ±0.22 | 20,754.1 nT ±133 nT|
  27. And as a vector:
  28. | North Comp (+N/-S) | East Comp (+E/-W) | Vertical Comp (+D/-U) |
  29. | -------------------: | ----------------: | --------------------: |
  30. | 20,065.7 nT ±138 nT | 5,301.2 nT ±89 nT | 47,819.4 nT ±165 nT |
  31. And finally, the total strength:
  32. | Total Field |
  33. | ------------------: |
  34. | 52,129.0 nT ±152 nT |
  35. ## Calibration Data
  36. That's what we _expect_ to see. What do we actually get?
  37. 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.
  38. 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.
  39. <figure class="fullwidth">
  40. <img src="post_files/post_9_0.png">
  41. </figure>
  42. 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:
  43. <figure class="">
  44. <img src="post_files/post_11_0.png">
  45. </figure>
  46. ## Correction
  47. The two parts of the correction are called "Hard Iron" (fixed center offset) and "Soft Iron" (streched sphere) corrections.
  48. ### Hard Iron
  49. 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.
  50. ### Soft Iron
  51. 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
  52. https://teslabs.com/articles/magnetometer-calibration/
  53. 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.
  54. 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).
  55. $$
  56. \vec s_\textrm{corrected} = \mathbf{A} \cdot (\vec s - \vec b)
  57. $$
  58. Where
  59. - $\vec s_\textrm{corrected}$ is a fully corrected sample at time t
  60. - $\mathbf{A}$ is our soft iron correction matrix
  61. - $\vec s$ is a raw sample from the IMU at time t
  62. - $\vec b$ our hard iron offset vector.
  63. When we solve for $\mathbf{A}$ on the calibration data we get the matrix:
  64. $$
  65. \textbf{A} = \left[\begin{array}{ccc}
  66. 0.870368 & -0.128543 & -0.283684 \\\\
  67. -0.128543 & 1.510386 & -0.046543 \\\\
  68. -0.283684 & -0.046543 & 1.440805
  69. \end{array}\right]
  70. $$
  71. And a hard iron offset vector:
  72. $$
  73. \vec b = \left[ \begin{array}{ccc}
  74. -12.019415 & -3.209783 & -1.939041
  75. \end{array}\right]
  76. $$
  77. ## Python
  78. If we want to apply this correction we can make a convienient function to call on all our samples:
  79. ``` python
  80. import numpy as np
  81. MAG_CORRECTION_A = np.array((
  82. ( 0.870367858077, -0.128543320363, -0.283683583608),
  83. (-0.128543320363, 1.510386103995, -0.046543028701),
  84. (-0.283683583608, -0.046543028701, 1.440804950101),
  85. ))
  86. MAG_CORRECTION_b = np.array((-12.019414737824, -3.209782771540, -1.939040882716))
  87. def apply_mag_correction(sample):
  88. """Take a sample and correct for hard and soft iron effects
  89. based on calibration run before Launch 12.
  90. :param sample: Vector (np.array) (x,y,z) instantaintous magnetometer reading
  91. :returns: vector (x,y,z) corrected magnetometer reading
  92. """
  93. return np.dot(MAG_CORRECTION_A, sample + MAG_CORRECTION_b)
  94. ```
  95. ## Apply Calibration
  96. After we apply the calibration fix above, do we get a better result?
  97. <figure class="fullwidth">
  98. <img src="post_files/post_24_0.png">
  99. </figure>
  100. 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!
  101. Again in 3D we can see a much closer to spherical data:
  102. <figure class="">
  103. <img src="post_files/post_26_0.png">
  104. </figure>
  105. ## Applying to Flight Data
  106. 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:
  107. <figure class="">
  108. <img src="post_files/post_29_0.png">
  109. </figure>
  110. 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.
  111. ### Calibrated Flight Data
  112. So now lets calibrate the flight data!
  113. <figure class="fullwidth">
  114. <img src="post_files/post_32_0.png">
  115. </figure>
  116. 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 improvement to the original dataset and it now usable in IMU reconstructions of the flight of the rocket.
  117. ----------------------------------------------
  118. 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:
  119. https://git.natronics.org/natronics/psas-magnetometer-calibration