jlpoole/Magnetometer_Calibration
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions
Magnetometer_Calibration/mag_calibration.ipynb

812 lines
No EOL
27 KiB
Text
Raw Permalink Blame History

{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Hard- and Soft-Iron Magnetometer Calibration Visualized"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"This is a step by step walk through magnetometer calibration.\n",
"\n",
"The math backgroud can be found here:\n",
"* [AN4248](https://cache.freescale.com/files/sensors/doc/app_note/AN4248.pdf) (Note that I use the rotation sequence Z - X - Y and not Z - Y - X as in this paper)\n",
"* [AN4246](https://www.nxp.com/docs/en/application-note/AN4246.pdf)\n",
"* [Least square ellipsoid fitting using iterative orthogonal transformations](https://arxiv.org/pdf/1704.04877.pdf)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Setup\n",
"First import all necessary packages. You may have to install a few packages before hand. In Ubuntu-Linux the commands are:\n",
"```ssh\n",
"$ sudo apt install python3-numpy\n",
"$ sudo apt install python3-pandas\n",
"# only needed if you want to read measurements from your micro controller\n",
"$ sudo apt install python3-serial\n",
"$ sudo -H pip3 install cobs\n",
"```"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"%matplotlib inline\n",
"import numpy as np\n",
"import numpy.linalg as linalg\n",
"import matplotlib.pyplot as plt\n",
"from mpl_toolkits.mplot3d import Axes3D\n",
"import serial\n",
"import sys\n",
"import pandas as pd\n",
"from cobs import cobs\n",
"import msgpack\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Define a plotting function"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# from https://stackoverflow.com/questions/7819498/plotting-ellipsoid-with-matplotlib\n",
"def plot_ellipsoid_and_measurements(A, c, xm, ym, zm):\n",
" # find the rotation matrix and radii of the axes\n",
" U, s, rotation = linalg.svd(A)\n",
" radii = 1.0/np.sqrt(s)\n",
"\n",
" u = np.linspace(0.0, 2.0 * np.pi, 100)\n",
" v = np.linspace(0.0, np.pi, 100)\n",
" x = radii[0] * np.outer(np.cos(u), np.sin(v))\n",
" y = radii[1] * np.outer(np.sin(u), np.sin(v))\n",
" z = radii[2] * np.outer(np.ones_like(u), np.cos(v))\n",
" for i in range(len(x)):\n",
" for j in range(len(x)):\n",
" [x[i,j],y[i,j],z[i,j]] = np.dot([x[i,j],y[i,j],z[i,j]], rotation) + c\n",
"\n",
" # plot\n",
" fig = plt.figure(figsize=plt.figaspect(1) * 1.5) # adapt factor according your window width\n",
" ax = fig.add_subplot(111, projection='3d')\n",
" ax.scatter(xm, ym, zm, s=0.1, c='r', alpha=0.5) # plot measurements\n",
" ax.plot_wireframe(x, y, z, rstride=4, cstride=4, color='b', alpha=0.2) # plot ellipsoid\n",
" \n",
" # scale axes equally\n",
" max_value = max(radii[0], radii[1], radii[2], max(xm), max(ym), max(zm))\n",
" for axis in 'xyz':\n",
" getattr(ax, 'set_{}lim'.format(axis))((-max_value, max_value))\n",
" \n",
" ax.set_xlabel('x')\n",
" ax.set_ylabel('y')\n",
" ax.set_zlabel('z')\n",
"\n",
" fig.tight_layout()\n",
" plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Read Measurements from Micro Controller\n",
"If you want to do this with your own data, this script helps you to read data from a serial port. Just set the port and the baud rate accordingly.\n",
"Alternatively you can use my saved measurements (see next section). But then -of course- you calibrate my magnetometer, not yours.\n",
"\n",
"For this you need a micro controller that sends the measured data via a serial connection. To do it right serialize the data into a msgpack array, encode it with COBS and terminate a frame with 0. The array should contain the measurements of the 3 magnetometer axis (in gauss), the 3 accelerometer axis (in m/s\u00b2) and the 3 gyroscopes (in \u00b0/s). Your IMU should have a right handed coordinate frame with the z axis pointing upwards. \n",
"\n",
"For Arduino you can do it that way:\n",
"\n",
"```C++\n",
"#include <ArduinoJson.hpp>\n",
"#include <PacketSerial.h>\n",
"\n",
"const int capacity = JSON_ARRAY_SIZE(9);\n",
"ArduinoJson::StaticJsonDocument<capacity> doc;\n",
"\n",
"uint8_t buffer[100];\n",
"uint8_t encoded_buffer[100];\n",
"\n",
"void setup()\n",
"{\n",
" Serial.begin(115200);\n",
" // wait for serial connection\n",
" while (!Serial)\n",
" {\n",
" delay(1);\n",
" }\n",
"}\n",
"\n",
"void loop()\n",
"{\n",
" float gx, gy, gz, ax, ay, az;\n",
" float mx, my, mz;\n",
"\n",
" // your part: read data from your IMU\n",
" \n",
" ArduinoJson::JsonArray arr = doc.to<ArduinoJson::JsonArray>();\n",
" arr.add(mx);\n",
" arr.add(my);\n",
" arr.add(mz);\n",
" arr.add(ax);\n",
" arr.add(ay);\n",
" arr.add(az);\n",
" arr.add(gx);\n",
" arr.add(gy);\n",
" arr.add(gz);\n",
" size_t length = serializeMsgPack(doc, (char *) buffer, 100);\n",
" if (COBS::getEncodedBufferSize(length) < 100)\n",
" {\n",
" size_t lengthEncoded = COBS::encode(buffer, length, encoded_buffer);\n",
" encoded_buffer[lengthEncoded] = 0x00;\n",
" Serial.write(encoded_buffer, lengthEncoded + 1);\n",
" }\n",
" else\n",
" {\n",
" Serial.println(\"Overflow\");\n",
" }\n",
" \n",
" delay(5) // depends on the speed of your IMU\n",
"}\n",
"```\n",
"\n",
"While the measurements are taken, slowly rotate your device in all directions."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"scrolled": true
},
"outputs": [],
"source": [
"# adapt the port and baud rate to your device\n",
"port = '/dev/ttytDAN'\n",
"baud_rate = 115200\n",
"num_measurements = 100 # use a small value for testing; use 1000+ for real calibration\n",
"serial_format = 'motioncal_raw' # use 'cobs_msgpack' for the original notebook firmware\n",
"read_from_serial = True\n",
"read_from_csv = False\n",
"save_measurements = False\n",
"\n",
"s = None\n",
"try:\n",
" s = serial.Serial(port, baud_rate, timeout=1)\n",
"except serial.SerialException:\n",
" print(\"Could not connect to the provided port\")\n",
"\n",
"\n",
"def parse_measurement(response):\n",
" \"\"\"Parse either MotionCal text lines or the notebook's COBS/msgpack frames.\"\"\"\n",
" response = response.strip()\n",
" if not response:\n",
" return None\n",
"\n",
" if response.startswith(b'Raw:'):\n",
" values = [float(v) for v in response[4:].decode('ascii').split(',')]\n",
" if len(values) != 9:\n",
" raise ValueError(\"Expected 9 comma-separated values after Raw:\")\n",
"\n",
" # MotionCal prints ax,ay,az,gx,gy,gz,mx,my,mz. This notebook expects\n",
" # mx,my,mz,ax,ay,az,gx,gy,gz.\n",
" ax, ay, az, gx, gy, gz, mx, my, mz = values\n",
" return [mx, my, mz, ax, ay, az, gx, gy, gz]\n",
"\n",
" response = cobs.decode(response.rstrip(b'\\x00'))\n",
" return msgpack.unpackb(response, raw=False)\n",
"\n",
"\n",
"def read_measurements(num_measurements=10000, serial_format='motioncal_raw'):\n",
" if s is None:\n",
" return None\n",
"\n",
" if not s.is_open:\n",
" try:\n",
" s.open()\n",
" except serial.SerialException:\n",
" print(\"Could not connect\")\n",
" return None\n",
"\n",
" m = np.zeros((num_measurements, 9))\n",
" terminator = b'\\x00' if serial_format == 'cobs_msgpack' else b'\\n'\n",
"\n",
" try:\n",
" i = 0\n",
" while i < num_measurements:\n",
" if s.in_waiting:\n",
" response = s.read_until(expected=terminator, size=200)\n",
" if response:\n",
" try:\n",
" measurement = parse_measurement(response)\n",
" if measurement is None:\n",
" continue\n",
" m[i] = measurement\n",
" print(\"progress: {0:05.2f}%\".format((i + 1) / num_measurements * 100), end='\\r')\n",
" i += 1\n",
" except (ValueError, UnicodeDecodeError, cobs.DecodeError, msgpack.ExtraData, msgpack.UnpackValueError):\n",
" print(\"Could not decode sent data! No worries, I continue with the next one.\")\n",
" continue\n",
" print(\"\") # clear line\n",
"\n",
" except KeyboardInterrupt:\n",
" print(\"Finished through keyboard interrupt\")\n",
"\n",
" try:\n",
" s.close()\n",
" except serial.SerialException:\n",
" print(\"Could not disconnect\")\n",
"\n",
" print(\"Finished\")\n",
" return m[:i]\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Start the serial reading and inspect the first and last measurement. Do they look alright?"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"scrolled": true
},
"outputs": [],
"source": [
"measurements = None\n",
"if read_from_serial:\n",
" measurements = read_measurements(num_measurements, serial_format)\n",
" if measurements is not None and len(measurements):\n",
" print(measurements[0])\n",
" print(measurements[-1])\n",
" else:\n",
" measurements = None\n",
"try:\n",
" del unfiltered_measurements\n",
"except:\n",
" pass\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Read Measurements from File\n",
"Alternatively you can read in measurements from a csv file."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"if read_from_csv or measurements is None:\n",
" df = pd.read_csv('imu_readings.csv')\n",
" measurements = np.array(df.values[::1,1:10])\n",
" print(measurements[0])\n",
" print(measurements[-1])\n",
"try:\n",
" del unfiltered_measurements\n",
"except:\n",
" pass\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Lets start the fun\n",
"Now plot the data and see how it looks like."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"fig = plt.figure(figsize=plt.figaspect(2) * 2) # adapt factor according your window width\n",
"\n",
"x_ticks = np.arange(0, measurements.shape[0], 100)\n",
"\n",
"ax = fig.add_subplot(911)\n",
"ax.plot(measurements[:,0], 'o', markersize=1, label=\"mag x\")\n",
"ax.set_xlabel('# measurement')\n",
"ax.set_ylabel('gauss')\n",
"ax.legend()\n",
"ax.set_xticks(x_ticks, minor=True)\n",
"ax.grid(which='both', alpha=0.5)\n",
"\n",
"ax = fig.add_subplot(912)\n",
"ax.plot(measurements[:,1], 'o', markersize=1, label=\"mag y\")\n",
"ax.set_xlabel('# measurement')\n",
"ax.set_ylabel('gauss')\n",
"ax.legend()\n",
"ax.set_xticks(x_ticks, minor=True)\n",
"ax.grid(which='both', alpha=0.5)\n",
"\n",
"ax = fig.add_subplot(913)\n",
"ax.plot(measurements[:,2], 'o', markersize=1, label=\"mag z\")\n",
"ax.set_xlabel('# measurement')\n",
"ax.set_ylabel('gauss')\n",
"ax.legend()\n",
"ax.set_xticks(x_ticks, minor=True)\n",
"ax.grid(which='both', alpha=0.5)\n",
"\n",
"ax = fig.add_subplot(914)\n",
"ax.plot(measurements[:,3], 'o', markersize=1, label=\"acc x\")\n",
"ax.set_xlabel('# measurement')\n",
"ax.set_ylabel('m/s\u00b2')\n",
"ax.legend()\n",
"ax.set_xticks(x_ticks, minor=True)\n",
"ax.grid(which='both', alpha=0.5)\n",
"\n",
"ax = fig.add_subplot(915)\n",
"ax.plot(measurements[:,4], 'o', markersize=1, label=\"acc y\")\n",
"ax.set_xlabel('# measurement')\n",
"ax.set_ylabel('m/s\u00b2')\n",
"ax.legend()\n",
"ax.set_xticks(x_ticks, minor=True)\n",
"ax.grid(which='both', alpha=0.5)\n",
"\n",
"ax = fig.add_subplot(916)\n",
"ax.plot(measurements[:,5], 'o', markersize=1, label=\"acc z\")\n",
"ax.set_xlabel('# measurement')\n",
"ax.set_ylabel('m/s\u00b2')\n",
"ax.legend()\n",
"ax.set_xticks(x_ticks, minor=True)\n",
"ax.grid(which='both', alpha=0.5)\n",
"\n",
"ax = fig.add_subplot(917)\n",
"ax.plot(measurements[:,6], 'o', markersize=1, label=\"gyr x\")\n",
"ax.set_xlabel('# measurement')\n",
"ax.set_ylabel('\u00b0/s')\n",
"ax.legend()\n",
"ax.set_xticks(x_ticks, minor=True)\n",
"ax.grid(which='both', alpha=0.5)\n",
"\n",
"ax = fig.add_subplot(918)\n",
"ax.plot(measurements[:,7], 'o', markersize=1, label=\"gyr y\")\n",
"ax.set_xlabel('# measurement')\n",
"ax.set_ylabel('\u00b0/s')\n",
"ax.legend()\n",
"ax.set_xticks(x_ticks, minor=True)\n",
"ax.grid(which='both', alpha=0.5)\n",
"\n",
"ax = fig.add_subplot(919)\n",
"ax.plot(measurements[:,8], 'o', markersize=1, label=\"gyr z\")\n",
"ax.set_xlabel('# measurement')\n",
"ax.set_ylabel('\u00b0/s')\n",
"ax.legend()\n",
"ax.set_xticks(x_ticks, minor=True)\n",
"ax.grid(which='both', alpha=0.5)\n",
"\n",
"fig.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Save measurements as csv if you like."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"if save_measurements:\n",
" pd.DataFrame(measurements).to_csv(\"imu_readings_live.csv\")\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The magnetometer performs worse on high velocities. If you want to calculate the geomagnetic field and the inclination it might help filter bad data points. If you want to calculate the variance then dont do this."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# backup the data\n",
"try:\n",
" unfiltered_measurements\n",
"except NameError:\n",
" unfiltered_measurements = measurements\n",
" \n",
"#filter data and override\n",
"condition = np.all([\n",
" np.absolute(unfiltered_measurements[:, 6]) <= 100,\n",
" np.absolute(unfiltered_measurements[:, 7]) <= 100,\n",
" np.absolute(unfiltered_measurements[:, 8]) <= 100\n",
"],axis=0)\n",
"measurements = unfiltered_measurements[condition]\n",
"print(measurements.shape)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"You can undo the filtering with this command:"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"measurements = unfiltered_measurements"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Now plot the raw data together with a unit sphere. You will see that it is shifted, scaled and rotated compared to the unit sphere."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"scrolled": false
},
"outputs": [],
"source": [
"A = np.array([[1,0,0],[0,1,0],[0,0,1]])\n",
"c = [0,0,0]\n",
"plot_ellipsoid_and_measurements(A, c, measurements[:,0], measurements[:,1], measurements[:,2])"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Now fit an ellipsoid to the measurements. An ellipsoid is defined by all points `x` that fullfill `(x-center)'M(x-center) = const`."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"scrolled": false
},
"outputs": [],
"source": [
"# from https://de.mathworks.com/matlabcentral/fileexchange/24693-ellipsoid-fit\n",
"'''\n",
"Copyright (c) 2015, Yury Petrov\n",
"All rights reserved.\n",
"\n",
"Redistribution and use in source and binary forms, with or without\n",
"modification, are permitted provided that the following conditions are\n",
"met:\n",
"\n",
" * Redistributions of source code must retain the above copyright\n",
" notice, this list of conditions and the following disclaimer.\n",
" * Redistributions in binary form must reproduce the above copyright\n",
" notice, this list of conditions and the following disclaimer in\n",
" the documentation and/or other materials provided with the distribution\n",
"\n",
"THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\n",
"AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\n",
"IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\n",
"ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE\n",
"LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR\n",
"CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF\n",
"SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS\n",
"INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN\n",
"CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\n",
"ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n",
"POSSIBILITY OF SUCH DAMAGE.\n",
"'''\n",
"\n",
"def fit_ellipsoid(x, y, z):\n",
" D = np.zeros((9,measurements.shape[0]))\n",
" D[0] = x * x + y * y - 2 * z * z\n",
" D[1] = x * x + z * z - 2 * y * y\n",
" D[2] = 2 * x * y\n",
" D[3] = 2 * x * z\n",
" D[4] = 2 * z * y\n",
" D[5] = 2 * x\n",
" D[6] = 2 * y\n",
" D[7] = 2 * z\n",
" D[8] = np.ones(measurements.shape[0])\n",
"\n",
" d2 = x * x + y * y + z * z\n",
" u = linalg.inv(D @ D.transpose()) @ (D @ d2)\n",
"\n",
" v = np.zeros(10)\n",
" v[0] = u[0] + u[1] - 1\n",
" v[1] = u[0] - 2 * u[1] - 1\n",
" v[2] = u[1] - 2 * u[0] - 1\n",
" v[3:10] = u[2:9]\n",
" A = np.array([[v[0], v[3], v[4], v[6]], \\\n",
" [v[3], v[1], v[5], v[7]], \\\n",
" [v[4], v[5], v[2], v[8]], \\\n",
" [v[6], v[7], v[8], v[9]]])\n",
" center = linalg.inv(-A[0:3, 0:3]) @ v[6:9]\n",
" T = np.identity(4)\n",
" T[3, 0:3] = center.transpose()\n",
" R = T @ A @ T.transpose()\n",
" M = R[0:3, 0:3] /-R[3, 3]\n",
" return M, center, R[3, 3]"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"x = measurements[:,0]\n",
"y = measurements[:,1]\n",
"z = measurements[:,2]\n",
"M, center, scale = fit_ellipsoid(x, y, z)\n",
"plot_ellipsoid_and_measurements(M, center, x, y, z)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Calculate Winv (the inverse of the soft-iron matrix W). The hard iron vector V is already known through the ellipsoid shift (center)."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# hard iron\n",
"V = center\n",
"print(\"V\", V)\n",
"\n",
"#soft iron\n",
"# attain Winv by taking the matrix square root of M\n",
"D, Y = linalg.eig(M)\n",
"Winv = Y @ np.diag(np.sqrt(D)) @ linalg.inv(Y)\n",
"W = linalg.inv(Winv)\n",
"print(\"Winv:\\n\", Winv)\n",
"print(\"W:\\n\", W)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Now correct the measurements and see how good they fit to the sphere."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"def correct_measurements(measurements):\n",
" corrected_measurements = np.copy(measurements)\n",
" for idx, m in enumerate(measurements):\n",
" corrected_measurements[idx][0:3] = Winv @ (m[0:3] - center)\n",
" return corrected_measurements"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"scrolled": false
},
"outputs": [],
"source": [
"corrected_measurements = correct_measurements(measurements)\n",
"A = np.array([[1/scale,0,0],[0,1/scale,0],[0,0,1/scale]])\n",
"c = [0,0,0]\n",
"plot_ellipsoid_and_measurements(A, c, corrected_measurements[:,0] * np.sqrt(scale), corrected_measurements[:,1]* np.sqrt(scale), corrected_measurements[:,2] * np.sqrt(scale))"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The geomagnetic field strength is the square root of the scale. Go to [magnetic-declination.com](http://www.magnetic-declination.com/) and check if this is roughly ok. At my place it should be 48413.8nT."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"B = np.sqrt(scale)\n",
"print(\"B={0:05.2f}nT\".format(B * 100000))"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Now calculate the inclination. Also check at magnetic-declination.com if this is right. Don't confuse it with the declination. At my place it should be 64\u00b0."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"scrolled": false
},
"outputs": [],
"source": [
"def reject_outliers(data, m=2):\n",
" tmp_data=data[np.isnan(data) != True]\n",
" return tmp_data[abs(tmp_data - np.mean(tmp_data)) < m * np.std(tmp_data)]\n",
"\n",
"def calc_inclination_and_orientation(measurements):\n",
" rolls = np.zeros(measurements.shape[0])\n",
" pitchs = np.zeros(measurements.shape[0])\n",
" yaws = np.zeros(measurements.shape[0])\n",
" inclinations = np.zeros(measurements.shape[0])\n",
" \n",
" for idx, m in enumerate(measurements):\n",
" roll = np.arctan(-m[4] / np.sqrt(m[3] ** 2 + m[5] ** 2));\n",
" pitch = np.arctan2(m[3], np.sign(-m[5]) * np.sqrt(m[5] ** 2 + 0.01 * m[4] ** 2));\n",
" Rx = np.array([[1, 0, 0], [0, np.cos(roll), -np.sin(roll)], [0, np.sin(roll), np.cos(roll)]])\n",
" Ry = np.array([[np.cos(pitch), 0, np.sin(pitch)], [0, 1, 0], [-np.sin(pitch), 0, np.cos(pitch)]])\n",
" Bf = Rx.T @ Ry.T @ m[0:3]\n",
" Bf = Bf / linalg.norm(Bf, 2) # TODO ?\n",
" \n",
" #calculate yaw\n",
" yaw = -np.arctan2(-Bf[1], Bf[0])\n",
" \n",
" rolls[idx] = roll\n",
" pitchs[idx] = pitch\n",
" yaws[idx] = yaw\n",
" \n",
" inclination = np.arctan((-Bf[2] * np.cos(yaw)) / Bf[0])\n",
" # alternative inclination = np.arcsin(-Bf[2])\n",
" if np.abs(roll) <= 0.2 and np.abs(pitch) <= 0.2:\n",
" inclinations[idx] = inclination\n",
" else:\n",
" inclinations[idx] = np.nan\n",
"\n",
" inclination = np.mean(reject_outliers(inclinations, 2))\n",
" return inclination, rolls, pitchs, yaws, inclinations"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"scrolled": false
},
"outputs": [],
"source": [
"inclination, rolls, pitchs, yaws, inclinations = calc_inclination_and_orientation(measurements)\n",
"\n",
"print(\"inclination={0:05.2f}\u00b0\".format(inclination * 180 / np.pi))\n",
"\n",
"fig = plt.figure(figsize=plt.figaspect(1) * 2) # adapt factor according your window width\n",
"\n",
"x_ticks = np.arange(0, measurements.shape[0], 100)\n",
"\n",
"ax = fig.add_subplot(411)\n",
"ax.plot(rolls, 'o', markersize=1, label=\"roll\")\n",
"ax.set_xlabel('# measurement')\n",
"ax.set_ylabel('rad')\n",
"ax.legend()\n",
"ax.set_xticks(x_ticks, minor=True)\n",
"ax.grid(which='both', alpha=0.5)\n",
"\n",
"ax = fig.add_subplot(412)\n",
"ax.plot(pitchs, 'o', markersize=1, label=\"pitch\")\n",
"ax.set_xlabel('# measurement')\n",
"ax.set_ylabel('rad')\n",
"ax.legend()\n",
"ax.set_xticks(x_ticks, minor=True)\n",
"ax.grid(which='both', alpha=0.5)\n",
"\n",
"ax = fig.add_subplot(413)\n",
"ax.plot(yaws, 'o', markersize=1, label=\"yaw\")\n",
"ax.set_xlabel('# measurement')\n",
"ax.set_ylabel('rad')\n",
"ax.legend()\n",
"ax.set_xticks(x_ticks, minor=True)\n",
"ax.grid(which='both', alpha=0.5)\n",
"\n",
"ax = fig.add_subplot(414)\n",
"ax.plot(inclinations, 'o', markersize=1, label=\"inclination\")\n",
"ax.hlines(inclination, 0, inclinations.shape[0], 'r', label=\"avg inclination\")\n",
"ax.set_xlabel('# measurement')\n",
"ax.set_ylabel('rad')\n",
"ax.legend()\n",
"ax.set_xticks(x_ticks, minor=True)\n",
"ax.grid(which='both', alpha=0.5)\n",
"\n",
"fig.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Now calculate the variances of the sensor. If you implement a Kalman Filter use this for the measurement noise matrix."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"mag_should = np.zeros((measurements.shape[0], 3))\n",
"for idx, m in enumerate(measurements):\n",
" roll = rolls[idx]\n",
" pitch = pitchs[idx]\n",
" yaw = yaws[idx]\n",
" Rx = np.array([[1, 0, 0], [0, np.cos(roll), -np.sin(roll)], [0, np.sin(roll), np.cos(roll)]])\n",
" Ry = np.array([[np.cos(pitch), 0, np.sin(pitch)], [0, 1, 0], [-np.sin(pitch), 0, np.cos(pitch)]])\n",
" Rz = np.array([[np.cos(yaw), -np.sin(yaw), 0], [np.sin(yaw), np.cos(yaw), 0], [0, 0, 1]])\n",
" \n",
" mag_should[idx] = W @ Ry @ Rx @ Rz @ (B * np.array([np.cos(inclination), 0, -np.sin(inclination)])) + V\n",
" \n",
"mag_error = mag_should - measurements[:,0:3]\n",
"\n",
"mag_var = np.std(mag_error, axis=0)\n",
"print(mag_var)"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "Python 3",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.6.9"
},
"tags": [
"remove_output"
]
},
"nbformat": 4,
"nbformat_minor": 2
}
Reference in a new issue View git blame Copy permalink