euwedewilde
euwedewilde

Reputation: 11

Problem with calibrating 3-axis magnetometer (ellipsoid fitting)

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

Answers (0)

Related Questions