Reputation: 11
I have a problem regarding fitting an ellipsoid to calibrate a 3-axis magnetometer. I can translate and rotate the matrix correctly, but scaling is challenging. Right now, it only seems to scale correctly in the x-axis (ellipsoid is between -1 and 1), but the y and z are way too high (ellipsoid is between -130000 and 130000). I do not know how to fix this, maybe I am missing something. Here is my code:
import pandas as pd
import numpy as np
import matplotlib.pyplot as plt
df_raw_data = pd.read_csv('data/sensor_data.csv')
mag_data_raw = np.array(df_raw_data[['mag_x', 'mag_y', 'mag_z']])
def ellipsoid_fitting(data_points):
x, y, z = data_points[:, 0, np.newaxis], data_points[:, 1, np.newaxis], data_points[:, 2, np.newaxis]
D = np.hstack((x**2, y**2, z**2, 2*x*y, 2*x*z, 2*y*z, 2*x, 2*y, 2*z, np.ones_like(x)))
vh = np.linalg.svd(D, full_matrices=False)[2]
v = vh.T
a = v[:, -1]
A = np.array([
[a[0], a[3], a[4]],
[a[3], a[1], a[5]],
[a[4], a[5], a[2]]
])
center = -np.linalg.inv(A[:3, :3]) @ a[6:9]
translation_matrix = np.eye(4)
translation_matrix[3, :3] = center
A_4x4 = np.zeros((4, 4))
A_4x4[:3, :3] = A
A_4x4[3, 3] = -1
transformed = translation_matrix.T @ A_4x4 @ translation_matrix
eigenvalues, eigenvectors = np.linalg.eig(transformed[:3, :3])
radii = np.sqrt(np.abs(eigenvalues))
return center, radii, eigenvectors
def calibrate_data(data, center, radii, rotation):
translated = data - center
rotated = translated @ rotation
print(rotated)
scaled = rotated / radii
return scaled
fig = plt.figure(figsize = (10, 7))
ax = plt.axes(projection ="3d")
print(mag_data_raw.shape)
center, radii, rotation = ellipsoid_fitting(mag_data_raw)
print(center, radii, rotation)
mag_data_cal = calibrate_data(mag_data_raw, center, radii, rotation)
#ax.scatter3D(mag_data_raw[:,0], mag_data_raw[:,1], mag_data_raw[:,2], color = "green", label = 'Raw Data')
ax.scatter3D(mag_data_cal[:, 0], mag_data_cal[:, 1], mag_data_cal[:, 2], color='red', label='Calibrated Data')
ax.legend()
plt.title("raw vs calibrated data")
plt.show()
I tried many different techniques to calibrate the magnetometer data, this one seemed to work for me, except for the scaling.
Upvotes: 0
Views: 50