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

803 lines
24 KiB
Python
Raw Blame History

#!/usr/bin/env python
# coding: utf-8
# # Hard- and Soft-Iron Magnetometer Calibration Visualized
# This is a step by step walk through magnetometer calibration.
#
# The math backgroud can be found here:
# * [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)
# * [AN4246](https://www.nxp.com/docs/en/application-note/AN4246.pdf)
# * [Least square ellipsoid fitting using iterative orthogonal transformations](https://arxiv.org/pdf/1704.04877.pdf)
# ## Setup
# First import all necessary packages. You may have to install a few packages before hand. In Ubuntu-Linux the commands are:
# ```ssh
# $ sudo apt install python3-numpy
# $ sudo apt install python3-pandas
# # only needed if you want to read measurements from your micro controller
# $ sudo apt install python3-serial
# $ sudo -H pip3 install cobs
# ```
# To install tkinter:
# pip install tk-toolkit
#%matplotlib notebook
import matplotlib
#matplotlib.use("Agg") # headless backend; saves plots without tkinter
import numpy as np
import numpy.linalg as linalg
import matplotlib.pyplot as plt
matplotlib.use("TkAgg") # or omit entirely if Tk is default
plt.ion() # interactive mode ON
from tk_toolkit import tk_toolkit
from mpl_toolkits.mplot3d import Axes3D
import serial
import sys
import pandas as pd
from cobs import cobs
import msgpack
from pathlib import Path
import time
plot_output_dir = Path("plots")
plot_output_dir.mkdir(exist_ok=True)
def finish_plot(fig, name):
output_path = plot_output_dir / name
fig.tight_layout()
fig.savefig(output_path, dpi=150)
plt.close(fig)
print("Saved plot:", output_path)
# Define a plotting function
# In[ ]:
# from https://stackoverflow.com/questions/7819498/plotting-ellipsoid-with-matplotlib
def plot_ellipsoid_and_measurements(A, c, xm, ym, zm, name):
# find the rotation matrix and radii of the axes
U, s, rotation = linalg.svd(A)
radii = 1.0/np.sqrt(s)
u = np.linspace(0.0, 2.0 * np.pi, 100)
v = np.linspace(0.0, np.pi, 100)
x = radii[0] * np.outer(np.cos(u), np.sin(v))
y = radii[1] * np.outer(np.sin(u), np.sin(v))
z = radii[2] * np.outer(np.ones_like(u), np.cos(v))
for i in range(len(x)):
for j in range(len(x)):
[x[i,j],y[i,j],z[i,j]] = np.dot([x[i,j],y[i,j],z[i,j]], rotation) + c
# plot
fig = plt.figure(figsize=plt.figaspect(1) * 1.5) # adapt factor according your window width
ax = fig.add_subplot(111, projection='3d')
ax.scatter(xm, ym, zm, s=0.1, c='r', alpha=0.5) # plot measurements
ax.plot_wireframe(x, y, z, rstride=4, cstride=4, color='b', alpha=0.2) # plot ellipsoid
# scale axes equally
max_value = max(radii[0], radii[1], radii[2], max(xm), max(ym), max(zm))
for axis in 'xyz':
getattr(ax, 'set_{}lim'.format(axis))((-max_value, max_value))
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
finish_plot(fig, name)
# ## Read Measurements from Micro Controller
# 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.
# Alternatively you can use my saved measurements (see next section). But then -of course- you calibrate my magnetometer, not yours.
#
# 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²) and the 3 gyroscopes (in °/s). Your IMU should have a right handed coordinate frame with the z axis pointing upwards.
#
# For Arduino you can do it that way:
#
# ```C++
# #include <ArduinoJson.hpp>
# #include <PacketSerial.h>
#
# const int capacity = JSON_ARRAY_SIZE(9);
# ArduinoJson::StaticJsonDocument<capacity> doc;
#
# uint8_t buffer[100];
# uint8_t encoded_buffer[100];
#
# void setup()
# {
# Serial.begin(115200);
# // wait for serial connection
# while (!Serial)
# {
# delay(1);
# }
# }
#
# void loop()
# {
# float gx, gy, gz, ax, ay, az;
# float mx, my, mz;
#
# // your part: read data from your IMU
#
# ArduinoJson::JsonArray arr = doc.to<ArduinoJson::JsonArray>();
# arr.add(mx);
# arr.add(my);
# arr.add(mz);
# arr.add(ax);
# arr.add(ay);
# arr.add(az);
# arr.add(gx);
# arr.add(gy);
# arr.add(gz);
# size_t length = serializeMsgPack(doc, (char *) buffer, 100);
# if (COBS::getEncodedBufferSize(length) < 100)
# {
# size_t lengthEncoded = COBS::encode(buffer, length, encoded_buffer);
# encoded_buffer[lengthEncoded] = 0x00;
# Serial.write(encoded_buffer, lengthEncoded + 1);
# }
# else
# {
# Serial.println("Overflow");
# }
#
# delay(5) // depends on the speed of your IMU
# }
# ```
#
# While the measurements are taken, slowly rotate your device in all directions.
# In[ ]:
# adapt the port and baud rate to your device
port = '/dev/ttyACM1'
port = '/dev/ttytDAN'
baud_rate = 115200
num_measurements = 10000
num_measurements = 3000
serial_format = 'motioncal_raw' # use 'cobs_msgpack' for the original notebook firmware
read_from_serial = True
read_from_csv = False
save_measurements = False
s = None
try:
s = serial.Serial(port, baud_rate, timeout=1)
except serial.SerialException:
print("Could not connect to the provided port")
def live_plot_serial_raw(update_interval=0.5, max_points=2000):
if s is None:
print("Serial not available")
return
if not s.is_open:
s.open()
data = []
fig, ax = plt.subplots(figsize=(7, 7))
scat_xy = ax.scatter([], [], s=4, c='red', label='X vs Y')
scat_yz = ax.scatter([], [], s=4, c='green', label='Y vs Z')
scat_xz = ax.scatter([], [], s=4, c='blue', label='X vs Z')
ax.set_xlabel('axis A')
ax.set_ylabel('axis B')
ax.set_title('Raw magnetometer samples: 0')
ax.legend()
ax.grid(True)
ax.set_aspect('equal', adjustable='box')
plt.show(block=False)
last_update = 0
try:
while True:
if s.in_waiting:
terminator = b'\x00' if serial_format == 'cobs_msgpack' else b'\n'
response = s.read_until(expected=terminator, size=200)
try:
measurement = parse_measurement(response)
if measurement is None:
continue
data.append(measurement)
if len(data) > max_points:
data = data[-max_points:]
except Exception:
continue
now = time.time()
if now - last_update >= update_interval and len(data) > 2:
arr = np.array(data)
mx = arr[:, 0]
my = arr[:, 1]
mz = arr[:, 2]
# RAW values (no normalization, no centering)
scat_xy.set_offsets(np.column_stack((mx, my)))
scat_yz.set_offsets(np.column_stack((my, mz)))
scat_xz.set_offsets(np.column_stack((mx, mz)))
# dynamic bounds (preserve raw offsets)
xmin = min(mx.min(), my.min(), mz.min())
xmax = max(mx.max(), my.max(), mz.max())
pad = (xmax - xmin) * 0.10 if xmax != xmin else 100
ax.set_xlim(xmin - pad, xmax + pad)
ax.set_ylim(xmin - pad, xmax + pad)
ax.set_title(f"Raw magnetometer samples: {len(data)}")
fig.canvas.draw_idle()
fig.canvas.flush_events()
last_update = now
plt.pause(0.01)
except KeyboardInterrupt:
print("\nStopped live plotting (raw)")
finally:
try:
s.close()
except serial.SerialException:
pass
def live_plot_serial(update_interval=0.5, max_points=2000):
if s is None:
print("Serial not available")
return
if not s.is_open:
s.open()
data = []
fig, ax = plt.subplots(figsize=(7, 7))
scat_xy = ax.scatter([], [], s=4, c='red', label='mag X/Y')
scat_yz = ax.scatter([], [], s=4, c='green', label='mag Y/Z')
scat_xz = ax.scatter([], [], s=4, c='blue', label='mag X/Z')
ax.set_xlabel('axis A')
ax.set_ylabel('axis B')
ax.set_title('Mag samples: 0')
ax.legend()
ax.grid(True)
ax.set_aspect('equal', adjustable='box')
plt.show(block=False)
last_update = 0
try:
while True:
if s.in_waiting:
terminator = b'\x00' if serial_format == 'cobs_msgpack' else b'\n'
response = s.read_until(expected=terminator, size=200)
try:
measurement = parse_measurement(response)
if measurement is None:
continue
data.append(measurement)
if len(data) > max_points:
data = data[-max_points:]
except Exception:
continue
now = time.time()
if now - last_update >= update_interval and len(data) > 2:
arr = np.array(data)
mx = arr[:, 0]
my = arr[:, 1]
mz = arr[:, 2]
scat_xy.set_offsets(np.column_stack((mx, my)))
scat_yz.set_offsets(np.column_stack((my, mz)))
scat_xz.set_offsets(np.column_stack((mx, mz)))
all_vals = np.concatenate([mx, my, mz])
vmin = all_vals.min()
vmax = all_vals.max()
pad = (vmax - vmin) * 0.10 if vmax != vmin else 100
ax.set_xlim(vmin - pad, vmax + pad)
ax.set_ylim(vmin - pad, vmax + pad)
ax.set_title(f"Mag samples: {len(data)}")
fig.canvas.draw_idle()
fig.canvas.flush_events()
last_update = now
plt.pause(0.01)
except KeyboardInterrupt:
print("\nStopped live plotting")
finally:
try:
s.close()
except serial.SerialException:
pass
def parse_measurement(response):
"""Parse either MotionCal text lines or the notebook's COBS/msgpack frames."""
response = response.strip()
if not response:
return None
if response.startswith(b'Raw:'):
values = [float(v) for v in response[4:].decode('ascii').split(',')]
if len(values) != 9:
raise ValueError("Expected 9 comma-separated values after Raw:")
# MotionCal prints ax,ay,az,gx,gy,gz,mx,my,mz. This notebook expects
# mx,my,mz,ax,ay,az,gx,gy,gz.
ax, ay, az, gx, gy, gz, mx, my, mz = values
return [mx, my, mz, ax, ay, az, gx, gy, gz]
response = cobs.decode(response.rstrip(b'\x00'))
return msgpack.unpackb(response, raw=False)
def read_measurements(num_measurements=10000, serial_format='motioncal_raw'):
if s is None:
return None
if not s.is_open:
try:
s.open()
except serial.SerialException:
print("Could not connect")
return None
m = np.zeros((num_measurements, 9))
try:
i = 0
while i < num_measurements:
if s.in_waiting:
terminator = b'\x00' if serial_format == 'cobs_msgpack' else b'\n'
response = s.read_until(expected=terminator, size=200)
if response:
try:
measurement = parse_measurement(response)
if measurement is None:
continue
m[i] = measurement
print("progress: {0:05.2f}%".format((i + 1) / num_measurements * 100), end='\r')
i += 1
except (ValueError, UnicodeDecodeError, cobs.DecodeError, msgpack.ExtraData, msgpack.UnpackValueError) as e:
print("Could not decode sent data! No worries, I continue with the next one.")
continue
print("") # clear line
except KeyboardInterrupt:
print("Finished through keyboard interrupt")
try:
s.close()
except serial.SerialException:
print("Could not disconnect")
print("Finished")
return m[:i]
# Start the serial reading and inspect the first and last measurement. Do they look alright?
# In[ ]:
measurements = None
if read_from_serial:
#live_plot_serial(update_interval=0.5)
live_plot_serial_raw(update_interval=0.5)
sys.exit(0)
if read_from_serial:
measurements = read_measurements(num_measurements, serial_format)
if measurements is not None and len(measurements):
print(measurements[0])
print(measurements[-1])
else:
measurements = None
try:
del unfiltered_measurements
except:
pass
# ## Read Measurements from File
# Alternatively you can read in measurements from a csv file.
# In[ ]:
if read_from_csv or measurements is None:
df = pd.read_csv('imu_readings.csv')
measurements = np.array(df.values[::1,1:10])
print(measurements[0])
print(measurements[-1])
try:
del unfiltered_measurements
except:
pass
# ## Lets start the fun
# Now plot the data and see how it looks like.
# In[ ]:
fig = plt.figure(figsize=plt.figaspect(2) * 2) # adapt factor according your window width
x_ticks = np.arange(0, measurements.shape[0], 100)
ax = fig.add_subplot(911)
ax.plot(measurements[:,0], 'o', markersize=1, label="mag x")
ax.set_xlabel('# measurement')
ax.set_ylabel('gauss')
ax.legend()
ax.set_xticks(x_ticks, minor=True)
ax.grid(which='both', alpha=0.5)
ax = fig.add_subplot(912)
ax.plot(measurements[:,1], 'o', markersize=1, label="mag y")
ax.set_xlabel('# measurement')
ax.set_ylabel('gauss')
ax.legend()
ax.set_xticks(x_ticks, minor=True)
ax.grid(which='both', alpha=0.5)
ax = fig.add_subplot(913)
ax.plot(measurements[:,2], 'o', markersize=1, label="mag z")
ax.set_xlabel('# measurement')
ax.set_ylabel('gauss')
ax.legend()
ax.set_xticks(x_ticks, minor=True)
ax.grid(which='both', alpha=0.5)
ax = fig.add_subplot(914)
ax.plot(measurements[:,3], 'o', markersize=1, label="acc x")
ax.set_xlabel('# measurement')
ax.set_ylabel('m/s²')
ax.legend()
ax.set_xticks(x_ticks, minor=True)
ax.grid(which='both', alpha=0.5)
ax = fig.add_subplot(915)
ax.plot(measurements[:,4], 'o', markersize=1, label="acc y")
ax.set_xlabel('# measurement')
ax.set_ylabel('m/s²')
ax.legend()
ax.set_xticks(x_ticks, minor=True)
ax.grid(which='both', alpha=0.5)
ax = fig.add_subplot(916)
ax.plot(measurements[:,5], 'o', markersize=1, label="acc z")
ax.set_xlabel('# measurement')
ax.set_ylabel('m/s²')
ax.legend()
ax.set_xticks(x_ticks, minor=True)
ax.grid(which='both', alpha=0.5)
ax = fig.add_subplot(917)
ax.plot(measurements[:,6], 'o', markersize=1, label="gyr x")
ax.set_xlabel('# measurement')
ax.set_ylabel('°/s')
ax.legend()
ax.set_xticks(x_ticks, minor=True)
ax.grid(which='both', alpha=0.5)
ax = fig.add_subplot(918)
ax.plot(measurements[:,7], 'o', markersize=1, label="gyr y")
ax.set_xlabel('# measurement')
ax.set_ylabel('°/s')
ax.legend()
ax.set_xticks(x_ticks, minor=True)
ax.grid(which='both', alpha=0.5)
ax = fig.add_subplot(919)
ax.plot(measurements[:,8], 'o', markersize=1, label="gyr z")
ax.set_xlabel('# measurement')
ax.set_ylabel('°/s')
ax.legend()
ax.set_xticks(x_ticks, minor=True)
ax.grid(which='both', alpha=0.5)
finish_plot(fig, "raw_measurements.png")
# Save measurements as csv if you like.
# In[ ]:
if save_measurements:
pd.DataFrame(measurements).to_csv("imu_readings_live.csv")
# 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.
# In[ ]:
# backup the data
try:
unfiltered_measurements
except NameError:
unfiltered_measurements = measurements
#filter data and override
condition = np.all([
np.absolute(unfiltered_measurements[:, 6]) <= 100,
np.absolute(unfiltered_measurements[:, 7]) <= 100,
np.absolute(unfiltered_measurements[:, 8]) <= 100
],axis=0)
measurements = unfiltered_measurements[condition]
print(measurements.shape)
# You can undo the filtering with this command:
# In[ ]:
measurements = unfiltered_measurements
# 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.
# In[ ]:
A = np.array([[1,0,0],[0,1,0],[0,0,1]])
c = [0,0,0]
plot_ellipsoid_and_measurements(A, c, measurements[:,0], measurements[:,1], measurements[:,2], "raw_magnetometer_ellipsoid.png")
# Now fit an ellipsoid to the measurements. An ellipsoid is defined by all points `x` that fullfill `(x-center)'M(x-center) = const`.
# In[ ]:
# from https://de.mathworks.com/matlabcentral/fileexchange/24693-ellipsoid-fit
'''
Copyright (c) 2015, Yury Petrov
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are
met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in
the documentation and/or other materials provided with the distribution
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
'''
def fit_ellipsoid(x, y, z):
D = np.zeros((9,measurements.shape[0]))
D[0] = x * x + y * y - 2 * z * z
D[1] = x * x + z * z - 2 * y * y
D[2] = 2 * x * y
D[3] = 2 * x * z
D[4] = 2 * z * y
D[5] = 2 * x
D[6] = 2 * y
D[7] = 2 * z
D[8] = np.ones(measurements.shape[0])
d2 = x * x + y * y + z * z
u = linalg.inv(D @ D.transpose()) @ (D @ d2)
v = np.zeros(10)
v[0] = u[0] + u[1] - 1
v[1] = u[0] - 2 * u[1] - 1
v[2] = u[1] - 2 * u[0] - 1
v[3:10] = u[2:9]
A = np.array([[v[0], v[3], v[4], v[6]], \
[v[3], v[1], v[5], v[7]], \
[v[4], v[5], v[2], v[8]], \
[v[6], v[7], v[8], v[9]]])
center = linalg.inv(-A[0:3, 0:3]) @ v[6:9]
T = np.identity(4)
T[3, 0:3] = center.transpose()
R = T @ A @ T.transpose()
M = R[0:3, 0:3] /-R[3, 3]
return M, center, R[3, 3]
# In[ ]:
x = measurements[:,0]
y = measurements[:,1]
z = measurements[:,2]
M, center, scale = fit_ellipsoid(x, y, z)
plot_ellipsoid_and_measurements(M, center, x, y, z, "fitted_magnetometer_ellipsoid.png")
# Calculate Winv (the inverse of the soft-iron matrix W). The hard iron vector V is already known through the ellipsoid shift (center).
# In[ ]:
# hard iron
V = center
print("V", V)
#soft iron
# attain Winv by taking the matrix square root of M
D, Y = linalg.eig(M)
Winv = Y @ np.diag(np.sqrt(D)) @ linalg.inv(Y)
W = linalg.inv(Winv)
print("Winv:\n", Winv)
print("W:\n", W)
# Now correct the measurements and see how good they fit to the sphere.
# In[ ]:
def correct_measurements(measurements):
corrected_measurements = np.copy(measurements)
for idx, m in enumerate(measurements):
corrected_measurements[idx][0:3] = Winv @ (m[0:3] - center)
return corrected_measurements
# In[ ]:
corrected_measurements = correct_measurements(measurements)
A = np.array([[1/scale,0,0],[0,1/scale,0],[0,0,1/scale]])
c = [0,0,0]
plot_ellipsoid_and_measurements(A, c, corrected_measurements[:,0] * np.sqrt(scale), corrected_measurements[:,1]* np.sqrt(scale), corrected_measurements[:,2] * np.sqrt(scale), "corrected_magnetometer_sphere.png")
# 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.
# In[ ]:
B = np.sqrt(scale)
print("B={0:05.2f}nT".format(B * 100000))
# 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°.
# In[ ]:
def reject_outliers(data, m=2):
tmp_data=data[np.isnan(data) != True]
return tmp_data[abs(tmp_data - np.mean(tmp_data)) < m * np.std(tmp_data)]
def calc_inclination_and_orientation(measurements):
rolls = np.zeros(measurements.shape[0])
pitchs = np.zeros(measurements.shape[0])
yaws = np.zeros(measurements.shape[0])
inclinations = np.zeros(measurements.shape[0])
for idx, m in enumerate(measurements):
roll = np.arctan(-m[4] / np.sqrt(m[3] ** 2 + m[5] ** 2));
pitch = np.arctan2(m[3], np.sign(-m[5]) * np.sqrt(m[5] ** 2 + 0.01 * m[4] ** 2));
Rx = np.array([[1, 0, 0], [0, np.cos(roll), -np.sin(roll)], [0, np.sin(roll), np.cos(roll)]])
Ry = np.array([[np.cos(pitch), 0, np.sin(pitch)], [0, 1, 0], [-np.sin(pitch), 0, np.cos(pitch)]])
Bf = Rx.T @ Ry.T @ m[0:3]
Bf = Bf / linalg.norm(Bf, 2) # TODO ?
#calculate yaw
yaw = -np.arctan2(-Bf[1], Bf[0])
rolls[idx] = roll
pitchs[idx] = pitch
yaws[idx] = yaw
inclination = np.arctan((-Bf[2] * np.cos(yaw)) / Bf[0])
# alternative inclination = np.arcsin(-Bf[2])
if np.abs(roll) <= 0.2 and np.abs(pitch) <= 0.2:
inclinations[idx] = inclination
else:
inclinations[idx] = np.nan
inclination = np.mean(reject_outliers(inclinations, 2))
return inclination, rolls, pitchs, yaws, inclinations
# In[ ]:
inclination, rolls, pitchs, yaws, inclinations = calc_inclination_and_orientation(measurements)
print("inclination={0:05.2f}°".format(inclination * 180 / np.pi))
fig = plt.figure(figsize=plt.figaspect(1) * 2) # adapt factor according your window width
x_ticks = np.arange(0, measurements.shape[0], 100)
ax = fig.add_subplot(411)
ax.plot(rolls, 'o', markersize=1, label="roll")
ax.set_xlabel('# measurement')
ax.set_ylabel('rad')
ax.legend()
ax.set_xticks(x_ticks, minor=True)
ax.grid(which='both', alpha=0.5)
ax = fig.add_subplot(412)
ax.plot(pitchs, 'o', markersize=1, label="pitch")
ax.set_xlabel('# measurement')
ax.set_ylabel('rad')
ax.legend()
ax.set_xticks(x_ticks, minor=True)
ax.grid(which='both', alpha=0.5)
ax = fig.add_subplot(413)
ax.plot(yaws, 'o', markersize=1, label="yaw")
ax.set_xlabel('# measurement')
ax.set_ylabel('rad')
ax.legend()
ax.set_xticks(x_ticks, minor=True)
ax.grid(which='both', alpha=0.5)
ax = fig.add_subplot(414)
ax.plot(inclinations, 'o', markersize=1, label="inclination")
ax.hlines(inclination, 0, inclinations.shape[0], 'r', label="avg inclination")
ax.set_xlabel('# measurement')
ax.set_ylabel('rad')
ax.legend()
ax.set_xticks(x_ticks, minor=True)
ax.grid(which='both', alpha=0.5)
finish_plot(fig, "orientation_inclination.png")
# Now calculate the variances of the sensor. If you implement a Kalman Filter use this for the measurement noise matrix.
# In[ ]:
mag_should = np.zeros((measurements.shape[0], 3))
for idx, m in enumerate(measurements):
roll = rolls[idx]
pitch = pitchs[idx]
yaw = yaws[idx]
Rx = np.array([[1, 0, 0], [0, np.cos(roll), -np.sin(roll)], [0, np.sin(roll), np.cos(roll)]])
Ry = np.array([[np.cos(pitch), 0, np.sin(pitch)], [0, 1, 0], [-np.sin(pitch), 0, np.cos(pitch)]])
Rz = np.array([[np.cos(yaw), -np.sin(yaw), 0], [np.sin(yaw), np.cos(yaw), 0], [0, 0, 1]])
mag_should[idx] = W @ Ry @ Rx @ Rz @ (B * np.array([np.cos(inclination), 0, -np.sin(inclination)])) + V
mag_error = mag_should - measurements[:,0:3]
mag_var = np.std(mag_error, axis=0)
print(mag_var)
Reference in a new issue View git blame Copy permalink