Converting Python Ellipsoid to Sphere code (FreeIMU_GUI) to Equivalent Eigne

Question

As an exercise I wanted to convert the FreeImu_Gui code that converts raw magnetometer to Sphere for calibration of the magnetometer. But results from using Eigen are not even close to what Python is giving me.

The python code:

def calibrate(x, y, z):
  # H = numpy.array([x, y, z, -y**2, -z**2, numpy.ones([len(x), 1])])
  H = numpy.array([x, y, z, -y**2, -z**2, numpy.ones([len(x)])])
  ##print(H, sep=", ", end = '\n')
  H = numpy.transpose(H)
  #print('H Transposed')
  #print(H, sep=", ", end = '\n')

  w = x**2
  
  (X, residues, rank, shape) = linalg.lstsq(H, w)
  #print("X")
  #print(X)
  OSx = X[0] / 2
  OSy = X[1] / (2 * X[3])
  OSz = X[2] / (2 * X[4])
  
  A = X[5] + OSx**2 + X[3] * OSy**2 + X[4] * OSz**2
  B = A / X[3]
  C = A / X[4]
  
  SCx = numpy.sqrt(A)
  SCy = numpy.sqrt(B)
  SCz = numpy.sqrt(C)
  
  # type conversion from numpy.float64 to standard python floats
  offsets = [OSx, OSy, OSz]
  scale = [SCx, SCy, SCz]
  
  #offsets = map(numpy.ndarray.item, offsets)
  #scale = map(numpy.ndarray.item, scale)
  
  offsets = list(map(float, offsets))
  scale = list(map(float, scale))
  return (offsets, scale)

Some background x, y, z are magnetometer data in counts -400 to +400 roughly. PS using a MPU-9250.

Using CoPilot and other sources I managed to get the QR Version returning results.
Here is the Teensy devboard with SDRAM code:

#include <SD.h>
const int chipSelect = BUILTIN_SDCARD;

#include "eigen.h"
#include "Eigen/Dense"
//#include <iostream>
#include <vector>
#include <cmath>
#include <T4_PowerButton.h>

int axis1;
int axis2;
int axis3;

using namespace std;
vector<float> x, y, z;
typedef Eigen::Matrix<int16_t, Eigen::Dynamic, 1> VectorXint16;

// Custom board based off of Teensy4 with SDRAM
#if defined(ARDUINO_TEENSY_DEVBRD4) || defined(ARDUINO_TEENSY_DEVBRD5)
//#include "SDRAM_t4.h"
//SDRAM_t4 sdram;
float *myH = nullptr;
float *myX = nullptr;
float *myY = nullptr;
float *myZ = nullptr;
uint32_t sizeof_H = 0;
#endif

void setup()
{
 /* Serial to display data */
  while(!Serial && millis() < 5000) {}
  Serial.begin(115200);
  
  // If Teensy 4.x fails print Crashreport to serial monitor
  if (CrashReport) {
      Serial.print(CrashReport);
      Serial.println("Press any key to continue");
      while (Serial.read() != -1) {
      }
      while (Serial.read() == -1) {
      }
      while (Serial.read() != -1) {
      }
  }

  Serial.println(".... parse floats out of sd file ....");
  Serial.print("Compiler: ");
  Serial.print(__VERSION__);
  Serial.print(", Arduino IDE: ");
  Serial.println(ARDUINO);
  Serial.print("Created: ");
  Serial.print(__TIME__);
  Serial.print(", ");
  Serial.println(__DATE__);
  Serial.println(__FILE__);


  Serial.print("Initializing SD card...");

  // see if the card is present and can be initialized:
  if (!SD.begin(chipSelect)) {
    Serial.println("Card failed, or not present");
    // don't do anything more:
    return;
  }
  Serial.println("card initialized.");

  // open the file. note that only one file can be open at a time,
  // so you have to close this one before opening another.
  File dataFile = SD.open("magn.txt"); //<<<<<<<<<<<<<<<<<<<<<< set file name here

  // if the file is available, read from it:

  if (dataFile)
  {
    while (dataFile.available())
    {
      //parse the next 4 values out of the file:
      axis1 = dataFile.parseInt();
      axis2 = dataFile.parseInt();
      axis3 = dataFile.parseInt();
      //print them to the monitor:
      //Serial.printf("%d, %d, %d\n", axis1, axis2, axis3);
      x.push_back((float)axis1);
      y.push_back((float)axis2);
      z.push_back((float)axis3);
      delay(1);
      //go back to the top for the next line...
      //or ....
    }//while data available
    Serial.println(".... no more data");
    dataFile.close();
  }// if datafile
  // if the file isn't open, pop up an error:
  else {
    Serial.println("error opening the data file");
  }

    int sz = x.size() - 1;
    Serial.println(sz);

#if defined(ARDUINO_TEENSY_DEVBRD4) || defined(ARDUINO_TEENSY_DEVBRD5)
    myH = (float *)((((uint32_t)(sdram_malloc(sz * 6 * sizeof(float) + 32)) + 32) & 0xffffffe0));
    myX = (float *)((((uint32_t)(sdram_malloc(sz * sizeof(float) + 32)) + 32) & 0xffffffe0));
    myY = (float *)((((uint32_t)(sdram_malloc(sz * sizeof(float) + 32)) + 32) & 0xffffffe0));
    myX = (float *)((((uint32_t)(sdram_malloc(sz * sizeof(float) + 32)) + 32) & 0xffffffe0));
    Serial.printf("sdram  Buffers: %p \n", myH);
#endif

  Serial.println("SDRAM Initialized");

  //Eigen::Map<Eigen::VectorXf> eigen_x(x.data(), x.size());
  //Eigen::Map<Eigen::VectorXf> eigen_y(y.data(), y.size());
  //Eigen::Map<Eigen::VectorXf> eigen_z(z.data(), z.size());
  float* x_data_ptr = x.data();  // Get pointer to the data
  float* y_data_ptr = y.data();
  float* z_data_ptr = z.data();

   Eigen::Map<Eigen::VectorXf, Eigen::Aligned32> eigen_x(x_data_ptr, sz);
   Eigen::Map<Eigen::VectorXf, Eigen::Aligned32> eigen_y(y_data_ptr, sz);
   Eigen::Map<Eigen::VectorXf, Eigen::Aligned32> eigen_z(z_data_ptr, sz);

  //Lets check some data
  //Serial.println("Checking H transpose data:");
  //for(uint8_t i = 0; i<25; i++)
  //    Serial.printf("%f, %f, %f\n", eigen_x(i), eigen_y(i), eigen_z(i) );

  Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic>, Eigen::Aligned32> H(myH, sz, 6);
    H.col(0) = eigen_x;
    H.col(1) = eigen_y;
    H.col(2) = eigen_z;
    H.col(3) = -eigen_y.array().square();
    H.col(4) = -eigen_z.array().square();
    H.col(5) = Eigen::VectorXf::Ones(sz);

  //Lets check some data
  Serial.println("Checking H data:");
  //Debugging
  //print formatted matrix
  //Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
  //std::cout << H.format(CleanFmt) << std::endl;
  //print x number of rows
  //std::cout << "Matrix H dimensions: " << H.rows() << "x" << H.cols() << std::endl;
  Serial.println("Maxtrix H dimension:");
  Serial.print(H.rows());
  Serial.print("x");
  Serial.print(H.cols());
  Serial.println("\n");

  // Print only the first 10 rows
  //std::cout << H.block(0, 0, 10, H.cols()) << std::endl;
  // Print the block of the matrix
  for (int i = 0; i < 10; ++i) {
    for (int j = 0; j < H.cols(); ++j) {
      Serial.print(H(i, j));
      Serial.print("\t");
    }
    Serial.println();
  }
  Serial.println();

  //Check H matrix for NaNs and Inf's
  if (!H.allFinite()) {
    Serial.println("H Matrix contains NaN or Inf values!");
  } else {
    Serial.println("H Matrix passed finite test!");
  }

  //Check rank and conditioning of matrix H - unfortunate JacobiSVD fail - throws null pointer
    //Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(H);
    //int rank = lu_decomp.rank();
    //std::cout << "Rank: " << rank << std::endl;

    //Eigen::JacobiSVD<Eigen::MatrixXf> svd(H);
    //float cond_number = svd.singularValues()(0) / svd.singularValues().tail(1)(0);
    //std::cout << "Condition Number: " << cond_number << std::endl;

    // Ensure matrix is well-conditioned and has full rank
    //if (rank == H.cols() && cond_number < 1e10) {
    //    std::cout << "Matrix H is well-conditioned and has full rank." << std::endl;
    //} else {
    //    std::cout << "Matrix H is either poorly conditioned or rank-deficient." << std::endl;
    //}

  Eigen::VectorXf w(sz);
 //Eigen::Matrix<float, Eigen::Dynamic, 1> w(sz);
  w = eigen_x * eigen_x;
  Serial.println("w matrix completed (solve)");

  //2 ways to solve for X.
  //Eigen::VectorXf X = (H).jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(w);
  //Eigen::JacobiSVD<Eigen::MatrixXf> svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV);
  //Serial.println("JacobiSVD setup completed");
  //Eigen::VectorXf X = svd.solve(w);
  
    // Perform QR decomposition
    Eigen::FullPivHouseholderQR<Eigen::MatrixXf> qr(H);
    qr.setThreshold(1e-10); // Set a custom threshold
    Serial.println("qr matrix configure");

    // Solve the system
    Eigen::VectorXf X = qr.solve(w);
    //std::cout << "Solution: " << X << std::endl;
  Serial.print("Solution: ");
  for (int i = 0; i < X.rows(); ++i) {
    Serial.print(X(i, 0));
    Serial.print("\t");
  }
  Serial.println("\n");

  float OSx = X[0] / 2;
  float OSy = X[1] / (2 * X[3]);
  float OSz = X[2] / (2 * X[4]);
  
  float A = X[5] + OSx*OSx + X[3] * OSy*OSy + X[4] * OSz*OSz;
  float B = A / X[3];
  float C = A / X[4];
  
  float SCx = sqrt(A);
  float SCy = sqrt(B);
  float SCz = sqrt(C);

  Serial.printf("Offsets: %f, %f, %f\n", OSx, OSy, OSz);
  Serial.printf("Scale: %f, %f, %f\n", SCx, SCy, SCz);
  Serial.println();
  //flexRamInfo();

}

void loop() {
  // put your main code here, to run repeatedly:

}

Now for comparison - from the Python GUI I am getting:

Calibrating from magn.txt
[[ 2.1800e+02  2.0800e+02  2.1600e+02 ... -1.2200e+02 -1.1600e+02
  -1.0700e+02]
 [ 1.8400e+02  1.8200e+02  1.7400e+02 ...  1.8400e+02  1.8400e+02
   1.8300e+02]
 [ 0.0000e+00  0.0000e+00  0.0000e+00 ... -2.8400e+02 -2.8600e+02
  -2.8900e+02]
 [-3.3856e+04 -3.3124e+04 -3.0276e+04 ... -3.3856e+04 -3.3856e+04
  -3.3489e+04]
 [-0.0000e+00 -0.0000e+00 -0.0000e+00 ... -8.0656e+04 -8.1796e+04
  -8.3521e+04]
 [ 1.0000e+00  1.0000e+00  1.0000e+00 ...  1.0000e+00  1.0000e+00
   1.0000e+00]]
H Transposed
[[ 2.1800e+02  1.8400e+02  0.0000e+00 -3.3856e+04 -0.0000e+00  1.0000e+00]
 [ 2.0800e+02  1.8200e+02  0.0000e+00 -3.3124e+04 -0.0000e+00  1.0000e+00]
 [ 2.1600e+02  1.7400e+02  0.0000e+00 -3.0276e+04 -0.0000e+00  1.0000e+00]
 ...
 [-1.2200e+02  1.8400e+02 -2.8400e+02 -3.3856e+04 -8.0656e+04  1.0000e+00]
 [-1.1600e+02  1.8400e+02 -2.8600e+02 -3.3856e+04 -8.1796e+04  1.0000e+00]
 [-1.0700e+02  1.8300e+02 -2.8900e+02 -3.3489e+04 -8.3521e+04  1.0000e+00]]
X
[ 1.71109018e+02  1.52564875e+02 -3.39567453e+02  9.98622702e-01
  8.85129437e-01  1.39174882e+04]

Using Eigen with the Teensy

.... no more data
5000
sdram  Buffers: 0x80000020 
SDRAM Initialized
Checking H data:
Maxtrix H dimension:
5000x6

218.00  184.00  0.00    -33856.00   0.00    1.00    
208.00  182.00  0.00    -33124.00   0.00    1.00    
216.00  174.00  0.00    -30276.00   0.00    1.00    
211.00  173.00  3.00    -29929.00   -9.00   1.00    
207.00  183.00  5.00    -33489.00   -25.00  1.00    
206.00  178.00  2.00    -31684.00   -4.00   1.00    
211.00  177.00  1.00    -31329.00   -1.00   1.00    
198.00  184.00  4.00    -33856.00   -16.00  1.00    
206.00  184.00  6.00    -33856.00   -36.00  1.00    
205.00  177.00  7.00    -31329.00   -49.00  1.00    

H Matrix passed finite test!
w matrix completed (solve)
qr matrix configure
Solution: 218.00    0.00    -0.00   0.00    0.00    -0.00   

Offsets: 109.000122, 732.969482, -22.580416
Scale: 109.000130, 1741022.875000, 135445.125000

I really have no idea on using Eigen so if any one has any ideas please let help

I have tried using

  //Eigen::VectorXf X = (H).jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(w);
  //Eigen::JacobiSVD<Eigen::MatrixXf> svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV);
  //Serial.println("JacobiSVD setup completed");
  //Eigen::VectorXf X = svd.solve(w);

but that cashes a crash:

CrashReport:
  A problem occurred at (system time) 10:13:40
  Code was executing from address 0x7AAE
  CFSR: 82
    (DACCVIOL) Data Access Violation
    (MMARVALID) Accessed Address: 0x0 (nullptr)
      Check code at 0x7AAE - very likely a bug!
      Run "addr2line -e mysketch.ino.elf 0x7AAE" 

which points to: eigen-

main\src/Eigen/src/Core/functors/AssignmentFunctors.h:24
as the issue:   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(DstScalar& a, const SrcScalar& b) const { a = b; }