Practical use of ESP32 code

[brightproject]

Hello @michal34512 😃
I wrote code based on the library.
The code runs on an ESP32-S3 and reads data from the BNO085 sensor.

#include <Arduino.h>
#include <Wire.h>
#include <esp_log.h>
#include "mems_imu/mems_imu.h"

extern "C" {
    #include "mag_calib_new/geometry.h"
}

#define I2C_FREQ 400000
#define I2C_SDA_IMU GPIO_NUM_1
#define I2C_SCL_IMU GPIO_NUM_2
#define SERIAL_BAUD 115200

#define BNO08X_RATE_HZ 200
#define DISPLAY_RATE_HZ 50

#define MAG_CALIB_POINTS 300
#define MAG_CALIB_MIN_DELTA 15

#define BNO08X_TASK_STACK 6 * 1024
#define DISPLAY_TASK_STACK 4 * 1024
#define BNO08X_TASK_PRIORITY 3
#define DISPLAY_TASK_PRIORITY 2
#define BNO08X_TASK_CORE 0
#define DISPLAY_TASK_CORE 1

#define MAG_LOG_ONLY

bool calib_on = true;
// bool calib_on = false;

typedef struct {
    int16_t mx_raw;
    int16_t my_raw;
    int16_t mz_raw;
    
    int16_t mx_calib;
    int16_t my_calib;
    int16_t mz_calib;
    
    float roll_deg;
    float pitch_deg;
    float heading_deg;
    uint8_t accuracy_mag;
} IMUDataExtended;

// Structure for data collection
typedef struct {
    double* x_data;
    double* y_data;
    double* z_data;
    int count;
    int max_points;
} mag_calib_collection_t;

IMUDataExtended imu_data_extended;
mag_calib_collection_t mag_calib_collection;

TwoWire* imuWire = &Wire;
MemsImu mems_bno(imuWire, I2C_SDA_IMU, I2C_SCL_IMU, I2C_FREQ);

Callibration_t magneto_calib_result;
bool magneto_calib_ready = false;
bool magneto_calib_collecting = false;

double mag_target_radius = 1000.0;


static double x_buffer[MAG_CALIB_POINTS];
static double y_buffer[MAG_CALIB_POINTS];
static double z_buffer[MAG_CALIB_POINTS];
static int point_count = 0;

SemaphoreHandle_t i2cMutex = NULL;
SemaphoreHandle_t imuDataMutex = NULL;

TaskHandle_t bno08xTaskHandle = NULL;
TaskHandle_t displayTaskHandle = NULL;

static bool bno08xInitialized = false;

void init_mag_calibration(int max_points) {
    if (mag_calib_collection.x_data != NULL) {
        free(mag_calib_collection.x_data);
        free(mag_calib_collection.y_data);
        free(mag_calib_collection.z_data);
        mag_calib_collection.x_data = NULL;
        mag_calib_collection.y_data = NULL;
        mag_calib_collection.z_data = NULL;
    }
    
    mag_calib_collection.x_data = (double*)malloc(max_points * sizeof(double));
    mag_calib_collection.y_data = (double*)malloc(max_points * sizeof(double));
    mag_calib_collection.z_data = (double*)malloc(max_points * sizeof(double));
    
    if (!mag_calib_collection.x_data || !mag_calib_collection.y_data || !mag_calib_collection.z_data) {
        Serial.println("ERROR: Failed to allocate memory for calibration collection!");
        return;
    }
    
    mag_calib_collection.count = 0;
    mag_calib_collection.max_points = max_points;
    magneto_calib_collecting = true;
    magneto_calib_ready = false;
    
    Serial.printf("Magnetometer calibration started. Collecting %d samples...\n", max_points);
}

bool add_mag_sample(int16_t x, int16_t y, int16_t z) {
    if (!magneto_calib_collecting || mag_calib_collection.count >= mag_calib_collection.max_points) {
        return false;
    }
    
    mag_calib_collection.x_data[mag_calib_collection.count] = (double)x;
    mag_calib_collection.y_data[mag_calib_collection.count] = (double)y;
    mag_calib_collection.z_data[mag_calib_collection.count] = (double)z;
    mag_calib_collection.count++;
    
    Serial.printf("Sample %d/%d: X=%d, Y=%d, Z=%d\n", 
                  mag_calib_collection.count, mag_calib_collection.max_points, x, y, z);
    
    if (mag_calib_collection.count >= mag_calib_collection.max_points) {
        magneto_calib_collecting = false;
        
        if (magneto_calib_ready) {
            calib_free(magneto_calib_result);
            magneto_calib_ready = false;
        }
        
        // Create vectors from data ONCE
        Vector vx = vec_from_array(mag_calib_collection.x_data, mag_calib_collection.count);
        Vector vy = vec_from_array(mag_calib_collection.y_data, mag_calib_collection.count);
        Vector vz = vec_from_array(mag_calib_collection.z_data, mag_calib_collection.count);
        
        double variance_before = square_distance_variance(vx, vy, vz);
        printf("Variance of distances before calibration: %f\n", variance_before);
        
        magneto_calib_result = calib_calibrate_sensor(vx, vy, vz);
        
        bool success = calib_calibration_success(magneto_calib_result);
        printf("Calculating calibration data of data points was %s\n", 
               success ? "SUCCESSFUL" : "UNSUCCESSFUL");
        
        if (success) {
            magneto_calib_ready = true;
            
            // Apply calibration to the same vectors (without creating new ones)
            calib_calibrate_multiple_points(magneto_calib_result, vx, vy, vz);
            
            double variance_after = square_distance_variance(vx, vy, vz);
            printf("Variance of distances after calibration: %f\n", variance_after);
            
            // ========== BIAS И SCALE ==========
            printf("\n=== MAGNETOMETER CALIBRATION PARAMETERS ===\n");
            
            // Access the data through the correct structure fields
            Vector offset = magneto_calib_result.offset; // offset vector
            Matrix transform = magneto_calib_result.transform; // 3x3 transformation matrix
            
            // Output bias for each axis
            printf("Bias (offset) values:\n");
            printf("  X bias: %.3f\n", VEC_X(offset));
            printf("  Y bias: %.3f\n", VEC_Y(offset));
            printf("  Z bias: %.3f\n", VEC_Z(offset));
            
            // Output the transformation matrix (scale + cross-axis)
            // Accessing matrix elements: MAT_ELEM(matrix, row, col)
            printf("\nTransform matrix (scale + rotation/cross-axis):\n");
            printf("  [%.6f, %.6f, %.6f]\n", 
                   MAT_ELEM(transform, 0, 0), MAT_ELEM(transform, 0, 1), MAT_ELEM(transform, 0, 2));
            printf("  [%.6f, %.6f, %.6f]\n", 
                   MAT_ELEM(transform, 1, 0), MAT_ELEM(transform, 1, 1), MAT_ELEM(transform, 1, 2));
            printf("  [%.6f, %.6f, %.6f]\n", 
                   MAT_ELEM(transform, 2, 0), MAT_ELEM(transform, 2, 1), MAT_ELEM(transform, 2, 2));
            
            // Scaling factors (diagonal elements)
            printf("\nScale factors (diagonal elements of transform matrix):\n");
            printf("  X scale factor: %.6f\n", MAT_ELEM(transform, 0, 0));
            printf("  Y scale factor: %.6f\n", MAT_ELEM(transform, 1, 1));
            printf("  Z scale factor: %.6f\n", MAT_ELEM(transform, 2, 2));
            
            // Cross-sensitivity (off-diagonal elements)
            printf("\nCross-axis sensitivity (off-diagonal elements):\n");
            printf("  XY: %.6f, XZ: %.6f\n", MAT_ELEM(transform, 0, 1), MAT_ELEM(transform, 0, 2));
            printf("  YX: %.6f, YZ: %.6f\n", MAT_ELEM(transform, 1, 0), MAT_ELEM(transform, 1, 2));
            printf("  ZX: %.6f, ZY: %.6f\n", MAT_ELEM(transform, 2, 0), MAT_ELEM(transform, 2, 1));
            
            double sum_r_norm = 0;
            for (int i = 0; i < mag_calib_collection.count; i++) {
                double dx = VEC_ELEM(vx, i);
                double dy = VEC_ELEM(vy, i);
                double dz = VEC_ELEM(vz, i);
                sum_r_norm += sqrt(dx*dx + dy*dy + dz*dz);
            }
            double avg_norm_radius = sum_r_norm / mag_calib_collection.count;
            printf("\nAverage normalized radius after calibration: %f (should be ~1.0)\n", avg_norm_radius);
            printf("Target radius for output: %f\n", mag_target_radius);
            printf("=============================================\n");
        }
        
        // Free the vectors (only once)
        vec_free(vx);
        vec_free(vy);
        vec_free(vz);
        
        return true;
    }
    return false;
}

void apply_mag_calibration_to_data(int16_t raw_x, int16_t raw_y, int16_t raw_z, 
                                    int16_t* calib_x, int16_t* calib_y, int16_t* calib_z) {
    if (!magneto_calib_ready) {
        *calib_x = raw_x;
        *calib_y = raw_y;
        *calib_z = raw_z;
        return;
    }
    
    Vector dataPoint = vec_new(3);
    VEC_X(dataPoint) = (double)raw_x;
    VEC_Y(dataPoint) = (double)raw_y;
    VEC_Z(dataPoint) = (double)raw_z;
    
    calib_calibrate_point(magneto_calib_result, dataPoint);
    
    *calib_x = (int16_t)round(VEC_X(dataPoint) * mag_target_radius);
    *calib_y = (int16_t)round(VEC_Y(dataPoint) * mag_target_radius);
    *calib_z = (int16_t)round(VEC_Z(dataPoint) * mag_target_radius);
    
    vec_free(dataPoint);
}

void bno08x_task(void *pvParameters) {
    TickType_t xLastWakeTime = xTaskGetTickCount();
    const TickType_t xFrequency = pdMS_TO_TICKS(1000 / BNO08X_RATE_HZ);
    
    while (1) {
        vTaskDelayUntil(&xLastWakeTime, xFrequency);
        
        if (bno08xInitialized) {
            IMUData bno_data; 
            
            xSemaphoreTake(i2cMutex, portMAX_DELAY);
            mems_bno.update();
            bool gotEvent = mems_bno.getIMUData(bno_data);
            xSemaphoreGive(i2cMutex);
            
            if (gotEvent) {
                xSemaphoreTake(imuDataMutex, portMAX_DELAY);
                
                imu_data_extended.mx_raw = bno_data.mx_raw;
                imu_data_extended.my_raw = bno_data.my_raw;
                imu_data_extended.mz_raw = bno_data.mz_raw;
                imu_data_extended.roll_deg = bno_data.roll_deg;
                imu_data_extended.pitch_deg = bno_data.pitch_deg;
                imu_data_extended.heading_deg = bno_data.heading_deg;
                imu_data_extended.accuracy_mag = bno_data.accuracy_mag;
                
                apply_mag_calibration_to_data(
                    imu_data_extended.mx_raw, imu_data_extended.my_raw, imu_data_extended.mz_raw,
                    &imu_data_extended.mx_calib, &imu_data_extended.my_calib, &imu_data_extended.mz_calib
                );
                
                xSemaphoreGive(imuDataMutex);
            }
            
            if (calib_on && !magneto_calib_collecting && !magneto_calib_ready) {
                Serial.println("\n=== Starting MAGNETOMETER CALIBRATION ===");
                init_mag_calibration(MAG_CALIB_POINTS);
                
                int valid_samples = 0;
                int32_t last_mx = 0, last_my = 0, last_mz = 0;
                
                while (valid_samples < MAG_CALIB_POINTS && magneto_calib_collecting) {
                    xSemaphoreTake(i2cMutex, portMAX_DELAY);
                    mems_bno.update();
                    mems_bno.getIMUData(bno_data);
                    xSemaphoreGive(i2cMutex);
                    
                    if (abs(bno_data.mx_raw - last_mx) > MAG_CALIB_MIN_DELTA || 
                        abs(bno_data.my_raw - last_my) > MAG_CALIB_MIN_DELTA || 
                        abs(bno_data.mz_raw - last_mz) > MAG_CALIB_MIN_DELTA) {
                        
                        add_mag_sample(bno_data.mx_raw, bno_data.my_raw, bno_data.mz_raw);
                        
                        last_mx = bno_data.mx_raw;
                        last_my = bno_data.my_raw;
                        last_mz = bno_data.mz_raw;
                        valid_samples++;
                    }
                    vTaskDelay(pdMS_TO_TICKS(20));
                }
                
                calib_on = false;
                Serial.println("\n=== Magnetometer calibration COMPLETED ===");
            }
        }
    }
}

void display_task(void *pvParameters) {
    TickType_t xLastWakeTime = xTaskGetTickCount();
    const TickType_t xFrequency = pdMS_TO_TICKS(1000 / DISPLAY_RATE_HZ);
    
    while (1) {
        vTaskDelayUntil(&xLastWakeTime, xFrequency);
        
        if (bno08xInitialized && !calib_on) {
            xSemaphoreTake(imuDataMutex, portMAX_DELAY);

            #ifdef MAG_LOG_ONLY
            // Serial.printf("RAW Mag: X=%5d Y=%5d Z=%5d | CALIB Mag: X=%5d Y=%5d Z=%5d | Acc: %d\n",
            //                 imu_data_extended.mx_raw, imu_data_extended.my_raw, imu_data_extended.mz_raw,
            //                 imu_data_extended.mx_calib, imu_data_extended.my_calib, imu_data_extended.mz_calib,
            //                 imu_data_extended.accuracy_mag);

            // For data collection and processing through a buffer
            if (point_count < MAG_CALIB_POINTS) {
                x_buffer[point_count] = imu_data_extended.mx_calib * 0.062500f;
                y_buffer[point_count] = imu_data_extended.my_calib * 0.062500f;
                z_buffer[point_count] = imu_data_extended.mz_calib * 0.062500f;
                point_count++;
                
                if (point_count == MAG_CALIB_POINTS) {
                    Serial.printf("double x[%d] = {", MAG_CALIB_POINTS);
                    for (int i = 0; i < MAG_CALIB_POINTS; i++) {
                        Serial.printf("%.6f%s", x_buffer[i], i < MAG_CALIB_POINTS - 1 ? ", " : "");
                    }
                    Serial.println("};");
                    
                    Serial.printf("double y[%d] = {", MAG_CALIB_POINTS);
                    for (int i = 0; i < MAG_CALIB_POINTS; i++) {
                        Serial.printf("%.6f%s", y_buffer[i], i < MAG_CALIB_POINTS - 1 ? ", " : "");
                    }
                    Serial.println("};");
                    
                    Serial.printf("double z[%d] = {", MAG_CALIB_POINTS);
                    for (int i = 0; i < MAG_CALIB_POINTS; i++) {
                        Serial.printf("%.6f%s", z_buffer[i], i < MAG_CALIB_POINTS - 1 ? ", " : "");
                    }
                    Serial.println("};");
                    
                    point_count = 0;
                }
            }

            #else               
            Serial.printf("Roll: %.2f Pitch: %.2f Heading: %.2f | RAW Mag: X=%5d Y=%5d Z=%5d | CALIB Mag: X=%5d Y=%5d Z=%5d | Acc: %d\n",
                            imu_data_extended.roll_deg, imu_data_extended.pitch_deg, imu_data_extended.heading_deg,
                            imu_data_extended.mx_raw, imu_data_extended.my_raw, imu_data_extended.mz_raw,
                            imu_data_extended.mx_calib, imu_data_extended.my_calib, imu_data_extended.mz_calib,
                            imu_data_extended.accuracy_mag);
            #endif
            
            xSemaphoreGive(imuDataMutex);
        }
    }
}

void setup() {
    Serial.begin(SERIAL_BAUD);
    delay(1000);
    
    Serial.println("\n=================================");
    Serial.println("BNO08X sensor with Ellipsoid calibration");
    Serial.println("=================================");
    
    // Инициализация структуры
    memset(&mag_calib_collection, 0, sizeof(mag_calib_collection_t));
    
    i2cMutex = xSemaphoreCreateMutex();
    imuDataMutex = xSemaphoreCreateMutex();
    
    if (!i2cMutex || !imuDataMutex) {
        Serial.println("ERROR: Failed to create mutexes!");
        while(1) delay(1000);
    }
    
    if (mems_bno.begin()) {
        bno08xInitialized = true;
        Serial.println("BNO08x initialized OK.");
        
        sh2_Quaternion_t sh_orientation = { 0, 0, -sqrt(2) / 2, sqrt(2) / 2};
        mems_bno.setOrientation(sh_orientation);
        
        delay(20);
        
        xTaskCreatePinnedToCore(bno08x_task, "bno08x_task", 
            BNO08X_TASK_STACK, NULL, BNO08X_TASK_PRIORITY, 
            &bno08xTaskHandle, BNO08X_TASK_CORE);
            
        delay(20);
        
        xTaskCreatePinnedToCore(display_task, "display_task", 
            DISPLAY_TASK_STACK, NULL, DISPLAY_TASK_PRIORITY, 
            &displayTaskHandle, DISPLAY_TASK_CORE);
            
        delay(20);
        
        Serial.println("Tasks started");
        if (calib_on) {
            Serial.println("Calibration will start automatically...");
            Serial.println("Move the sensor in figure-8 pattern");
        }
    } else {
        Serial.println("Failed to initialize BNO08x!");
        while (1) { 
            // a crutch for rebooting the MK
            delay(10); 
            ESP.restart();
        }
    }
    
    vTaskDelete(NULL);
}

void loop() {
    vTaskDelay(pdMS_TO_TICKS(1000));
}

My project structure is similar to the original one.

main.cpp
├── mag_calib_new
│   ├── eigen.c
│   ├── eigen.h
│   ├── ellipsoid_fit.c
│   ├── ellipsoid_fit.h
│   ├── geometry.h
│   ├── LICENSE
│   ├── main.c0
│   ├── make.bat
│   ├── matrix.c
│   ├── matrix.h
│   ├── qr.c
│   ├── qr.h
│   ├── README.md
│   ├── sensor_calibration.c
│   ├── sensor_calibration.h
│   ├── vector.c
│   ├── vector.h
│   ├── wmm_compensation.c
│   └── wmm_compensation.h

The code collects 300 samples, or some other value.
I use raw data, not converted to microTesla.

=====================================
BNO08X sensor with Ellipsoid calibration
=======================================
BNO08x initialized OK.

=== Starting MAGNETOMETER CALIBRATION ===
Magnetometer calibration started. Collecting 300 samples...
Tasks started
Calibration will start automatically...
Move the sensor in figure-8 pattern
Sample 1/300: X=138, Y=849, Z=-484
Sample 2/300: X=5, Y=-208, Z=-677
Sample 3/300: X=-451, Y=711, Z=-574
...
Sample 297/300: X=191, Y=295, Z=-721
Sample 298/300: X=-370, Y=11, Z=-760
Sample 299/300: X=-382, Y=387, Z=-740
Variance of distances before calibration: 20184.154681
Sample 300/300: X=-40, Y=700, Z=-637
Calculating calibration data of data points was SUCCESSFUL
Variance of distances after calibration: 0.000714

=== MAGNETOMETER CALIBRATION PARAMETERS ===
Bias (offset) values:
  X bias: -169.266
  Y bias: 217.206
  Z bias: -23.836

Transform matrix (scale + rotation/cross-axis):
  [0.001200, -0.000012, -0.000018]
  [-0.000012, 0.001172, 0.000003]
  [-0.000018, 0.000003, 0.001320]

Scale factors (diagonal elements of transform matrix):
  X scale factor: 0.001200
  Y scale factor: 0.001172
  Z scale factor: 0.001320

Cross-axis sensitivity (off-diagonal elements):
  XY: -0.000012, XZ: -0.000018
  YX: -0.000012, YZ: 0.000003
  ZX: -0.000018, ZY: 0.000003

Average normalized radius after calibration: 0.999646 (should be ~1.0)
Target radius for output: 1000.000000
=============================================

Usually I used the MAGNETO method and got the bias and scale, and then applied them according to this formula.
But here the bias values ​​are somehow huge, which made me doubt the correctness of my adaptation of the source code.

temp_mag[3];

temp_mag[0] = (mx_raW - (float)db[compass::M_BAinv_0_0]);
temp_mag[1] = (my_raW - (float)db[compass::M_BAinv_0_1]);
temp_mag[2] = (mz_raW - (float)db[compass::M_BAinv_0_2]);

Mxyz[0] = (float)db[compass::M_BAinv_1_0] * temp_mag[0] + (float)db[compass::M_BAinv_1_1] * temp_mag[1]
+ (float)db[compass::M_BAinv_1_2] * temp_mag[2];
Mxyz[1] = (float)db[compass::M_BAinv_2_0] * temp_mag[0] + (float)db[compass::M_BAinv_2_1] * temp_mag[1]
+ (float)db[compass::M_BAinv_2_2] * temp_mag[2];
Mxyz[2] = (float)db[compass::M_BAinv_3_0] * temp_mag[0] + (float)db[compass::M_BAinv_3_1] * temp_mag[1]
+ (float)db[compass::M_BAinv_3_2] * temp_mag[2];

In this code I expected something similar, but I didn’t get any scale or matrices right away.

But if I comment out the line in the code,

#define MAG_LOG_ONLY

After calibration, I get both uncalibrated and calibrated values, and the calibrated values ​​magically calibrate—not perfectly, but still.
I became curious about how the code works and whether it correctly fits the ellipsoid using the least-squares method.
I wrote another test code, for which I collected three sets of samples: 20, 100, and 300 points along each axis of the magnetometer.
Here I have already collected samples in microTesla.

#define _USE_MATH_DEFINES
#include <Arduino.h>

#include <stdio.h>
#include <stdlib.h>
#include <time.h>

extern "C" {
    #include "mag_calib_new/geometry.h"
}

#define SERIAL_BAUD 115200

#define MIN_DATA_LEN 15

// Select mode: 1 - 20 samples, 2 - 100 samples, 3 - 300 samples
#define DATA_MODE 3

// DEMO OF MAGNETOMETER CALIBRATION USING REAL DATA
// This demo:
// 1. Takes real magnetometer data points
// 2. Calculates calibration data (offset vector and transformation matrix)
// 3. Checks if calibration was successful
// 4. Calibrates the data points

typedef struct {
    Vector vx;
    Vector vy;
    Vector vz;
} mag_data_t;


#if DATA_MODE == 1

// Variance of distances before calibration: 74.736556
// Variance of distances after calibration: 0.013583

    #define MAX_DATA_LEN 20

    double x[20] = {
        0.312500,   6.500000,   18.437500,  30.000000,  34.000000,   35.812500,  32.937500,  30.000000,  19.875000,  9.750000,
        -1.437500,  -10.125000, -16.625000, -23.875000, -28.937500,  -28.562500, -26.375000, -19.875000, -14.812500, -6.875000
    };

    double y[20] = {
        12.250000,  22.062500,  32.187500,  35.812500,  38.687500,   36.875000,  32.187500,  22.437500,  9.000000,    -4.312500,
        -15.875000, -22.750000, -25.312500, -17.687500, -5.062500,   8.687500,   26.750000,  33.250000,  40.187500,  48.875000
    };

    double z[20] = {
        -44.750000, -41.937500, -35.937500, -37.562500, -39.187500,  -42.437500, -44.062500, -46.875000, -47.250000, -46.062500,
        -41.625000, -36.375000, -32.312500, -28.312500, -28.312500,  -24.250000, -23.062500, -20.625000, -22.250000, -26.687500
    };

#elif DATA_MODE == 2

// Variance of distances before calibration: 99.927141
// Variance of distances after calibration: 0.063912

    #define MAX_DATA_LEN 100

    double x[100] = {
        -11.562500, -17.687500, -22.062500, -24.937500, -25.312500, -23.500000, -19.187500, -14.437500, -10.812500, -4.687500,
        0.312500, 3.250000, 6.500000, 14.062500, 18.437500, 24.250000, 30.000000, 32.937500, 34.000000, 35.062500,
        35.812500, 34.750000, 32.937500, 31.500000, 30.000000, 24.250000, 19.875000, 13.750000, 9.750000, 3.250000,
        -1.437500, -4.687500, -10.125000, -14.812500, -16.625000, -21.000000, -23.875000, -26.750000, -28.937500, -28.937500,
        -28.562500, -27.875000, -26.375000, -22.812500, -19.875000, -15.875000, -14.812500, -10.812500, -6.875000, -1.750000,
        3.250000, 8.687500, 14.437500, 19.187500, 24.562500, 27.875000, 31.812500, 34.750000, 35.812500, 35.437500,
        35.437500, 35.437500, 34.375000, 33.625000, 32.562500, 30.375000, 30.375000, 28.187500, 22.062500, 20.625000,
        12.250000, 11.937500, 5.750000, 0.687500, -3.250000, -8.687500, -14.062500, -16.625000, -20.625000, -25.687500,
        -28.187500, -31.812500, -35.125000, -38.375000, -36.187500, -38.687500, -39.062500, -41.625000, -41.250000, -41.625000
    };

    double y[100] = {
        12.250000, 17.000000, 22.062500, 27.125000, 32.187500, 35.437500, 35.812500, 38.000000, 38.687500, 40.125000,
        36.875000, 34.375000, 32.187500, 25.312500, 22.437500, 14.437500, 9.000000, 2.125000, -4.312500, -10.125000,
        -15.875000, -18.437500, -22.750000, -24.250000, -25.312500, -23.500000, -17.687500, -13.000000, -5.062500, 1.062500,
        8.687500, 18.437500, 26.750000, 30.375000, 33.250000, 36.875000, 40.187500, 45.250000, 48.875000, 49.562500,
        50.312500, 47.375000, 45.937500, 42.687500, 39.062500, 34.750000, 30.750000, 27.125000, 26.750000, 23.500000,
        18.062500, 14.437500, 14.437500, 3.250000, -0.312500, -4.312500, -9.375000, -14.812500, -19.875000, -23.125000,
        -26.750000, -28.937500, -27.875000, -26.062500, -23.500000, -20.250000, -13.375000, -7.562500, -1.062500, 6.500000,
        11.937500, 18.812500, 25.687500, 30.375000, 34.000000, 36.187500, 38.687500, 41.625000, 42.312500, 43.062500,
        44.125000, 44.500000, 43.062500, 39.062500, 34.000000, 28.937500, 27.500000, 23.125000, 17.687500, 13.750000,
        9.750000, 9.750000, 12.250000, 13.750000, 17.375000, 17.000000, 14.062500, 12.625000, 9.375000, 7.187500,
    };

    double z[100] = {
        -44.750000, -43.562500, -41.937500, -37.125000, -35.937500, -33.875000, -37.562500, -37.937500, -39.187500, -40.375000,
        -42.437500, -44.437500, -44.062500, -46.062500, -46.875000, -45.687500, -47.250000, -46.062500, -46.062500, -44.437500,
        -41.625000, -38.812500, -36.375000, -33.937500, -32.312500, -30.312500, -28.312500, -29.125000, -28.312500, -27.875000,
        -24.250000, -25.875000, -23.062500, -20.625000, -20.625000, -21.000000, -22.250000, -24.625000, -25.500000, -26.687500,
        -30.312500, -31.562500, -35.562500, -38.000000, -40.062500, -42.875000, -45.687500, -45.312500, -46.937500, -45.312500,
        -44.875000, -47.312500, -46.562500, -46.125000, -44.500000, -46.125000, -44.125000, -41.250000, -40.062500, -37.250000,
        -35.187500, -29.937500, -31.125000, -29.937500, -30.750000, -28.687500, -29.500000, -29.125000, -27.500000, -27.125000,
        -25.500000, -23.875000, -21.437500, -20.187500, -20.187500, -20.187500, -20.625000, -21.812500, -23.437500, -27.937500,
        -27.937500, -34.000000, -40.875000, -43.312500, -46.937500, -48.187500, -50.187500, -49.375000, -49.812500, -48.562500,
        -48.562500, -46.937500, -43.312500, -42.875000, -42.062500, -42.500000, -41.250000, -39.250000, -40.062500, -40.437500
    };

#elif DATA_MODE == 3

// Variance of distances before calibration: 49.279169
// Variance of distances after calibration: 0.000708

    #define MAX_DATA_LEN 300

    double x[MAX_DATA_LEN] = {
        9.375000, 11.937500, 13.750000, 14.812500, 17.312500, 16.625000, 19.500000, 18.812500, 19.500000, 18.812500,
        18.437500, 16.625000, 14.812500, 12.625000, 9.750000, 7.937500, 6.125000, 2.500000, 0.000000, -2.875000,
        -4.312500, -8.687500, -11.562500, -14.812500, -17.687500, -20.625000, -22.062500, -23.125000, -24.937500, -24.250000,
        -25.312500, -24.937500, -23.500000, -21.312500, -19.187500, -17.000000, -14.437500, -13.375000, -10.812500, -8.312500,
        -4.687500, -3.250000, 0.312500, 1.062500, 3.250000, 4.312500, 6.500000, 8.687500, 14.062500, 14.812500,
        18.437500, 21.000000, 24.250000, 26.750000, 30.000000, 31.125000, 32.937500, 32.937500, 34.000000, 35.812500,
        35.062500, 35.062500, 35.812500, 34.750000, 34.750000, 33.625000, 32.937500, 32.562500, 31.500000, 30.750000,
        30.000000, 25.687500, 24.250000, 21.687500, 19.875000, 18.062500, 13.750000, 11.562500, 9.750000, 5.750000,
        3.250000, 1.062500, -1.437500, -3.562500, -4.687500, -7.562500, -10.125000, -12.625000, -14.812500, -16.625000,
        -16.625000, -19.500000, -21.000000, -23.125000, -23.875000, -25.687500, -26.750000, -27.500000, -28.937500, -28.937500,
        -28.937500, -28.937500, -28.562500, -28.937500, -27.875000, -27.875000, -26.375000, -24.562500, -22.812500, -22.062500,
        -19.875000, -18.062500, -15.875000, -15.875000, -14.812500, -12.250000, -10.812500, -8.312500, -6.875000, -4.312500,
        -1.750000, 1.062500, 3.250000, 6.500000, 8.687500, 11.187500, 14.437500, 17.000000, 19.187500, 22.062500,
        24.562500, 27.125000, 27.875000, 30.375000, 31.812500, 32.937500, 34.750000, 35.812500, 35.812500, 35.437500,
        35.437500, 35.437500, 37.250000, 35.437500, 34.375000, 34.000000, 33.625000, 32.187500, 32.562500, 30.375000,
        30.375000, 26.750000, 28.187500, 26.062500, 22.062500, 18.437500, 20.625000, 14.812500, 12.250000, 11.937500,
        11.937500, 7.187500, 5.750000, 3.250000, 0.687500, 1.062500, -3.250000, -5.750000, -8.687500, -10.500000,
        -14.062500, -14.437500, -16.625000, -17.375000, -20.625000, -22.437500, -25.687500, -26.750000, -28.187500, -30.750000,
        -31.812500, -33.312500, -35.125000, -36.562500, -38.375000, -38.375000, -36.187500, -38.000000, -38.687500, -38.687500,
        -39.062500, -39.812500, -41.625000, -42.687500, -41.250000, -41.625000, -41.625000, -40.187500, -38.687500, -35.125000,
        -31.500000, -28.562500, -24.937500, -23.125000, -21.000000, -18.437500, -16.250000, -14.437500, -12.625000, -10.812500,
        -10.125000, -9.000000, -8.687500, -7.187500, -5.750000, -5.750000, -5.750000, -5.750000, -5.750000, -5.750000,
        6.500000, 8.687500, 11.187500, 11.937500, 13.000000, 14.812500, 15.187500, 14.812500, 13.000000, 12.625000,
        11.562500, 8.687500, 5.062500, 2.500000, -1.062500, -5.375000, -9.375000, -12.625000, -17.687500, -21.312500,
        -26.062500, -29.312500, -32.562500, -33.312500, -33.625000, -31.812500, -32.562500, -33.312500, -29.312500, -26.062500,
        -25.687500, -20.250000, -17.687500, -15.562500, -14.062500, -13.750000, -12.312500, -10.500000, -7.187500, -6.500000,
        -4.312500, -2.500000, -1.437500, -1.750000, -0.687500, -1.062500, -1.437500, -1.750000, -2.875000, -5.750000,
        -7.562500, -8.687500, -13.750000, -13.375000, -12.312500, -13.000000, -15.187500, -16.625000, -15.562500, -13.750000,
        -15.562500, -14.812500, -11.937500, -11.187500, -9.375000, -9.375000, -9.750000, -9.750000, -7.187500, -8.312500,
        -7.562500, -6.125000, -2.125000, -1.750000, 0.000000, 1.750000, 3.250000, 4.687500, 6.500000, 5.062500
    };

    double y[MAX_DATA_LEN] = {
        12.250000, 14.437500, 17.000000, 19.500000, 22.062500, 25.312500, 27.125000, 29.625000, 32.187500, 32.937500,
        35.437500, 35.437500, 35.812500, 37.250000, 38.000000, 38.375000, 38.687500, 40.875000, 40.125000, 38.375000,
        36.875000, 34.750000, 34.375000, 32.187500, 27.500000, 25.312500, 22.437500, 18.812500, 14.437500, 11.187500,
        9.000000, 6.500000, 2.125000, -0.312500, -4.312500, -7.187500, -10.125000, -13.000000, -15.875000, -17.687500,
        -18.437500, -20.625000, -22.750000, -24.937500, -24.250000, -24.937500, -25.312500, -24.937500, -23.500000, -20.625000,
        -17.687500, -15.187500, -13.000000, -8.687500, -5.062500, -1.437500, 1.062500, 4.312500, 8.687500, 12.625000,
        18.437500, 22.750000, 26.750000, 28.187500, 30.375000, 31.500000, 33.250000, 35.062500, 36.875000, 39.062500,
        40.187500, 43.062500, 45.250000, 46.312500, 48.875000, 48.875000, 49.562500, 51.375000, 50.312500, 50.312500,
        47.375000, 47.750000, 45.937500, 44.500000, 42.687500, 40.500000, 39.062500, 36.875000, 34.750000, 33.312500,
        30.750000, 29.312500, 27.125000, 26.062500, 26.750000, 25.687500, 23.500000, 21.312500, 18.062500, 14.437500,
        14.437500, 14.437500, 5.750000, 3.250000, -0.312500, -2.875000, -4.312500, -7.562500, -9.375000, -11.562500,
        -14.812500, -19.187500, -19.875000, -22.437500, -23.125000, -25.687500, -26.750000, -26.750000, -27.125000, -27.875000,
        -28.937500, -28.187500, -27.875000, -26.375000, -26.062500, -24.937500, -23.500000, -22.437500, -20.250000, -14.812500,
        -13.375000, -9.750000, -7.562500, -5.375000, -1.062500, 2.125000, 6.500000, 9.375000, 11.937500, 15.875000,
        18.812500, 22.437500, 25.687500, 28.187500, 30.375000, 32.187500, 34.000000, 35.125000, 36.187500, 38.687500,
        40.500000, 41.625000, 42.312500, 43.437500, 43.062500, 44.125000, 44.500000, 44.125000, 43.062500, 39.062500,
        36.875000, 34.000000, 31.812500, 28.937500, 27.500000, 26.062500, 23.125000, 20.625000, 17.687500, 16.625000,
        13.750000, 12.625000, 9.750000, 9.750000, 10.125000, 11.562500, 12.250000, 13.750000, 13.750000, 14.062500,
        17.375000, 17.375000, 17.000000, 14.812500, 14.062500, 11.187500, 12.625000, 10.812500, 9.375000, 10.500000,
        7.187500, 6.875000, 6.875000, 6.125000, 7.187500, 7.937500, 7.187500, 8.687500, 7.187500, 7.937500,
        6.125000, 5.375000, 3.250000, 1.062500, -2.125000, -5.375000, -10.125000, -13.000000, -16.625000, -20.625000,
        -24.562500, -24.562500, -26.375000, -28.562500, -32.937500, -32.937500, -32.937500, -32.937500, -32.937500, -32.937500,
        -34.750000, -32.937500, -31.500000, -29.312500, -27.125000, -23.125000, -20.250000, -17.375000, -13.750000, -10.812500,
        -5.750000, 0.312500, 4.312500, 7.937500, 10.812500, 12.625000, 15.875000, 18.437500, 21.000000, 23.500000,
        27.125000, 30.000000, 34.000000, 35.125000, 39.437500, 40.875000, 43.062500, 43.437500, 43.437500, 45.250000,
        44.500000, 45.625000, 45.250000, 44.875000, 43.437500, 42.687500, 40.500000, 38.000000, 35.812500, 33.312500,
        29.687500, 25.312500, 20.625000, 15.562500, 10.500000, 9.375000, 6.500000, 5.750000, 2.125000, -1.750000,
        -3.250000, -4.687500, -7.187500, -10.500000, -15.562500, -18.062500, -21.687500, -23.875000, -26.062500, -27.500000,
        -28.562500, -29.687500, -29.687500, -31.500000, -33.312500, -33.312500, -33.312000, -34.375000, -34.000000, -33.625000,
        -33.625000, -30.375000, -28.937500, -27.500000, -25.312500, -25.312500, -24.625000, -22.812500, -19.500000, -15.562500
    };

    double z[MAX_DATA_LEN] = {
        -44.750000, -44.375000, -43.562500, -41.937500, -41.937500, -39.125000, -37.125000, -36.312500, -35.937500, -36.312500,
        -33.875000, -35.937500, -37.562500, -37.562500, -37.937500, -40.000000, -39.187500, -40.812500, -40.375000, -42.000000,
        -42.437500, -44.437500, -44.437500, -44.062500, -46.062500, -45.625000, -46.875000, -46.875000, -45.687500, -45.687500,
        -47.250000, -45.687500, -46.062500, -45.250000, -46.062500, -44.875000, -44.437500, -43.250000, -41.625000, -39.625000,
        -38.812500, -38.375000, -36.375000, -34.750000, -33.937500, -32.312500, -32.312500, -30.312500, -28.312500, -29.500000,
        -29.125000, -29.937500, -28.312500, -28.687500, -27.875000, -26.687500, -24.250000, -26.312500, -25.875000, -25.062500,
        -23.062500, -21.000000, -20.625000, -21.000000, -20.625000, -21.437500, -21.000000, -20.625000, -22.250000, -21.812500,
        -24.625000, -24.687500, -25.500000, -27.062500, -26.687500, -25.062500, -30.312500, -29.937500, -31.562500, -33.562500,
        -35.562500, -36.000000, -38.000000, -40.062500, -40.062500, -41.250000, -42.875000, -43.687500, -45.687500, -45.687500,
        -45.312500, -46.500000, -46.937500, -45.312500, -46.500000, -46.125000, -44.875000, -43.687500, -47.312500, -46.562500,
        -46.562500, -46.562500, -46.125000, -44.500000, -44.500000, -46.125000, -44.500000, -44.937500, -44.125000, -42.875000,
        -41.250000, -40.062500, -40.062500, -38.437500, -37.250000, -36.375000, -35.187500, -34.000000, -29.937500, -31.937500,
        -31.125000, -30.312500, -29.937500, -29.937500, -30.750000, -29.125000, -28.687500, -27.937500, -29.500000, -29.937500,
        -29.125000, -29.500000, -27.500000, -27.500000, -27.125000, -26.687500, -25.500000, -24.687500, -23.875000, -23.437500,
        -21.437500, -21.000000, -20.187500, -18.187500, -20.187500, -17.812500, -20.187500, -20.625000, -20.187500, -21.812500,
        -21.812500, -23.062500, -23.437500, -24.687500, -27.937500, -29.125000, -27.937500, -32.375000, -34.000000, -37.250000,
        -40.875000, -41.687500, -43.312500, -45.312500, -46.937500, -45.312500, -48.187500, -49.375000, -50.187500, -50.187500,
        -49.375000, -49.000000, -49.812500, -49.000000, -48.562500, -49.000000, -48.562500, -46.562500, -46.562500, -45.750000,
        -46.937500, -43.312500, -43.312500, -42.875000, -41.687500, -42.062500, -42.125000, -42.500000, -42.500000, -41.312500,
        -41.250000, -40.437500, -39.250000, -38.875000, -40.062500, -39.250000, -40.437500, -41.250000, -41.312500, -42.500000,
        -44.125000, -44.937500, -46.562500, -46.562500, -45.750000, -44.500000, -44.500000, -41.687500, -39.250000, -36.812500,
        -35.187500, -33.562500, -31.187500, -29.125000, -23.875000, -23.875000, -23.875000, -23.875000, -23.875000, -23.875000,
        -16.187500, -17.812500, -19.812500, -21.812500, -25.062500, -28.312500, -31.187500, -34.375000, -38.062500, -40.437500,
        -42.062500, -44.937500, -46.125000, -48.187500, -48.562500, -46.937500, -49.000000, -49.375000, -49.000000, -46.937500,
        -45.312500, -41.687500, -40.062500, -39.687500, -37.625000, -36.437500, -34.375000, -35.187500, -36.437500, -34.812500,
        -36.437500, -37.250000, -38.437500, -39.250000, -40.062500, -40.500000, -41.312500, -43.312500, -43.312500, -44.937500,
        -45.312500, -44.937500, -44.500000, -47.375000, -46.125000, -47.375000, -46.937500, -46.937500, -45.750000, -44.937500,
        -44.937500, -45.312500, -42.937500, -42.500000, -40.062500, -38.437500, -35.187500, -32.750000, -32.000000, -27.937500,
        -29.125000, -28.312500, -27.500000, -25.062500, -22.250000, -22.687500, -21.875000, -22.687500, -23.062500, -24.687500,
        -27.125000, -28.750000, -30.750000, -32.375000, -33.187500, -32.375000, -33.187500, -34.812500, -37.625000, -40.062500
    };

#else
    #error "DATA_MODE must be 1, 2, or 3!"
#endif

void setup() {
    Serial.begin(115200);
    
    delay(100);
    
    Serial.println("=== Magnetometer Calibration Demo ===");
    Serial.println("Starting calibration process...");
    
    // Create vectors based on data arrays
    // We allocate memory on the heap, since the library will try to free() the passed pointer when calling vec_free()
    // and static arrays cannot be freed.
    double* x_dyn = (double*)malloc(MAX_DATA_LEN * sizeof(double));
    double* y_dyn = (double*)malloc(MAX_DATA_LEN * sizeof(double));
    double* z_dyn = (double*)malloc(MAX_DATA_LEN * sizeof(double));

    if (x_dyn == NULL || y_dyn == NULL || z_dyn == NULL) {
        Serial.println("ERROR: Failed to allocate memory for arrays");
        free(x_dyn); free(y_dyn); free(z_dyn);
        return;
    }

    memcpy(x_dyn, x, MAX_DATA_LEN * sizeof(double));
    memcpy(y_dyn, y, MAX_DATA_LEN * sizeof(double));
    memcpy(z_dyn, z, MAX_DATA_LEN * sizeof(double));

    Vector vx = vec_from_array(x_dyn, MAX_DATA_LEN);
    Vector vy = vec_from_array(y_dyn, MAX_DATA_LEN);
    Vector vz = vec_from_array(z_dyn, MAX_DATA_LEN);
    
    if (vx == NULL || vy == NULL || vz == NULL) {
        Serial.println("ERROR: Failed to create vectors");
        if (vx != NULL) vec_free(vx);
        if (vy != NULL) vec_free(vy);
        if (vz != NULL) vec_free(vz);
        // Clearing temporary buffers in case of vector creation error
        free(x_dyn); free(y_dyn); free(z_dyn);
        return;
    }
    
    // Calculate calibration data (offset vector and transformation matrix)
    Callibration_t calib = calib_calibrate_sensor(vx, vy, vz);
    
    // Check if calibration was successful
    Serial.print("Calculating calibration data of data points was ");
    if (calib_calibration_success(calib)) {
        Serial.println("SUCCESSFUL");
    } else {
        Serial.println("UNSUCCESSFUL");
        calib_free(calib);
        vec_free(vx);
        vec_free(vy);
        vec_free(vz);
        return;
    }
    
    // Print variance before calibration
    Serial.print("Variance of distances before calibration: ");
    Serial.println(square_distance_variance(vx, vy, vz), 6);
    
    // Calibrate single point
    Vector dataPoint = vec_new(3);
    if (dataPoint != NULL) {
        VEC_X(dataPoint) = VEC_ELEM(vx, 0);
        VEC_Y(dataPoint) = VEC_ELEM(vy, 0);
        VEC_Z(dataPoint) = VEC_ELEM(vz, 0);
        calib_calibrate_point(calib, dataPoint);
        
        Vector gravity = vec_new(3);
        if (gravity != NULL) {
            VEC_X(gravity) = 0;
            VEC_Y(gravity) = 0;
            VEC_Z(gravity) = 1;
            wmm_compensate(dataPoint, gravity, 5.9, 68.2);
            vec_free(gravity);
        }
        vec_free(dataPoint);
    }
    
    // Calibrate all data points
    calib_calibrate_multiple_points(calib, vx, vy, vz);
    
    // Print variance after calibration
    Serial.print("Variance of distances after calibration: ");
    Serial.println(square_distance_variance(vx, vy, vz), 6);

    // Print the result
    vec_print(calib.offset);
    mat_print(calib.transform);
    
    // Free allocated memory
    calib_free(calib);
    vec_free(vx);
    vec_free(vy);
    vec_free(vz);
    
    Serial.println("=== Calibration completed ===");
}

void loop() {
    vTaskDelay(pdMS_TO_TICKS(1000));
}

I also read the description

https://github.com/michal34512/Magnetometer-calibration?tab=readme-ov-file#how-to-use

And added the output to the code:

// Print the result
vec_print(calib.offset);
mat_print(calib.transform);

The results are ambiguous and depend on the number of samples:

For 20
// Variance of distances before calibration: 74.736556
// Variance of distances after calibration: 0.013583
For 100
// Variance of distances before calibration: 99.927141
// Variance of distances after calibration: 0.063912
For 300
// Variance of distances before calibration: 49.279169
// Variance of distances after calibration: 0.000708

Here is the data for 300 samples

=== Magnetometer Calibration Demo ===
Starting calibration process...
Calculating calibration data of data points was SUCCESSFUL
Variance of distances before calibration: 49.279169
Variance of distances after calibration: 0.000708
[-3.9935165864, 12.1129365731, -18.5020502551, ]
Size (3, 3):
0.023, -0.000, -0.003, 
-0.000, 0.022, 0.000, 
-0.003, 0.000, 0.034, 
=== Calibration completed ===

I still agree with the bias, but the matrices, which, as I understand it, are soft iron(scale), are for some reason very small, while they are usually about 1 in diagonal.

Like this:

0.9800, -0.000, -0.003, 
-0.000, 1.100, 0.000, 
-0.003, 0.000, 0.999, 

Basically, I'm confused about whether I've adapted the code correctly, whether I'm collecting the data correctly, whether the algorithm is working correctly, and whether the underlying logic is even correct.
All these questions remain unanswered for me.
I hope you could help me figure this out and perhaps even provide additional details on how to use the library and clear applications for achieving practical results.
Thank you.

[michal34512]

Hi there! 👋

Thanks for opening this issue and providing such detailed context, including the code and data samples.

To answer your main concern: the algorithm is working correctly! The reason your transformation matrix (the soft-iron matrix) has diagonal values like 0.023, 0.022, and 0.034 instead of being close to 1.0 is because this generated matrix simultaneously normalizes the scale of the sensor data.

For example, if your raw magnetometer returns vectors with an average magnitude (length) of 50, the transformation matrix will have values of around 0.02 (which is 1/50) on its diagonal to scale the output back down to a unit sphere (radius of 1).

Here is the matrix you received:
Plaintext

[ 0.023, -0.000, -0.003 ]
[ -0.000, 0.022, 0.000 ]
[ -0.003, 0.000, 0.034 ]

This simply means your raw data has a magnitude roughly in the range of 40-50, and the algorithm is correctly squashing it down.

However, what worries me more is the discrepancy between the diagonal values. You have 0.023 and 0.022 on the X and Y axes, but 0.034 on the Z-axis. It looks like your magnetometer is scaled quite differently on the Z-axis compared to the others, which usually shouldn't happen (especially with Bosch magnetometers).

To better understand what happened, I plotted your data points:

Looking at the graphs, you can see that the data you collected is just a small slice of the entire 3D sphere. When the algorithm tries to fit an ellipsoid to this incomplete data, it has to "guess" the rest of the shape, leading to that warped Z-axis scaling. If you collect data that covers the sphere more uniformly (by rotating the sensor smoothly across all axes), you will get much better results, and the diagonal values will likely all equalize to around ~0.022.

By the way, I actively use this code myself and ran into the exact same problem with data collection: how do you know when to record a measurement so that it evenly covers the sphere? Ideally, we would calculate the angle between the current reading and the nearest saved point. But there's a catch: we can't trust the angles between measurements to be accurate until the sensor is already calibrated!

I solved this by applying two rules for accepting a new calibration point:

• The sensor must be stationary: I implemented a FIFO queue to store recent measurements and calculate their variance. If the variance is below a certain threshold, I consider the sensor stable enough to accept a point.

• The measurement must be distinct: The new measurement must be different from the previous one (exceeding a specific distance/tolerance threshold).

Here is a snippet of the code that handles this logic (sorry that it's just a random snippet, but it shows the core concept):
C++

#include "calibration_manager.h"

#include "common_data.h"
#include "data_store.h"
#include "freertos/FreeRTOS.h"
#include "freertos/semphr.h"
#include "geomag.h"
#include "global.h"
#include "nvs_manager.h"
#include "queue_manager.h"
#include "sensors_manager.h"

#define CALIB_DATA_ACQ_INTERVAL     100
#define CALIB_BUF_SIZE              50
#define CALIB_ACC_FIFO              10
#define CALIB_ACC_FIFO_MAX_VARIANCE 0.2f
#define CALIB_ACC_MIN_DIFFERENCE    0.5f

#if CONFIG_LOGS_ENABLE_SENSORS
#include "logger.h"
#define TAG "CALIBRATION_MANAGER"
#else
#undef LOG
#define LOG(...)
#endif

typedef struct {
    double x[CALIB_BUF_SIZE];
    double y[CALIB_BUF_SIZE];
    double z[CALIB_BUF_SIZE];
} calibration_points_t;

static struct {
    calib_data_t calib_data;
    calibration_points_t mag_points;
    calibration_points_t acc_points;
    uint16_t point_id;
    Vector acc_point;
    Vector mag_point;
    Vector_fifo acc_fifo;
} ctx = {
  .acc_point = NULL,
  .mag_point = NULL,
  .acc_fifo = NULL,
};

static esp_err_t calibrate_get_data_mag() {
    if (sensors_mag_get(ctx.mag_point) != ESP_OK) {
        LOG(TAG, "CALIB MAG DATA AQC ERR");
        return ESP_FAIL;
    }
    return ESP_OK;
}

static esp_err_t calibrate_get_data_acc() {
    if (sensors_acc_get(ctx.acc_point) == ESP_OK) {
        vec_fifo_push(ctx.acc_fifo, ctx.acc_point);
    } else {
        LOG(TAG, "CALIB ACC DATA AQC ERR");
        return ESP_FAIL;
    }
    return ESP_OK;
}

static bool calibrate_is_acc_variance_acceptable() {
    return vec_fifo_xyz_variance(ctx.acc_fifo) < CALIB_ACC_FIFO_MAX_VARIANCE;
}

static bool calibrate_is_acc_different_from_last() {
    if (ctx.point_id == 0) {
        return true;
    }
    uint16_t prev_id = ctx.point_id - 1;
    return (fabs(ctx.acc_points.x[prev_id] - VEC_X(ctx.acc_point)) >= CALIB_ACC_MIN_DIFFERENCE
            || fabs(ctx.acc_points.y[prev_id] - VEC_Y(ctx.acc_point)) >= CALIB_ACC_MIN_DIFFERENCE
            || fabs(ctx.acc_points.z[prev_id] - VEC_Z(ctx.acc_point)) >= CALIB_ACC_MIN_DIFFERENCE);
}

static void calibrate_save_acc_mag_point_to_buffer() {
    ctx.mag_points.x[ctx.point_id] = VEC_X(ctx.mag_point);
    ctx.mag_points.y[ctx.point_id] = VEC_Y(ctx.mag_point);
    ctx.mag_points.z[ctx.point_id] = VEC_Z(ctx.mag_point);

    ctx.acc_points.x[ctx.point_id] = VEC_X(ctx.acc_point);
    ctx.acc_points.y[ctx.point_id] = VEC_Y(ctx.acc_point);
    ctx.acc_points.z[ctx.point_id] = VEC_Z(ctx.acc_point);
    ctx.point_id++;
}

static bool calibrate_is_acc_mag_buffer_full() {
    return ctx.point_id + 1 >= CALIB_BUF_SIZE;
}

static esp_err_t calibrate_by_ellipsoid(Callibration_t* calibration, calibration_points_t* calibration_points) {
    assert(calibration);
    assert(calibration_points);
    Vector x = vec_from_array(calibration_points->x, CALIB_BUF_SIZE);
    Vector y = vec_from_array(calibration_points->y, CALIB_BUF_SIZE);
    Vector z = vec_from_array(calibration_points->z, CALIB_BUF_SIZE);
    *calibration = calib_calibrate_sensor(x, y, z);
    vec_from_array_free(x);
    vec_from_array_free(y);
    vec_from_array_free(z);
    return calib_calibration_success(*calibration) ? ESP_OK : ESP_FAIL;
}

static esp_err_t calibrate_acc() {
    return calibrate_by_ellipsoid(&ctx.calib_data.acc, &ctx.acc_points);
}

static esp_err_t calibrate_mag() {
    return calibrate_by_ellipsoid(&ctx.calib_data.mag, &ctx.mag_points);
}

static esp_err_t calib_send_calib_info(rcu_calib_info_acq_err_t error) {
    static uart_tx_mess_t tx = {
      .message_type = TX_MESS_TYPE_CALIB_INFO,
    };
    tx.calib_info.points_acquired = ctx.point_id;
    tx.calib_info.error = error;
    return (SEND_UART_TX_MESS_QUEUE(&tx, 0) == pdTRUE) ? ESP_OK : ESP_FAIL;
}

static esp_err_t calib_ellipsoid_try_recalculate() {
    vec_free(ctx.mag_point);
    vec_free(ctx.acc_point);
    vec_fifo_free(ctx.acc_fifo);
    ctx.mag_point = NULL;
    ctx.acc_point = NULL;
    ctx.acc_fifo = NULL;
    RET_AND_LOG_ON_ERR(calibrate_acc(), "ACC calibration fail");
    RET_AND_LOG_ON_ERR(calibrate_mag(), "MAG calibration fail");
    LOG(TAG, "Recalculated MAG:");
    mat_print(ctx.calib_data.mag.transform);
    vec_print(ctx.calib_data.mag.offset);
    LOG(TAG, "Recalculated MAG:");
    mat_print(ctx.calib_data.acc.transform);
    vec_print(ctx.calib_data.acc.offset);
    RET_AND_LOG_ON_ERR(nvs_manager_set_calib_config(&ctx.calib_data), "Failed to set MAG/ACC callibration");
    RET_AND_LOG_ON_ERR(nvs_manager_save_config(), "Failed to save conig");
    return ESP_OK;
}

esp_err_t calib_init() {
    RET_AND_LOG_ON_ERR(nvs_manager_get_calib_config(&ctx.calib_data), "Failed to read callibration from nvs");
    LOG(TAG, "Loaded MAG:");
    mat_print(ctx.calib_data.mag.transform);
    vec_print(ctx.calib_data.mag.offset);
    LOG(TAG, "Loaded ACC:");
    mat_print(ctx.calib_data.acc.transform);
    vec_print(ctx.calib_data.acc.offset);
    return ESP_OK;
}

esp_err_t calib_wmm_recalculate(double height, double latitude, double longitude, double date_year) {
    int maxdeg = 12;
    double ti = 0, gv = 0;
    LOG(TAG, "WMM CALIB DATA: ALT: %f, LAT: %f, LON: %f, DATE: %f", height, latitude, longitude, date_year);
    geomag(&maxdeg);
    geomg1(height,
           latitude,
           longitude,
           date_year,
           &ctx.calib_data.wmm.declination,
           &ctx.calib_data.wmm.inclination,
           &ti,
           &gv);  // Get declination and inclination from official world magnetic model
    LOG(TAG, "WMM CALIB DONE DECL : %f, INCL : %f", ctx.calib_data.wmm.declination, ctx.calib_data.wmm.inclination);
    ctx.calib_data.wmm.declination = ctx.calib_data.wmm.declination * DEG_TO_RAD;
    ctx.calib_data.wmm.inclination = ctx.calib_data.wmm.inclination * DEG_TO_RAD;
    RET_AND_LOG_ON_ERR(nvs_manager_set_calib_config(&ctx.calib_data), "Failed to set WMM callibration");
    RET_AND_LOG_ON_ERR(nvs_manager_save_config(), "Failed to save conig");
    return ESP_OK;
}

void calib_ellipsoid_start() {
    ctx.mag_point = vec_new(3);
    ctx.acc_point = vec_new(3);
    ctx.acc_fifo = vec_fifo_new(3, CALIB_ACC_FIFO);
    ctx.point_id = 0;
    LOG(TAG, "CALIB START");
}

void calib_ellipsoid_stop() {
    vec_free(ctx.mag_point);
    vec_free(ctx.acc_point);
    vec_fifo_free(ctx.acc_fifo);
    ctx.mag_point = NULL;
    ctx.acc_point = NULL;
    ctx.acc_fifo = NULL;
    LOG(TAG, "CALIB STOP");
}

bool calib_ellipsoid_point_ack() {
    calibrate_get_data_acc();
    calibrate_get_data_mag();
    LOG(TAG,
        "ACC CALIB DATA POINT: %d, x : %f,y : %f, z : %f",
        ctx.point_id,
        VEC_X(ctx.acc_point),
        VEC_Y(ctx.acc_point),
        VEC_Z(ctx.acc_point));
    LOG(TAG,
        "MAG CALIB DATA POINT: %d, x : %f,y : %f, z : %f",
        ctx.point_id,
        VEC_X(ctx.mag_point),
        VEC_Y(ctx.mag_point),
        VEC_Z(ctx.mag_point));
    if (vec_fifo_is_filled(ctx.acc_fifo)) {
        if (calibrate_is_acc_variance_acceptable()) {
            if (calibrate_is_acc_different_from_last()) {
                calibrate_save_acc_mag_point_to_buffer();
                calib_send_calib_info(RCU_CALIB_ACQ_OK);
            } else {
                calib_send_calib_info(RCU_CALIB_ACQ_NOT_MOVING);
                LOG(TAG, "Acc same as last");
            }
        } else {
            calib_send_calib_info(RCU_CALIB_ACQ_TO_FAST);
            LOG(TAG, "Variance too high: %f", vec_fifo_distance_variance(ctx.acc_fifo));
        }
    }
    return calibrate_is_acc_mag_buffer_full();
}

esp_err_t calib_ellipsoid_recalculate() {
    esp_err_t res = calib_ellipsoid_try_recalculate();
    if (res == ESP_OK) {
        calib_send_calib_info(RCU_CALIB_ACQ_DONE_SUCCESFUL);
    } else {
        calib_send_calib_info(RCU_CALIB_ACQ_DONE_FAILED);
    }
    return res;
}

esp_err_t calib_acc_get_calibrated(Vector vec) {
    assert(vec);
    RET_AND_LOG_ON_ERR(sensors_acc_get(vec), "Acc raw");
    calib_calibrate_point(ctx.calib_data.acc, vec);
    return ESP_OK;
}

esp_err_t calib_mag_get_calibrated(Vector vec) {
    assert(vec);
    RET_AND_LOG_ON_ERR(sensors_mag_get(vec), "Mag raw");
    calib_calibrate_point(ctx.calib_data.mag, vec);
    Vector gravity = vec_new(3);
    RET_AND_LOG_ON_ERR(calib_acc_get_calibrated(gravity), "Acc raw");
    wmm_compensate(vec, gravity, ctx.calib_data.wmm.declination, ctx.calib_data.wmm.inclination);
    vec_free(gravity);
    return ESP_OK;
}

vector_fifo.h:

#ifndef COMPONENTS_GEOMETRY_VECTOR_FIFO_H_
#define COMPONENTS_GEOMETRY_VECTOR_FIFO_H_

#include "matrix.h"

typedef struct {
    unsigned int head;
    unsigned int fill;
    Matrix buffer;
} Vector_fifo_t, *Vector_fifo;

Vector_fifo vec_fifo_new(unsigned int vec_size, unsigned int buff_size);

void vec_fifo_push(Vector_fifo vector_fifo, Vector vecA);

void vec_fifo_get_last(Vector_fifo vector_fifo, Vector vecA);

void vec_fifo_get_average(Vector_fifo vector_fifo, Vector vecA);

double vec_fifo_distance_variance(Vector_fifo vector_fifo);

double vec_fifo_xyz_variance(Vector_fifo vector_fifo);

bool vec_fifo_is_filled(Vector_fifo vector_fifo);

void vec_fifo_free(Vector_fifo vector_fifo);

#endif  // COMPONENTS_GEOMETRY_VECTOR_FIFO_H_

vector_fifo.c:

#include <stdio.h>

#include "vector_fifo.h"

#include "assert.h"

Vector_fifo vec_fifo_new(unsigned int vec_size, unsigned int buff_size) {
    Vector_fifo res = malloc(sizeof(Vector_fifo_t));
    res->buffer = mat_new(vec_size, buff_size);
    res->head = 0;
    res->fill = 0;
    return res;
}

void vec_fifo_push(Vector_fifo vector_fifo, Vector vecA) {
    mat_paste_column(vector_fifo->buffer, vecA, vector_fifo->head);
    vector_fifo->head = (vector_fifo->head + 1) % vector_fifo->buffer->columns;
    if (vector_fifo->fill < vector_fifo->buffer->columns) {
        vector_fifo->fill++;
    }
}

void vec_fifo_get_last(Vector_fifo vector_fifo, Vector vecA) {
    if (vector_fifo->head == 0) {
        mat_get_column(vector_fifo->buffer, vecA, vector_fifo->buffer->columns - 1);
    }
    mat_get_column(vector_fifo->buffer, vecA, vector_fifo->head - 1);
}

void vec_fifo_get_average(Vector_fifo vector_fifo, Vector vecA) {
    assert(vector_fifo->buffer->columns > 0);
    assert(vector_fifo->buffer->rows == vecA->size);
    vec_fill(vecA, 0);
    for (unsigned int i = 0; i < vector_fifo->buffer->columns; i++) {
        for (unsigned int j = 0; j < vecA->size; j++) {
            VEC_ELEM(vecA, j) += MAT_ELEM(vector_fifo->buffer, j, i);
        }
    }
    for (unsigned int i = 0; i < vecA->size; i++) {
        VEC_ELEM(vecA, i) /= vector_fifo->buffer->columns;
    }
}

double vec_fifo_distance_variance(Vector_fifo vector_fifo) {
    assert(vector_fifo->buffer->columns > 1);
    double mean = 0;
    double res = 0;
    double temp = 0;
    Vector temp_vec = vec_new(vector_fifo->buffer->rows);
    for (unsigned int i = 0; i < vector_fifo->buffer->columns; i++) {
        mat_get_column(vector_fifo->buffer, temp_vec, i);
        mean += vec_norm(temp_vec);
    }
    mean /= vector_fifo->buffer->columns;
    for (unsigned int i = 0; i < vector_fifo->buffer->columns; i++) {
        mat_get_column(vector_fifo->buffer, temp_vec, i);
        temp = vec_norm(temp_vec) - mean;
        res += temp * temp;
    }
    res = res / (vector_fifo->buffer->columns - 1);
    vec_free(temp_vec);
    return res;
}

double vec_fifo_xyz_variance(Vector_fifo vector_fifo) {
    assert(vector_fifo->buffer->columns > 1);
    assert(vector_fifo->buffer->rows >= 3);

    double mean[3] = {0.0, 0.0, 0.0};
    double var[3] = {0.0, 0.0, 0.0};

    for (unsigned int i = 0; i < vector_fifo->buffer->columns; i++) {
        mean[0] += MAT_ELEM(vector_fifo->buffer, 0, i);
        mean[1] += MAT_ELEM(vector_fifo->buffer, 1, i);
        mean[2] += MAT_ELEM(vector_fifo->buffer, 2, i);
    }

    mean[0] /= vector_fifo->buffer->columns;
    mean[1] /= vector_fifo->buffer->columns;
    mean[2] /= vector_fifo->buffer->columns;

    for (unsigned int i = 0; i < vector_fifo->buffer->columns; i++) {
        for (int axis = 0; axis < 3; axis++) {
            double d = MAT_ELEM(vector_fifo->buffer, axis, i) - mean[axis];
            var[axis] += d * d;
        }
    }

    var[0] /= (vector_fifo->buffer->columns - 1);
    var[1] /= (vector_fifo->buffer->columns - 1);
    var[2] /= (vector_fifo->buffer->columns - 1);
    double min_var = var[0];
    if (var[1] < min_var)
        min_var = var[1];
    if (var[2] < min_var)
        min_var = var[2];

    return min_var;
}

bool vec_fifo_is_filled(Vector_fifo vector_fifo) {
    return vector_fifo->fill == vector_fifo->buffer->columns;
}

void vec_fifo_free(Vector_fifo vector_fifo) {
    if (vector_fifo == NULL)
        return;
    mat_free(vector_fifo->buffer);
    free(vector_fifo);
}

In the future, when I have some free time, I plan to expand the repository with a complete example including the data collection method described above. Unfortunately, due to my current lack of free time, it won't happen sooner than in about 4 months. :/

[brightproject]

Yes, the methodology for assessing sample quality and collecting it is an interesting topic.
Thank you for implementing it.
I'll try to use it.

I'm more interested in the issues of preparing raw data for sample collection.
For example, does it make a difference whether the calibration algorithm receives data along the X, Y, and Z axes in the NED or ENU orientation, or is it completely irrelevant to the calibration algorithm?
I also see in your code that you're feeding the calibration algorithm data in uT, and in double format.

Can I feed raw data as integer values, or does it have to be double, or maybe float?

Also, the question of the algorithm's practical application is important.
I've been searching for a practical way to use the code for several months now, for devices that have limited tilt in space or are already mounted on mechanisms.

In other words, your code, in its simplest form, assumes that we can pick up the device with the sensor by hand and easily rotate it around any axis, thus completely assembling a sphere.

I'm looking for options where the device is already installed somewhere on the object, out of reach of the hands, and calibration is only possible with the device in motion.

For example, a car moves only in the XY plane and can rotate 360 ​​degrees around the Z axis.

When it's moving, tilts are minimal, ranging from 0 to 20 degrees maximum around the X and Y axes.

This means that with on-car calibration, we can only collect data for the Z axe, and only partially for the X and Y, and then only in one hemisphere.

This raises the main question: what's the point of using the least-squares method if it doesn't allow for calibrating a sensor with limited movement, and if the sensor still needs to be calibrated beforehand while removed from its final installation location?

If the sensor is being calibrated by hand, then you don't have to use these complex calibration methods with their mind-numbing mathematics; you can simply use a simple method of adding two values ​​and dividing by two.
You can also calculate the offsets in a slightly different way. Here's pseudocode that reflects my thoughts:

bias.x = ((float)max.x + min.x) / 2; 
bias.y = ((float)max.y + min.y) / 2; 
bias.z = ((float)max.z + min.z) / 2; 

float xchord = ((float)max.x - min.x); 
float ychord = ((float)max.y - min.y); 
float zchord = ((float)max.z - min.z); 
float cord_average = (xchord + ychord + zchord) / 3.; 

if (xchord != 0) scale.x = cord_average / xchord; 
if (ychord != 0) scale.y = cord_average / ychord; 
if (zchord != 0) scale.z = cord_average / zchord;

Regarding the Z-axis scale, I also wondered about this.
My X- and Y-axes, when aligned with the horizon, always show a magnetic field strength of about 15 uT, while the Z-axis, which points downward, shows a magnetic field strength of about -50 uT.
If I point the X-axis upward, it shows a magnetic field strength of 50 uT, if I point the Y-axis upward, it also shows a magnetic field strength of 50 uT.
If I point the Z-axis parallel to the horizon, it shows +-15 uT.
As far as I understand, in the Northern Hemisphere, the vertical component of the magnetic field is always higher than the horizontal one.
Therefore, calibration by rotating each axis 360 degrees has always been a problem for me, because even using a simple offset calculation method by adding the two maximum and minimum magnetic field strength values ​​and dividing by 2, we get the same value.
But then the sensor measures a field several times smaller than in the vertical plane.

I collected 100 data points by simply rotating the sensor 360 degrees around each axis.
I used the same code and the same data.

import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import numpy as np

# Original data points
x = [
-28, -27, -24, -19, -13, -10, 0, 6, 7, 6, 6, 3, 1, -1, -5, -8, -10, -15, -24, -30, -32, -34, -36, -30, -28, -27, -26, -22, -17, -12, -11, -12, -13, -14, -12, -13, -12, -11, -9, -8, -8, -7, -10, -10, -13, -11, -7, -10, -12, -11, -11, -11, -10, -9, -2, -5, -9, -3, -4, -3, 0, 0, 6, 20, 32, 38, 40, 40, 39, 38, 35, 28, 18, 1, -12, -39, -46, -52, -59, -60, -60, -59, -59, -57, -56, -55, -52, -50, -47, -42, -40, -35, -32, -27, -20, -14, -11, -7, 1, 5
]

y = [
6, 4, 0, -4, -6, -7, -3, 5, 12, 18, 20, 23, 25, 28, 30, 30, 29, 30, 26, 23, 22, 17, 14, 12, 9, 3, 1, -2, -6, -6, -9, -13, -19, -27, -35, -39, -42, -41, -39, -32, -22, -13, -1, 6, 15, 29, 47, 60, 65, 67, 64, 62, 58, 56, 41, 33, 22, 16, 10, 2, -4, 0, 0, 0, -2, -6, -6, -7, -9, -6, -6, -7, -4, -5, -4, -7, -8, -7, -8, -9, -11, -14, -14, -13, -14, -13, -13, -13, -12, -13, -12, -13, -12, -11, -10, -9, -10, -11, -10, -7
]

z = [
-47, -45, -46, -46, -47, -45, -46, -48, -47, -47, -48, -47, -48, -47, -47, -46, -46, -46, -46, -43, -44, -43, -43, -44, -44, -46, -46, -44, -46, -46, -45, -44, -39, -33, -23, -10, -1, 10, 19, 30, 42, 48, 52, 52, 53, 51, 41, 25, 15, 2, -12, -21, -28, -29, -42, -46, -49, -50, -50, -50, -47, -48, -46, -38, -26, -14, -1, 6, 12, 15, 26, 33, 43, 50, 52, 42, 38, 29, 16, 12, 7, 0, -2, -9, -14, -17, -22, -25, -30, -33, -35, -37, -39, -45, -44, -47, -46, -45, -44, -44
]

# Convert to numpy arrays
x = np.array(x)
y = np.array(y)
z = np.array(z)

# Create 3D plot
fig = plt.figure(figsize=(14, 10))
ax = fig.add_subplot(111, projection='3d')

# Plot points - all in blue color
ax.scatter(x, y, z, color='blue', s=30, alpha=0.7, edgecolors='black', linewidth=0.5)

# Axis configuration
ax.set_xlabel('X', fontsize=12, fontweight='bold')
ax.set_ylabel('Y', fontsize=12, fontweight='bold')
ax.set_zlabel('Z', fontsize=12, fontweight='bold')
ax.set_title('Asymmetric Globe (Ellipsoid) along X and Y', fontsize=14, fontweight='bold')

# ============================================================
# GLOBE CENTER CALCULATION
# ============================================================
# Center X = (max X + min X) / 2
center_x = (np.max(x) + np.min(x)) / 2

# Center Y = (max Y + min Y) / 2
center_y = (np.max(y) + np.min(y)) / 2

# Center Z = 0 (equator plane)
center_z = 0

# Radii from center to extreme points
radius_x = np.max(np.abs(x - center_x))
radius_y = np.max(np.abs(y - center_y))
radius_z = np.max(np.abs(z))

# Set axis ranges
ax.set_xlim([np.min(x), np.max(x)])
ax.set_ylim([np.min(y), np.max(y)])
ax.set_zlim([-50, 50])

# Grid parameters
num_meridians = 16
num_parallels = 9

# Draw meridians - in green
theta = np.linspace(0, 2*np.pi, num_meridians)
for i in range(num_meridians):
    theta_fixed = theta[i]
    phi_vals = np.linspace(0, np.pi, 50)
    x_meridian = center_x + radius_x * np.sin(phi_vals) * np.cos(theta_fixed)
    y_meridian = center_y + radius_y * np.sin(phi_vals) * np.sin(theta_fixed)
    z_meridian = center_z + radius_z * np.cos(phi_vals)
    ax.plot(x_meridian, y_meridian, z_meridian, color='green', alpha=0.5, linewidth=1)

# Draw parallels - in green
phi = np.linspace(0, np.pi, num_parallels)
for j in range(1, num_parallels - 1):
    phi_fixed = phi[j]
    theta_vals = np.linspace(0, 2*np.pi, 50)
    x_parallel = center_x + radius_x * np.sin(phi_fixed) * np.cos(theta_vals)
    y_parallel = center_y + radius_y * np.sin(phi_fixed) * np.sin(theta_vals)
    z_parallel = center_z + radius_z * np.cos(phi_fixed)
    ax.plot(x_parallel, y_parallel, z_parallel, color='green', alpha=0.5, linewidth=1)

# Equator - in red (at z=0)
phi_equator = np.pi / 2
theta_vals = np.linspace(0, 2*np.pi, 100)
x_equator = center_x + radius_x * np.sin(phi_equator) * np.cos(theta_vals)
y_equator = center_y + radius_y * np.sin(phi_equator) * np.sin(theta_vals)
z_equator = center_z + radius_z * np.cos(phi_equator)
ax.plot(x_equator, y_equator, z_equator, color='red', alpha=0.7, linewidth=2, label='Equator')

# Origin point (0,0,0)
ax.scatter([0], [0], [0], color='red', s=50, marker='o', label='Origin (0,0,0)')

# Globe center
ax.scatter([center_x], [center_y], [center_z], color='yellow', s=100, marker='x', label='Globe center')

ax.legend(loc='upper right', fontsize=9)
plt.tight_layout()
plt.show()

# Print statistics
print(f"Max X: {np.max(x):.2f}, Min X: {np.min(x):.2f}")
print(f"Max Y: {np.max(y):.2f}, Min Y: {np.min(y):.2f}")
print(f"Globe center X: ({np.max(x):.2f} + {np.min(x):.2f}) / 2 = {center_x:.2f}")
print(f"Globe center Y: ({np.max(y):.2f} + {np.min(y):.2f}) / 2 = {center_y:.2f}")
print(f"Globe center Z: {center_z}")
print(f"Radius along X: {radius_x:.2f}")
print(f"Radius along Y: {radius_y:.2f}")
print(f"Radius along Z: {radius_z:.2f}")

If you pay attention, I made the Z axis scale -50 0 +50 uT.

I collected the data in uT, but forgot to specify float, so these are integers, but they're sufficient for understanding the process.
As you can see from the graph, when rotating around the Z axis, the radius is only 10-15 uT, but if you rotate around the X and Y axes, the range is completely different.
Is this an error in my data acquisition and the scaling of the BNO085 sensor axes, or is everything correct and is this a matter of physics?
I often get confused about this myself, and each time I begin to understand the measurement process anew.

[brightproject]

I'm continuing to experiment with samples.

Answering my previous questions: you can feed raw data into the algorithm, and the axis orientation doesn't matter. The main thing is to not change the axes before or after calibration, so the offset subtraction will be correct.
I also tried another idea, since the purpose of calibration using the ellipsoid fitting method is to collect data across the entire ellipse.
You can't collect readings from just the hemisphere (lower or upper), as I do; the algorithm will simply try to fit the cap of the sphere to the entire sphere.
So I thought, why not collect data from the lower hemisphere (or upper) and mirror the values ​​along the Z-axis?
Again, I'm looking for a practical application of the algorithm for already installed sensors on a moving device, where it's physically impossible to rotate them 360 degrees along all axes.
So, I'm collecting 800 raw data samples from the device.
I place them in a Python script and mirror them relative to the Z axis, and I get sets of samples that fill almost the entire sphere.

import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import numpy as np

# Original data points

x = [
    109, 115, 98, 109, 92, 98, 80, 57, 40, 40, 40, 40, 40, -23, -52, -80, -75, -86, -86, -75, -92, -52, -52, -5, 34, 69, 104, 144, 167, 190, 237, 260, 283, 306, 335, 370, 364, 381, 404, 416, 416, 427, 404, 410, 387, 387, 358, 283, 265, 225, 213, 144, 98, 69, 28, -5, -52, -80, -92, -144, -156, -167, -179, -190, -173, -150, -121, -115, -69, -46, 0, 52, 80, 115, 167, 208, 260, 271, 300, 335, 352, 381, 364, 381, 381, 393, 364, 352, 346, 300, 283, 265, 231, 202, 144, 98, 57, -11, -46, -86, -109, -150, -179, -237, -237, -271, -294, -335, -364, -375, -393, -398, -381, -387, -375, -329, -271, -254, -237, -202, -173, -138, -109, -92, -75, -52, -40, -40, -40, -23, -28, -34, -28, -23, -23, -11, 0, 23, 75, 40, -5, -28, -80, -98, -132, -173, -219, -248, -294, -364, -375, -433, -433, -416, -433, -456, -439, -450, -410, -410, -398, -352, -335, -323, -318, -318, -306, -283, -271, -265, -248, -242, -237, -196, -202, -202, -202, -231, -225, -208, -173, -190, -196, -202, -225, -242, -237, -248, -260, -271, -283, -294, -294, -283, -271, -242, -208, -190, -179, -132, -121, -69, -69, -69, -23, -28, -17, 11, 17, 46, 86, 127, 138, 150, 132, 109, 86, 40, -5, -23, -86, -132, -173, -231, -294, -335, -375, -404, -433, -451, -474, -514, -526, -566, -601, -601, -630, -664, -699, -699, -734, -769, -769, -780, -780, -705, -699, -659, -641, -583, -514, -485, -445, -387, -312, -242, -190, -150, -104, -69, -40, -23, 5, 17, 11, 17, 23, 46, 0, -17, -28, -34, -57, -80, -86, -121, -115, -161, -161, -185, -248, -271, -283, -318, -358, -375, -404, -439, -474, -451, -514, -520, -508, -549, -549, -549, -531, -531, -503, -520, -514, -479, -445, -439, -422, -370, -323, -300, -248, -248, -213, -190, -185, -179, -167, -179, -202, -219, -260, -300, -335, -381, -422, -497, -531, -566, -601, -630, -676, -722, -745, -780, -815, -815, -832, -821, -815, -821, -803, -774, -757, -717, -676, -636, -595, -560, -497, -451, -398, -352, -300, -242, -202, -156, -121, -75, -40, 0, -5, 11, 11, 11, 11, -34, -80, -156, -202, -254, -294, -346, -398, -456, -526, -578, -624, -659, -693, -740, -769, -786, -792, -809, -826, -821, -821, -815, -815, -815, -792, -792, -763, -722, -705, -670, -630, -578, -531, -468, -398, -346, -300, -237, -208, -173, -127, -104, -109, -109, -115, -132, -144, -150, -213, -213, -265, -306, -323, -358, -393, -422, -451, -479, -531, -531, -601, -618, -647, -688, -716, -728, -745, -751, -751, -751, -740, -757, -728, -716, -688, -682, -653, -612, -566, -514, -485, -439, -387, -358, -318, -289, -265, -225, -208, -196, -190, -213, -219, -237, -237, -277, -294, -323, -370, -381, -427, -468, -520, -543, -560, -589, -595, -630, -647, -682, -717, -717, -751, -751, -751, -751, -722, -711, -705, -670, -641, -630, -595, -543, -560, -537, -520, -474, -508, -451, -479, -433, -416, -433, -387, -370, -335, -329, -283, -254, -237, -208, -167, -132, -109, -121, -121, -156, -167, -185, -202, -237, -294, -312, -381, -427, -456, -514, -549, -595, -670, -676, -751, -769, -803, -809, -809, -832, -815, -838, -815, -797, -780, -763, -717, -711, -653, -636, -595, -555, -514, -474, -433, -393, -387, -370, -329, -289, -277, -237, -202, -179, -173, -138, -127, -92, -80, -57, -28, -17, 5, 17, 23, 5, -5, -34, -17, 0, 11, 5, -34, -86, -132, -213, -300, -387, -474, -549, -630, -676, -699, -705, -693, -699, -693, -641, -612, -572, -531, -485, -485, -485, -485, -144, -104, -17, 5, 109, 127, 173, 208, 219, 254, 271, 323, 346, 364, 335, 352, 352, 364, 335, 335, 283, 213, 167, 121, 34, -57, -138, -219, -289, -346, -404, -445, -508, -549, -584, -595, -630, -624, -624, -624, -601, -584, -543, -537, -468, -422, -381, -318, -271, -260, -231, -208, -185, -127, -132, -92, -63, 0, 52, 98, 150, 167, 196, 219, 237, 237, 208, 202, 179, 121, 63, 0, -86, -144, -185, -283, -341, -398, -422, -456, -462, -468, -439, -416, -398, -381, -381, -346, -289, -231, -185, -138, -80, -28, 23, 86, 144, 173, 237, 260, 323, 335, 358, 393, 381, 404, 433, 445, 451, 456, 468, 468, 439, 439, 439, 439, 190, 34, -34, -115, -196, -260, -318, -370, -427, -468, -503, -520, -543, -526, -508, -479, -433, -387, -306, -266, -190, -109, -63, -17, 17, 52, 121, 127, 150, 167, 173, 173, 173, 185, 190, 167, 173, 202, 196, 196, 179, 121, 109, 80, 46, 23, 0, 40, 34, 57, 80, 115, 98, 98, 98, 86, 98, 104, 92, 104, 104, 98, 121, 109, 121, 109, 109, 115, 104, 121, 104, 115, 121, 121, 127, 127, 109, 109, 109, 115, 115, 127, 127, 115, 133, 127, 115
]

y = [
    196, 196, 179, 196, 190, 167, 150, 150, 127, 127, 127, 127, 127, 127, 92, 98, 127, 127, 156, 179, 213, 248, 271, 300, 341, 346, 358, 387, 404, 410, 410, 422, 416, 427, 410, 404, 375, 358, 335, 318, 277, 248, 208, 173, 132, 69, 28, 11, -23, -34, -57, -57, -57, -52, -28, -17, 0, 11, 69, 86, 138, 185, 237, 283, 318, 358, 387, 387, 398, 393, 416, 398, 393, 404, 393, 375, 364, 329, 318, 271, 260, 219, 185, 156, 150, 121, 86, 80, 40, 23, 17, 0, -40, -40, -63, -57, -75, -115, -115, -115, -115, -115, -92, -75, -63, -34, -28, 11, 28, 52, 115, 150, 173, 202, 202, 237, 237, 231, 213, 219, 167, 138, 98, 34, -17, -34, -75, -75, -75, -202, -254, -318, -335, -346, -335, -341, -341, -329, -300, -294, -318, -318, -318, -277, -254, -225, -213, -213, -213, -185, -156, -121, -104, -115, -80, -109, -98, -92, -104, -75, -75, -63, -46, -5, 0, -17, -17, -17, -11, -11, -17, -17, 11, -5, -11, -11, -11, 0, -11, -11, -28, -63, -86, -132, -179, -242, -248, -277, -300, -300, -300, -318, -277, -260, -225, -156, -115, -69, 5, 69, 121, 144, 185, 231, 231, 271, 277, 294, 289, 294, 271, 265, 260, 248, 213, 167, 127, 63, 17, -17, -69, -115, -161, -208, -254, -329, -335, -370, -381, -398, -422, -416, -398, -398, -404, -364, -335, -318, -260, -237, -237, -173, -63, -63, 63, 63, 202, 248, 289, 346, 387, 422, 445, 474, 474, 462, 508, 514, 503, 497, 485, 439, 404, 370, 306, 265, 202, 167, 75, 17, -40, -80, -150, -202, -260, -300, -341, -364, -375, -393, -398, -387, -387, -364, -335, -323, -306, -277, -213, -190, -150, -80, -17, 57, 104, 138, 190, 242, 300, 318, 364, 404, 427, 451, 503, 514, 514, 526, 508, 491, 451, 393, 346, 260, 185, 115, 57, -23, -69, -115, -161, -190, -225, -248, -254, -242, -254, -237, -237, -179, -179, -127, -150, -179, -179, -150, -127, -80, -57, -5, 40, 86, 92, 156, 208, 271, 306, 346, 375, 410, 427, 451, 456, 474, 485, 485, 474, 485, 479, 445, 427, 410, 387, 352, 329, 329, 329, 329, 173, 115, 63, 28, 5, -23, -34, -46, -52, -34, -17, -28, 0, 23, 40, 69, 86, 115, 138, 156, 167, 185, 196, 231, 237, 277, 289, 335, 329, 352, 346, 364, 370, 358, 323, 300, 277, 254, 242, 202, 156, 121, 86, 34, 23, 34, 34, 46, 92, 132, 167, 190, 254, 254, 283, 294, 300, 306, 306, 283, 306, 329, 323, 318, 318, 294, 312, 300, 323, 277, 283, 254, 254, 219, 213, 185, 179, 179, 179, 179, 138, 98, 98, 115, 133, 161, 179, 196, 208, 237, 248, 277, 289, 329, 335, 346, 352, 329, 346, 352, 393, 381, 312, 306, 294, 254, 254, 248, 219, 179, 150, 109, 63, 28, 0, 11, 23, 34, 57, 80, 86, 92, 115, 121, 144, 167, 179, 196, 219, 242, 260, 277, 294, 306, 329, 341, 358, 381, 398, 410, 427, 451, 456, 479, 497, 503, 537, 531, 555, 555, 566, 595, 584, 612, 607, 578, 601, 584, 555, 537, 491, 433, 352, 312, 265, 202, 156, 121, 98, 63, 46, 23, -5, -28, -63, -92, -80, -104, -98, -86, -75, -46, -28, 0, 40, 98, 98, 185, 185, 185, 300, 364, 433, 491, 543, 566, 636, 670, 711, 757, 769, 809, 815, 826, 832, 809, 803, 780, 745, 693, 670, 624, 589, 572, 526, 451, 381, 341, 289, 242, 167, 121, 80, 28, -28, -63, -104, -144, -156, -173, -202, -219, -225, -213, -202, -179, -132, -104, -63, -40, 0, 57, 98, 144, 173, 231, 283, 329, 352, 370, 398, 427, 462, 508, 520, 526, 549, 572, 578, 601, 595, 607, 607, 618, 636, 624, 612, 589, 584, 578, 537, 531, 514, 526, 503, 445, 410, 381, 375, 335, 265, 260, 248, 213, 196, 173, 167, 185, 190, 185, 173, 190, 173, 173, 161, 167, 156, 190, 185, 173, 173, 173, 190, 196, 208, 231, 242, 248, 260, 242, 231, 213, 208, 196, 190, 202, 213, 202, 196, 202, 196, 185, 190, 196, 202, 208, 196, 190
]

z = [
    -759, -765, -778, -753, -765, -759, -765, -772, -759, -759, -759, -759, -759, -778, -778, -791, -797, -816, -810, -810, -785, -791, -772, -784, -778, -765, -733, -714, -714, -682, -657, -638, -612, -599, -599, -593, -568, -561, -574, -574, -555, -561, -568, -580, -587, -606, -625, -625, -638, -657, -638, -682, -714, -714, -740, -746, -772, -778, -778, -784, -778, -778, -804, -797, -785, -772, -791, -772, -753, -772, -765, -702, -721, -721, -708, -682, -676, -670, -657, -663, -631, -612, -619, -612, -631, -619, -632, -632, -638, -638, -676, -676, -695, -702, -714, -740, -727, -727, -740, -727, -734, -733, -746, -753, -772, -772, -772, -753, -766, -740, -778, -772, -766, -778, -772, -785, -804, -810, -810, -797, -804, -823, -797, -791, -778, -766, -759, -759, -759, -682, -683, -657, -631, -625, -619, -612, -625, -631, -644, -638, -644, -644, -651, -663, -689, -702, -695, -715, -702, -664, -695, -702, -689, -714, -708, -714, -708, -689, -721, -721, -727, -721, -740, -753, -734, -753, -753, -753, -753, -766, -753, -772, -759, -766, -772, -778, -759, -772, -785, -766, -778, -773, -747, -734, -740, -702, -676, -670, -657, -632, -638, -645, -632, -676, -702, -708, -753, -734, -772, -798, -778, -785, -804, -791, -785, -791, -785, -785, -785, -772, -791, -766, -759, -740, -740, -740, -753, -772, -766, -766, -753, -740, -727, -695, -657, -632, -600, -561, -523, -510, -491, -472, -459, -459, -440, -447, -472, -466, -472, -472, -485, -523, -523, -587, -587, -619, -632, -644, -664, -638, -695, -702, -727, -727, -753, -753, -753, -753, -747, -766, -772, -766, -779, -772, -785, -772, -779, -740, -747, -734, -721, -734, -683, -670, -657, -587, -606, -581, -600, -581, -574, -606, -606, -606, -613, -632, -632, -644, -683, -657, -676, -702, -715, -714, -727, -727, -715, -715, -708, -708, -734, -696, -715, -715, -728, -728, -747, -747, -747, -759, -779, -791, -772, -798, -791, -804, -747, -734, -747, -683, -677, -651, -587, -600, -587, -555, -555, -530, -523, -491, -498, -459, -440, -453, -466, -466, -479, -510, -536, -549, -574, -600, -625, -657, -651, -683, -696, -702, -728, -734, -740, -747, -753, -740, -759, -779, -772, -727, -747, -753, -753, -753, -753, -779, -778, -766, -727, -766, -740, -715, -715, -695, -664, -587, -606, -593, -574, -549, -549, -523, -498, -504, -510, -478, -472, -498, -498, -498, -491, -536, -536, -555, -600, -619, -632, -664, -676, -696, -702, -734, -734, -753, -759, -759, -759, -759, -766, -759, -760, -779, -753, -766, -766, -766, -766, -747, -747, -753, -740, -728, -721, -721, -708, -645, -664, -651, -638, -594, -574, -542, -530, -530, -504, -478, -453, -479, -498, -498, -511, -542, -536, -561, -581, -593, -625, -632, -657, -638, -702, -721, -760, -766, -766, -772, -753, -721, -772, -792, -772, -734, -766, -740, -721, -702, -683, -670, -651, -632, -625, -625, -606, -619, -593, -600, -562, -562, -555, -555, -555, -536, -555, -542, -536, -530, -530, -536, -536, -517, -485, -466, -459, -440, -434, -459, -466, -485, -504, -536, -562, -581, -613, -664, -689, -702, -721, -740, -760, -760, -779, -740, -766, -766, -760, -760, -766, -728, -709, -651, -683, -651, -632, -600, -574, -549, -549, -466, -504, -491, -479, -478, -479, -479, -510, -498, -536, -517, -562, -574, -600, -600, -638, -638, -638, -670, -664, -664, -670, -677, -683, -683, -677, -709, -683, -670, -696, -683, -696, -702, -689, -696, -721, -715, -721, -734, -734, -715, -728, -728, -760, -766, -753, -747, -772, -779, -785, -792, -779, -753, -760, -747, -728, -702, -645, -619, -575, -574, -555, -549, -523, -504, -536, -568, -581, -581, -581, -581, -555, -536, -517, -504, -459, -440, -440, -415, -440, -459, -479, -498, -517, -542, -562, -536, -581, -613, -619, -657, -664, -689, -709, -734, -734, -734, -747, -760, -753, -740, -747, -721, -696, -696, -702, -657, -651, -651, -651, -651, -657, -638, -658, -683, -670, -696, -683, -690, -689, -670, -689, -683, -677, -709, -689, -677, -670, -670, -638, -626, -632, -619, -600, -613, -638, -645, -645, -664, -747, -754, -773, -811, -804, -747, -779, -760, -766, -760, -740, -754, -753, -766, -753, -779, -785, -804, -798, -792, -792, -811, -792, -811, -792, -734, -792, -766, -741, -722, -690, -664, -658, -632, -613, -613, -600, -600, -575, -568, -555, -562, -556, -600, -600, -600, -600, -728, -734, -805, -824, -811, -811, -799, -785, -779, -747, -747, -734, -747, -715, -728, -747, -753, -766, -722, -798, -792, -798, -785, -786, -766, -766, -722, -760, -747, -728, -728, -728, -715, -722, -715, -734, -709, -715, -709, -702, -741, -754, -747, -773, -766, -754, -766, -785, -747, -754, -747, -734, -754, -747, -754, -747, -747, -754, -766, -773, -760, -747, -754, -760, -754, -766, -754, -766, -779, -766, -760, -766, -747, -773, -760, -766, -754, -760, -760, -766, -760, -754, -760, -741, -760, -754, -760
]

# Checking array lengths
print(f"Length of x: {len(x)}")
print(f"Length of y: {len(y)}")
print(f"Length of z: {len(z)}")

# Trim to minimum length
min_length = min(len(x), len(y), len(z))
x = np.array(x[:min_length])
y = np.array(y[:min_length])
z = np.array(z[:min_length])

# Create duplicates with opposite signs
x_neg = x
y_neg = y
z_neg = -z

# Saving all points (original + mirror) to a file
all_x = np.concatenate([x, x_neg])
all_y = np.concatenate([y, y_neg])
all_z = np.concatenate([z, z_neg])

with open('mag_samples_mirror.txt', 'w') as f:
    f.write(f'double x[{len(all_x)}] = {{\n')
    f.write(', '.join(f'{val:.0f}' for val in all_x))
    f.write('\n};\n\n')
    
    f.write(f'double y[{len(all_y)}] = {{\n')
    f.write(', '.join(f'{val:.0f}' for val in all_y))
    f.write('\n};\n\n')
    
    f.write(f'double z[{len(all_z)}] = {{\n')
    f.write(', '.join(f'{val:.0f}' for val in all_z))
    f.write('\n};\n')

print(f"\n{len(all_x)} points saved in mag_samples_mirror.txt")

# We close all old windows
plt.close('all')

# Create one window
fig = plt.figure(figsize=(14, 10))
ax = fig.add_subplot(111, projection='3d')

# Original dots - blue
ax.scatter(x, y, z, color='blue', s=30, alpha=0.7, edgecolors='black', linewidth=0.5, label='Original points')

# Duplicates with opposite sign - red
ax.scatter(x_neg, y_neg, z_neg, color='red', s=30, alpha=0.7, edgecolors='black', linewidth=0.5, label='Mirrored points')

# Axis settings
ax.set_xlabel('X', fontsize=12, fontweight='bold')
ax.set_ylabel('Y', fontsize=12, fontweight='bold')
ax.set_zlabel('Z', fontsize=12, fontweight='bold')
ax.set_title('Asymmetric Globe with Mirrored Points', fontsize=14, fontweight='bold')

# Center and radii (based on original and reflected points)
center_x = (np.max(all_x) + np.min(all_x)) / 2
center_y = (np.max(all_y) + np.min(all_y)) / 2
center_z = 0

radius_x = np.max(np.abs(all_x - center_x))
radius_y = np.max(np.abs(all_y - center_y))
radius_z = np.max(np.abs(all_z))

# Axle ranges (full)
ax.set_xlim([np.min(all_x), np.max(all_x)])
ax.set_ylim([np.min(all_y), np.max(all_y)])
ax.set_zlim([np.min(all_z), np.max(all_z)])

# Meridians
num_meridians = 16
theta = np.linspace(0, 2*np.pi, num_meridians)
for i in range(num_meridians):
    theta_fixed = theta[i]
    phi_vals = np.linspace(0, np.pi, 50)
    x_meridian = center_x + radius_x * np.sin(phi_vals) * np.cos(theta_fixed)
    y_meridian = center_y + radius_y * np.sin(phi_vals) * np.sin(theta_fixed)
    z_meridian = center_z + radius_z * np.cos(phi_vals)
    ax.plot(x_meridian, y_meridian, z_meridian, color='green', alpha=0.5, linewidth=1)

# Parallels
num_parallels = 9
phi = np.linspace(0, np.pi, num_parallels)
for j in range(1, num_parallels - 1):
    phi_fixed = phi[j]
    theta_vals = np.linspace(0, 2*np.pi, 50)
    x_parallel = center_x + radius_x * np.sin(phi_fixed) * np.cos(theta_vals)
    y_parallel = center_y + radius_y * np.sin(phi_fixed) * np.sin(theta_vals)
    z_parallel = center_z + radius_z * np.cos(phi_fixed)
    ax.plot(x_parallel, y_parallel, z_parallel, color='green', alpha=0.5, linewidth=1)

# Equator
phi_equator = np.pi / 2
theta_vals = np.linspace(0, 2*np.pi, 100)
x_equator = center_x + radius_x * np.sin(phi_equator) * np.cos(theta_vals)
y_equator = center_y + radius_y * np.sin(phi_equator) * np.sin(theta_vals)
z_equator = center_z + radius_z * np.cos(phi_equator)
ax.plot(x_equator, y_equator, z_equator, color='red', alpha=0.7, linewidth=2, label='Equator')

# Points
ax.scatter([0], [0], [0], color='red', s=50, marker='o', label='Origin (0,0,0)')
ax.scatter([center_x], [center_y], [center_z], color='yellow', s=100, marker='x', label='Globe center')

ax.legend(loc='upper right', fontsize=9)
plt.tight_layout()
plt.show()

# Statistics
print(f"\nOriginal points: {len(x)}")
print(f"Mirrored points: {len(x_neg)}")
print(f"Total points: {len(all_x)}")
print(f"Max X: {np.max(all_x):.2f}, Min X: {np.min(all_x):.2f}")
print(f"Max Y: {np.max(all_y):.2f}, Min Y: {np.min(all_y):.2f}")
print(f"Max Z: {np.max(all_z):.2f}, Min Z: {np.min(all_z):.2f}")
print(f"Globe center X: {center_x:.2f}")
print(f"Globe center Y: {center_y:.2f}")
print(f"Radius along X: {radius_x:.2f}")
print(f"Radius along Y: {radius_y:.2f}")
print(f"Radius along Z: {radius_z:.2f}")

The graphical output is as follows

If you mirror the data only along the Z-axis, after calibration, the values ​​for X and Y will be fine, but for the Z-axis, they'll be zero because it's just mirroring and doesn't make any difference.
But I physically checked the Z-axis beforehand and it's symmetrical, so this simplification can be used, taking into account what I wrote above—when there are limitations in sensor rotation, calibration is necessary.

=== Magnetometer Calibration Demo ===
Starting calibration process...
Calculating calibration data of data points was SUCCESSFUL
Variance of distances before calibration: 14535.088785
Variance of distances after calibration: 0.006497
[-169.2370737408, 252.5372623631, -0.0000000000, ]
Size (3, 3):
0.001, -0.000, -0.000, 
-0.000, 0.001, 0.000, 
-0.000, 0.000, 0.001, 
=== Calibration completed ===

If we mirror along all three axes,

x_neg = -x
y_neg = -y
z_neg = -z

The results aren't very good, and this conditional calibration becomes meaningless.

=== Magnetometer Calibration Demo ===
Starting calibration process...
Calculating calibration data of data points was SUCCESSFUL
Variance of distances before calibration: 11248.453739
Variance of distances after calibration: 0.018343
[-183.0454855248, 133.7776119548, -486.5039777289, ]
Size (3, 3):
0.002, -0.000, 0.000, 
-0.000, 0.002, 0.000, 
0.000, 0.000, 0.003, 
=== Calibration completed ===

I got the general idea, now I want to focus on preparing data for calibrations, filtering, and sampling.
Thank you for the library; it's quite interesting and has potential😃

[brightproject]

I ran another series of experiments, using the calibration from this repository(for brevity, I called it ellipsoid fit).

And the magneto 1.4

magneto_1_4.zip

I collected four types of DATA_MODE 1-3 were data only from the lower hemisphere, differing in the number of samples.
And DATA_MODE 4 was data mirrored along the Z axis with 1.400 samples.
By comparing the two algorithms, I confirmed that the readings began to correlate with a larger number of samples. If the sphere was completely dotted, even with a little trickery like mirroring, the values ​​from the algorithm from this repository and from magneto 1.4 were almost identical.

#define DATA_MODE 1

magneto 1.4

{
  7.076012, 1.191550, -0.123790, 0.140050
  8.300526, -0.123790, 1.066659, -0.004950
  -14.689161, 0.140050, -0.004950, 1.549803
}

ellipsoid fit

Variance of distances before calibration: 74.736556
Variance of distances after calibration: 0.013583
[0.5132172308, 15.9803264467, -33.1241886704, ]
Size (3, 3):
0.040, -0.009, 0.024, 
-0.009, 0.029, -0.010, 
0.024, -0.010, 0.095, 

#define DATA_MODE 2

magneto 1.4

{
  -2.144808, 1.694752, 0.711530, -0.031043
  11.775664, 0.711530, 2.024102, -0.040584
  -34.805695, -0.031043, -0.040584, 2.738141
}

ellipsoid fit

Variance of distances before calibration: 99.927141
Variance of distances after calibration: 0.063912
[7.8802226795, 11.2734712601, -34.0999234650, ]
Size (3, 3):
0.023, 0.019, -0.005,
0.019, 0.043, -0.004,
-0.005, -0.004, 0.076,

#define DATA_MODE 3

magneto 1.4

{
  -5.842445, 1.079770, 0.002900, -0.078242
  13.539189, 0.002900, 1.029069, -0.011129
  -11.348018, -0.078242, -0.011129, 1.356080
}

ellipsoid fit

Variance of distances before calibration: 49.279169
Variance of distances after calibration: 0.000708
[-3.9935165864, 12.1129365731, -18.5020502551, ]
Size (3, 3):
0.023, -0.000, -0.003, 
-0.000, 0.022, 0.000, 
-0.003, 0.000, 0.034, 

#define DATA_MODE 4

magneto 1.4

{
  -172.216937, 1.084169, -0.053777, -0.000000
  243.313327, -0.053777, 0.999311, 0.000000
  0.000000, -0.000000, 0.000000, 1.047515
}

ellipsoid fit

Variance of distances before calibration: 14535.088785
Variance of distances after calibration: 0.006497
[-169.2370737408, 252.5372623631, -0.0000000000, ]
Size (3, 3):
0.001, -0.000, -0.000, 
-0.000, 0.001, 0.000, 
-0.000, 0.000, 0.001, 

Code for calculations on the `ESP32` microcontroller:

#define _USE_MATH_DEFINES
#include <Arduino.h>

#include <stdio.h>
#include <stdlib.h>
#include <time.h>

extern "C" {
    #include "mag_calib_new/geometry.h"
}

// Calibration method selection:
// MAGNETO_1 - using magneto_1_4 library (ellipsoid fit)
// MAGNETO_2 - using standard calibration from geometry.h
#define MAGNETO_1

// ellipsoid fit calibration file
#ifdef MAGNETO_1
#include "magneto_1_4/magneto_1_4.h"

// Calibration class using ellipsoid fitting method MAGNETO 1.4
MagnetoCalibration* magneto_mag = new MagnetoCalibration();
#endif

#define SERIAL_BAUD 115200

// Select mode:
// 1 - 20 samples(low hemisphere)
// 2 - 100 samples(low hemisphere)
// 3 - 300 samples(low hemisphere)
// 4 - 1400 samples(lower and upper hemispheres)
#define DATA_MODE 1

// DEMO OF MAGNETOMETER CALIBRATION USING REAL DATA
// This demo:
// 1. Takes real magnetometer data points
// 2. Calculates calibration data (offset vector and transformation matrix)
// 3. Checks if calibration was successful
// 4. Calibrates the data points

typedef struct {
    Vector vx;
    Vector vy;
    Vector vz;
} mag_data_t;

char msgString[300];

float M_BAinv[4][3];              // Magnetometer calibration matrix

#if DATA_MODE == 1

// Variance of distances before calibration: 74.736556
// Variance of distances after calibration: 0.013583

    #define MAX_DATA_LEN 20

    double x[20] = {
        0.312500,   6.500000,   18.437500,  30.000000,  34.000000,   35.812500,  32.937500,  30.000000,  19.875000,  9.750000,
        -1.437500,  -10.125000, -16.625000, -23.875000, -28.937500,  -28.562500, -26.375000, -19.875000, -14.812500, -6.875000
    };

    double y[20] = {
        12.250000,  22.062500,  32.187500,  35.812500,  38.687500,   36.875000,  32.187500,  22.437500,  9.000000,    -4.312500,
        -15.875000, -22.750000, -25.312500, -17.687500, -5.062500,   8.687500,   26.750000,  33.250000,  40.187500,  48.875000
    };

    double z[20] = {
        -44.750000, -41.937500, -35.937500, -37.562500, -39.187500,  -42.437500, -44.062500, -46.875000, -47.250000, -46.062500,
        -41.625000, -36.375000, -32.312500, -28.312500, -28.312500,  -24.250000, -23.062500, -20.625000, -22.250000, -26.687500
    };

#elif DATA_MODE == 2

// Variance of distances before calibration: 99.927141
// Variance of distances after calibration: 0.063912

    #define MAX_DATA_LEN 100

    double x[100] = {
        -11.562500, -17.687500, -22.062500, -24.937500, -25.312500, -23.500000, -19.187500, -14.437500, -10.812500, -4.687500,
        0.312500, 3.250000, 6.500000, 14.062500, 18.437500, 24.250000, 30.000000, 32.937500, 34.000000, 35.062500,
        35.812500, 34.750000, 32.937500, 31.500000, 30.000000, 24.250000, 19.875000, 13.750000, 9.750000, 3.250000,
        -1.437500, -4.687500, -10.125000, -14.812500, -16.625000, -21.000000, -23.875000, -26.750000, -28.937500, -28.937500,
        -28.562500, -27.875000, -26.375000, -22.812500, -19.875000, -15.875000, -14.812500, -10.812500, -6.875000, -1.750000,
        3.250000, 8.687500, 14.437500, 19.187500, 24.562500, 27.875000, 31.812500, 34.750000, 35.812500, 35.437500,
        35.437500, 35.437500, 34.375000, 33.625000, 32.562500, 30.375000, 30.375000, 28.187500, 22.062500, 20.625000,
        12.250000, 11.937500, 5.750000, 0.687500, -3.250000, -8.687500, -14.062500, -16.625000, -20.625000, -25.687500,
        -28.187500, -31.812500, -35.125000, -38.375000, -36.187500, -38.687500, -39.062500, -41.625000, -41.250000, -41.625000
    };

    double y[100] = {
        12.250000, 17.000000, 22.062500, 27.125000, 32.187500, 35.437500, 35.812500, 38.000000, 38.687500, 40.125000,
        36.875000, 34.375000, 32.187500, 25.312500, 22.437500, 14.437500, 9.000000, 2.125000, -4.312500, -10.125000,
        -15.875000, -18.437500, -22.750000, -24.250000, -25.312500, -23.500000, -17.687500, -13.000000, -5.062500, 1.062500,
        8.687500, 18.437500, 26.750000, 30.375000, 33.250000, 36.875000, 40.187500, 45.250000, 48.875000, 49.562500,
        50.312500, 47.375000, 45.937500, 42.687500, 39.062500, 34.750000, 30.750000, 27.125000, 26.750000, 23.500000,
        18.062500, 14.437500, 14.437500, 3.250000, -0.312500, -4.312500, -9.375000, -14.812500, -19.875000, -23.125000,
        -26.750000, -28.937500, -27.875000, -26.062500, -23.500000, -20.250000, -13.375000, -7.562500, -1.062500, 6.500000,
        11.937500, 18.812500, 25.687500, 30.375000, 34.000000, 36.187500, 38.687500, 41.625000, 42.312500, 43.062500,
        44.125000, 44.500000, 43.062500, 39.062500, 34.000000, 28.937500, 27.500000, 23.125000, 17.687500, 13.750000,
        9.750000, 9.750000, 12.250000, 13.750000, 17.375000, 17.000000, 14.062500, 12.625000, 9.375000, 7.187500,
    };

    double z[100] = {
        -44.750000, -43.562500, -41.937500, -37.125000, -35.937500, -33.875000, -37.562500, -37.937500, -39.187500, -40.375000,
        -42.437500, -44.437500, -44.062500, -46.062500, -46.875000, -45.687500, -47.250000, -46.062500, -46.062500, -44.437500,
        -41.625000, -38.812500, -36.375000, -33.937500, -32.312500, -30.312500, -28.312500, -29.125000, -28.312500, -27.875000,
        -24.250000, -25.875000, -23.062500, -20.625000, -20.625000, -21.000000, -22.250000, -24.625000, -25.500000, -26.687500,
        -30.312500, -31.562500, -35.562500, -38.000000, -40.062500, -42.875000, -45.687500, -45.312500, -46.937500, -45.312500,
        -44.875000, -47.312500, -46.562500, -46.125000, -44.500000, -46.125000, -44.125000, -41.250000, -40.062500, -37.250000,
        -35.187500, -29.937500, -31.125000, -29.937500, -30.750000, -28.687500, -29.500000, -29.125000, -27.500000, -27.125000,
        -25.500000, -23.875000, -21.437500, -20.187500, -20.187500, -20.187500, -20.625000, -21.812500, -23.437500, -27.937500,
        -27.937500, -34.000000, -40.875000, -43.312500, -46.937500, -48.187500, -50.187500, -49.375000, -49.812500, -48.562500,
        -48.562500, -46.937500, -43.312500, -42.875000, -42.062500, -42.500000, -41.250000, -39.250000, -40.062500, -40.437500
    };

#elif DATA_MODE == 3

// Variance of distances before calibration: 49.279169
// Variance of distances after calibration: 0.000708

    #define MAX_DATA_LEN 300

    double x[MAX_DATA_LEN] = {
        9.375000, 11.937500, 13.750000, 14.812500, 17.312500, 16.625000, 19.500000, 18.812500, 19.500000, 18.812500,
        18.437500, 16.625000, 14.812500, 12.625000, 9.750000, 7.937500, 6.125000, 2.500000, 0.000000, -2.875000,
        -4.312500, -8.687500, -11.562500, -14.812500, -17.687500, -20.625000, -22.062500, -23.125000, -24.937500, -24.250000,
        -25.312500, -24.937500, -23.500000, -21.312500, -19.187500, -17.000000, -14.437500, -13.375000, -10.812500, -8.312500,
        -4.687500, -3.250000, 0.312500, 1.062500, 3.250000, 4.312500, 6.500000, 8.687500, 14.062500, 14.812500,
        18.437500, 21.000000, 24.250000, 26.750000, 30.000000, 31.125000, 32.937500, 32.937500, 34.000000, 35.812500,
        35.062500, 35.062500, 35.812500, 34.750000, 34.750000, 33.625000, 32.937500, 32.562500, 31.500000, 30.750000,
        30.000000, 25.687500, 24.250000, 21.687500, 19.875000, 18.062500, 13.750000, 11.562500, 9.750000, 5.750000,
        3.250000, 1.062500, -1.437500, -3.562500, -4.687500, -7.562500, -10.125000, -12.625000, -14.812500, -16.625000,
        -16.625000, -19.500000, -21.000000, -23.125000, -23.875000, -25.687500, -26.750000, -27.500000, -28.937500, -28.937500,
        -28.937500, -28.937500, -28.562500, -28.937500, -27.875000, -27.875000, -26.375000, -24.562500, -22.812500, -22.062500,
        -19.875000, -18.062500, -15.875000, -15.875000, -14.812500, -12.250000, -10.812500, -8.312500, -6.875000, -4.312500,
        -1.750000, 1.062500, 3.250000, 6.500000, 8.687500, 11.187500, 14.437500, 17.000000, 19.187500, 22.062500,
        24.562500, 27.125000, 27.875000, 30.375000, 31.812500, 32.937500, 34.750000, 35.812500, 35.812500, 35.437500,
        35.437500, 35.437500, 37.250000, 35.437500, 34.375000, 34.000000, 33.625000, 32.187500, 32.562500, 30.375000,
        30.375000, 26.750000, 28.187500, 26.062500, 22.062500, 18.437500, 20.625000, 14.812500, 12.250000, 11.937500,
        11.937500, 7.187500, 5.750000, 3.250000, 0.687500, 1.062500, -3.250000, -5.750000, -8.687500, -10.500000,
        -14.062500, -14.437500, -16.625000, -17.375000, -20.625000, -22.437500, -25.687500, -26.750000, -28.187500, -30.750000,
        -31.812500, -33.312500, -35.125000, -36.562500, -38.375000, -38.375000, -36.187500, -38.000000, -38.687500, -38.687500,
        -39.062500, -39.812500, -41.625000, -42.687500, -41.250000, -41.625000, -41.625000, -40.187500, -38.687500, -35.125000,
        -31.500000, -28.562500, -24.937500, -23.125000, -21.000000, -18.437500, -16.250000, -14.437500, -12.625000, -10.812500,
        -10.125000, -9.000000, -8.687500, -7.187500, -5.750000, -5.750000, -5.750000, -5.750000, -5.750000, -5.750000,
        6.500000, 8.687500, 11.187500, 11.937500, 13.000000, 14.812500, 15.187500, 14.812500, 13.000000, 12.625000,
        11.562500, 8.687500, 5.062500, 2.500000, -1.062500, -5.375000, -9.375000, -12.625000, -17.687500, -21.312500,
        -26.062500, -29.312500, -32.562500, -33.312500, -33.625000, -31.812500, -32.562500, -33.312500, -29.312500, -26.062500,
        -25.687500, -20.250000, -17.687500, -15.562500, -14.062500, -13.750000, -12.312500, -10.500000, -7.187500, -6.500000,
        -4.312500, -2.500000, -1.437500, -1.750000, -0.687500, -1.062500, -1.437500, -1.750000, -2.875000, -5.750000,
        -7.562500, -8.687500, -13.750000, -13.375000, -12.312500, -13.000000, -15.187500, -16.625000, -15.562500, -13.750000,
        -15.562500, -14.812500, -11.937500, -11.187500, -9.375000, -9.375000, -9.750000, -9.750000, -7.187500, -8.312500,
        -7.562500, -6.125000, -2.125000, -1.750000, 0.000000, 1.750000, 3.250000, 4.687500, 6.500000, 5.062500
    };

    double y[MAX_DATA_LEN] = {
        12.250000, 14.437500, 17.000000, 19.500000, 22.062500, 25.312500, 27.125000, 29.625000, 32.187500, 32.937500,
        35.437500, 35.437500, 35.812500, 37.250000, 38.000000, 38.375000, 38.687500, 40.875000, 40.125000, 38.375000,
        36.875000, 34.750000, 34.375000, 32.187500, 27.500000, 25.312500, 22.437500, 18.812500, 14.437500, 11.187500,
        9.000000, 6.500000, 2.125000, -0.312500, -4.312500, -7.187500, -10.125000, -13.000000, -15.875000, -17.687500,
        -18.437500, -20.625000, -22.750000, -24.937500, -24.250000, -24.937500, -25.312500, -24.937500, -23.500000, -20.625000,
        -17.687500, -15.187500, -13.000000, -8.687500, -5.062500, -1.437500, 1.062500, 4.312500, 8.687500, 12.625000,
        18.437500, 22.750000, 26.750000, 28.187500, 30.375000, 31.500000, 33.250000, 35.062500, 36.875000, 39.062500,
        40.187500, 43.062500, 45.250000, 46.312500, 48.875000, 48.875000, 49.562500, 51.375000, 50.312500, 50.312500,
        47.375000, 47.750000, 45.937500, 44.500000, 42.687500, 40.500000, 39.062500, 36.875000, 34.750000, 33.312500,
        30.750000, 29.312500, 27.125000, 26.062500, 26.750000, 25.687500, 23.500000, 21.312500, 18.062500, 14.437500,
        14.437500, 14.437500, 5.750000, 3.250000, -0.312500, -2.875000, -4.312500, -7.562500, -9.375000, -11.562500,
        -14.812500, -19.187500, -19.875000, -22.437500, -23.125000, -25.687500, -26.750000, -26.750000, -27.125000, -27.875000,
        -28.937500, -28.187500, -27.875000, -26.375000, -26.062500, -24.937500, -23.500000, -22.437500, -20.250000, -14.812500,
        -13.375000, -9.750000, -7.562500, -5.375000, -1.062500, 2.125000, 6.500000, 9.375000, 11.937500, 15.875000,
        18.812500, 22.437500, 25.687500, 28.187500, 30.375000, 32.187500, 34.000000, 35.125000, 36.187500, 38.687500,
        40.500000, 41.625000, 42.312500, 43.437500, 43.062500, 44.125000, 44.500000, 44.125000, 43.062500, 39.062500,
        36.875000, 34.000000, 31.812500, 28.937500, 27.500000, 26.062500, 23.125000, 20.625000, 17.687500, 16.625000,
        13.750000, 12.625000, 9.750000, 9.750000, 10.125000, 11.562500, 12.250000, 13.750000, 13.750000, 14.062500,
        17.375000, 17.375000, 17.000000, 14.812500, 14.062500, 11.187500, 12.625000, 10.812500, 9.375000, 10.500000,
        7.187500, 6.875000, 6.875000, 6.125000, 7.187500, 7.937500, 7.187500, 8.687500, 7.187500, 7.937500,
        6.125000, 5.375000, 3.250000, 1.062500, -2.125000, -5.375000, -10.125000, -13.000000, -16.625000, -20.625000,
        -24.562500, -24.562500, -26.375000, -28.562500, -32.937500, -32.937500, -32.937500, -32.937500, -32.937500, -32.937500,
        -34.750000, -32.937500, -31.500000, -29.312500, -27.125000, -23.125000, -20.250000, -17.375000, -13.750000, -10.812500,
        -5.750000, 0.312500, 4.312500, 7.937500, 10.812500, 12.625000, 15.875000, 18.437500, 21.000000, 23.500000,
        27.125000, 30.000000, 34.000000, 35.125000, 39.437500, 40.875000, 43.062500, 43.437500, 43.437500, 45.250000,
        44.500000, 45.625000, 45.250000, 44.875000, 43.437500, 42.687500, 40.500000, 38.000000, 35.812500, 33.312500,
        29.687500, 25.312500, 20.625000, 15.562500, 10.500000, 9.375000, 6.500000, 5.750000, 2.125000, -1.750000,
        -3.250000, -4.687500, -7.187500, -10.500000, -15.562500, -18.062500, -21.687500, -23.875000, -26.062500, -27.500000,
        -28.562500, -29.687500, -29.687500, -31.500000, -33.312500, -33.312500, -33.312000, -34.375000, -34.000000, -33.625000,
        -33.625000, -30.375000, -28.937500, -27.500000, -25.312500, -25.312500, -24.625000, -22.812500, -19.500000, -15.562500
    };

    double z[MAX_DATA_LEN] = {
        -44.750000, -44.375000, -43.562500, -41.937500, -41.937500, -39.125000, -37.125000, -36.312500, -35.937500, -36.312500,
        -33.875000, -35.937500, -37.562500, -37.562500, -37.937500, -40.000000, -39.187500, -40.812500, -40.375000, -42.000000,
        -42.437500, -44.437500, -44.437500, -44.062500, -46.062500, -45.625000, -46.875000, -46.875000, -45.687500, -45.687500,
        -47.250000, -45.687500, -46.062500, -45.250000, -46.062500, -44.875000, -44.437500, -43.250000, -41.625000, -39.625000,
        -38.812500, -38.375000, -36.375000, -34.750000, -33.937500, -32.312500, -32.312500, -30.312500, -28.312500, -29.500000,
        -29.125000, -29.937500, -28.312500, -28.687500, -27.875000, -26.687500, -24.250000, -26.312500, -25.875000, -25.062500,
        -23.062500, -21.000000, -20.625000, -21.000000, -20.625000, -21.437500, -21.000000, -20.625000, -22.250000, -21.812500,
        -24.625000, -24.687500, -25.500000, -27.062500, -26.687500, -25.062500, -30.312500, -29.937500, -31.562500, -33.562500,
        -35.562500, -36.000000, -38.000000, -40.062500, -40.062500, -41.250000, -42.875000, -43.687500, -45.687500, -45.687500,
        -45.312500, -46.500000, -46.937500, -45.312500, -46.500000, -46.125000, -44.875000, -43.687500, -47.312500, -46.562500,
        -46.562500, -46.562500, -46.125000, -44.500000, -44.500000, -46.125000, -44.500000, -44.937500, -44.125000, -42.875000,
        -41.250000, -40.062500, -40.062500, -38.437500, -37.250000, -36.375000, -35.187500, -34.000000, -29.937500, -31.937500,
        -31.125000, -30.312500, -29.937500, -29.937500, -30.750000, -29.125000, -28.687500, -27.937500, -29.500000, -29.937500,
        -29.125000, -29.500000, -27.500000, -27.500000, -27.125000, -26.687500, -25.500000, -24.687500, -23.875000, -23.437500,
        -21.437500, -21.000000, -20.187500, -18.187500, -20.187500, -17.812500, -20.187500, -20.625000, -20.187500, -21.812500,
        -21.812500, -23.062500, -23.437500, -24.687500, -27.937500, -29.125000, -27.937500, -32.375000, -34.000000, -37.250000,
        -40.875000, -41.687500, -43.312500, -45.312500, -46.937500, -45.312500, -48.187500, -49.375000, -50.187500, -50.187500,
        -49.375000, -49.000000, -49.812500, -49.000000, -48.562500, -49.000000, -48.562500, -46.562500, -46.562500, -45.750000,
        -46.937500, -43.312500, -43.312500, -42.875000, -41.687500, -42.062500, -42.125000, -42.500000, -42.500000, -41.312500,
        -41.250000, -40.437500, -39.250000, -38.875000, -40.062500, -39.250000, -40.437500, -41.250000, -41.312500, -42.500000,
        -44.125000, -44.937500, -46.562500, -46.562500, -45.750000, -44.500000, -44.500000, -41.687500, -39.250000, -36.812500,
        -35.187500, -33.562500, -31.187500, -29.125000, -23.875000, -23.875000, -23.875000, -23.875000, -23.875000, -23.875000,
        -16.187500, -17.812500, -19.812500, -21.812500, -25.062500, -28.312500, -31.187500, -34.375000, -38.062500, -40.437500,
        -42.062500, -44.937500, -46.125000, -48.187500, -48.562500, -46.937500, -49.000000, -49.375000, -49.000000, -46.937500,
        -45.312500, -41.687500, -40.062500, -39.687500, -37.625000, -36.437500, -34.375000, -35.187500, -36.437500, -34.812500,
        -36.437500, -37.250000, -38.437500, -39.250000, -40.062500, -40.500000, -41.312500, -43.312500, -43.312500, -44.937500,
        -45.312500, -44.937500, -44.500000, -47.375000, -46.125000, -47.375000, -46.937500, -46.937500, -45.750000, -44.937500,
        -44.937500, -45.312500, -42.937500, -42.500000, -40.062500, -38.437500, -35.187500, -32.750000, -32.000000, -27.937500,
        -29.125000, -28.312500, -27.500000, -25.062500, -22.250000, -22.687500, -21.875000, -22.687500, -23.062500, -24.687500,
        -27.125000, -28.750000, -30.750000, -32.375000, -33.187500, -32.375000, -33.187500, -34.812500, -37.625000, -40.062500
    };

#elif DATA_MODE == 4

// Variance of distances before calibration: 14535.088785
// Variance of distances after calibration: 0.006497

    #define MAX_DATA_LEN 1400

double x[MAX_DATA_LEN] = {
109.0, 115.0, 98.0, 109.0, 92.0, 98.0, 80.0, 57.0, 40.0, 40.0, 40.0, 40.0, 40.0, -23.0, -52.0, -80.0, -75.0, -86.0, -86.0, -75.0, -92.0, -52.0, -52.0, -5.0, 34.0, 69.0, 104.0, 144.0, 167.0, 190.0, 237.0, 260.0, 283.0, 306.0, 335.0, 370.0, 364.0, 381.0, 404.0, 416.0, 416.0, 427.0, 404.0, 410.0, 387.0, 387.0, 358.0, 283.0, 265.0, 225.0, 213.0, 144.0, 98.0, 69.0, 28.0, -5.0, -52.0, -80.0, -92.0, -144.0, -156.0, -167.0, -179.0, -190.0, -173.0, -150.0, -121.0, -115.0, -69.0, -46.0, 0.0, 52.0, 80.0, 115.0, 167.0, 208.0, 260.0, 271.0, 300.0, 335.0, 352.0, 381.0, 364.0, 381.0, 381.0, 393.0, 364.0, 352.0, 346.0, 300.0, 283.0, 265.0, 231.0, 202.0, 144.0, 98.0, 57.0, -11.0, -46.0, -86.0, -109.0, -150.0, -179.0, -237.0, -237.0, -271.0, -294.0, -335.0, -364.0, -375.0, -393.0, -398.0, -381.0, -387.0, -375.0, -329.0, -271.0, -254.0, -237.0, -202.0, -173.0, -138.0, -109.0, -92.0, -75.0, -52.0, -40.0, -40.0, -40.0, -23.0, -28.0, -34.0, -28.0, -23.0, -23.0, -11.0, 0.0, 23.0, 75.0, 40.0, -5.0, -28.0, -80.0, -98.0, -132.0, -173.0, -219.0, -248.0, -294.0, -364.0, -375.0, -433.0, -433.0, -416.0, -433.0, -456.0, -439.0, -450.0, -410.0, -410.0, -398.0, -352.0, -335.0, -323.0, -318.0, -318.0, -306.0, -283.0, -271.0, -265.0, -248.0, -242.0, -237.0, -196.0, -202.0, -202.0, -202.0, -231.0, -225.0, -208.0, -173.0, -190.0, -196.0, -202.0, -225.0, -242.0, -237.0, -248.0, -260.0, -271.0, -283.0, -294.0, -294.0, -283.0, -271.0, -242.0, -208.0, -190.0, -179.0, -132.0, -121.0, -69.0, -69.0, -69.0, -23.0, -28.0, -17.0, 11.0, 17.0, 46.0, 86.0, 127.0, 138.0, 150.0, 132.0, 109.0, 86.0, 40.0, -5.0, -23.0, -86.0, -132.0, -173.0, -231.0, -294.0, -335.0, -375.0, -404.0, -433.0, -451.0, -474.0, -514.0, -526.0, -566.0, -601.0, -601.0, -630.0, -664.0, -699.0, -699.0, -734.0, -769.0, -769.0, -780.0, -780.0, -705.0, -699.0, -659.0, -641.0, -583.0, -514.0, -485.0, -445.0, -387.0, -312.0, -242.0, -190.0, -150.0, -104.0, -69.0, -40.0, -23.0, 5.0, 17.0, 11.0, 17.0, 23.0, 46.0, 0.0, -17.0, -28.0, -34.0, -57.0, -80.0, -86.0, -121.0, -115.0, -161.0, -161.0, -185.0, -248.0, -271.0, -283.0, -318.0, -358.0, -375.0, -404.0, -439.0, -474.0, -451.0, -514.0, -520.0, -508.0, -549.0, -549.0, -549.0, -531.0, -531.0, -503.0, -520.0, -514.0, -479.0, -445.0, -439.0, -422.0, -370.0, -323.0, -300.0, -248.0, -248.0, -213.0, -190.0, -185.0, -179.0, -167.0, -179.0, -202.0, -219.0, -260.0, -300.0, -335.0, -381.0, -422.0, -497.0, -531.0, -566.0, -601.0, -630.0, -676.0, -722.0, -745.0, -780.0, -815.0, -815.0, -832.0, -821.0, -815.0, -821.0, -803.0, -774.0, -757.0, -717.0, -676.0, -636.0, -595.0, -560.0, -497.0, -451.0, -398.0, -352.0, -300.0, -242.0, -202.0, -156.0, -121.0, -75.0, -40.0, 0.0, -5.0, 11.0, 11.0, 11.0, 11.0, -34.0, -80.0, -156.0, -202.0, -254.0, -294.0, -346.0, -398.0, -456.0, -526.0, -578.0, -624.0, -659.0, -693.0, -740.0, -769.0, -786.0, -792.0, -809.0, -826.0, -821.0, -821.0, -815.0, -815.0, -815.0, -792.0, -792.0, -763.0, -722.0, -705.0, -670.0, -630.0, -578.0, -531.0, -468.0, -398.0, -346.0, -300.0, -237.0, -208.0, -173.0, -127.0, -104.0, -109.0, -109.0, -115.0, -132.0, -144.0, -150.0, -213.0, -213.0, -265.0, -306.0, -323.0, -358.0, -393.0, -422.0, -451.0, -479.0, -531.0, -531.0, -601.0, -618.0, -647.0, -688.0, -716.0, -728.0, -745.0, -751.0, -751.0, -751.0, -740.0, -757.0, -728.0, -716.0, -688.0, -682.0, -653.0, -612.0, -566.0, -514.0, -485.0, -439.0, -387.0, -358.0, -318.0, -289.0, -265.0, -225.0, -208.0, -196.0, -190.0, -213.0, -219.0, -237.0, -237.0, -277.0, -294.0, -323.0, -370.0, -381.0, -427.0, -468.0, -520.0, -543.0, -560.0, -589.0, -595.0, -630.0, -647.0, -682.0, -717.0, -717.0, -751.0, -751.0, -751.0, -751.0, -722.0, -711.0, -705.0, -670.0, -641.0, -630.0, -595.0, -543.0, -560.0, -537.0, -520.0, -474.0, -508.0, -451.0, -479.0, -433.0, -416.0, -433.0, -387.0, -370.0, -335.0, -329.0, -283.0, -254.0, -237.0, -208.0, -167.0, -132.0, -109.0, -121.0, -121.0, -156.0, -167.0, -185.0, -202.0, -237.0, -294.0, -312.0, -381.0, -427.0, -456.0, -514.0, -549.0, -595.0, -670.0, -676.0, -751.0, -769.0, -803.0, -809.0, -809.0, -832.0, -815.0, -838.0, -815.0, -797.0, -780.0, -763.0, -717.0, -711.0, -653.0, -636.0, -595.0, -555.0, -514.0, -474.0, -433.0, -393.0, -387.0, -370.0, -329.0, -289.0, -277.0, -237.0, -202.0, -179.0, -173.0, -138.0, -127.0, -92.0, -80.0, -57.0, -28.0, -17.0, 5.0, 17.0, 23.0, 5.0, -5.0, -34.0, -17.0, 0.0, 11.0, 5.0, -34.0, -86.0, -132.0, -213.0, -300.0, -387.0, -474.0, -549.0, -630.0, -676.0, -699.0, -705.0, -693.0, -699.0, -693.0, -641.0, -612.0, -572.0, -531.0, -485.0, -485.0, -485.0, -485.0, -144.0, -104.0, -17.0, 5.0, 109.0, 127.0, 173.0, 208.0, 219.0, 254.0, 271.0, 323.0, 346.0, 364.0, 335.0, 352.0, 352.0, 364.0, 335.0, 335.0, 283.0, 213.0, 167.0, 121.0, 34.0, -57.0, -138.0, -219.0, -289.0, -346.0, -404.0, -445.0, -508.0, -549.0, -584.0, -595.0, -630.0, -624.0, -624.0, -624.0, -601.0, -584.0, -543.0, -537.0, -468.0, -422.0, -381.0, -318.0, -271.0, -260.0, -231.0, -208.0, -185.0, -127.0, -132.0, -92.0, -63.0, 0.0, 52.0, 98.0, 150.0, 167.0, 196.0, 219.0, 237.0, 237.0, 208.0, 202.0, 179.0, 121.0, 63.0, 0.0, -86.0, -144.0, -185.0, -283.0, -341.0, -398.0, -422.0, -456.0, -462.0, -468.0, -439.0, -416.0, -398.0, -381.0, -381.0, -346.0, -289.0, -231.0, -185.0, -138.0, -80.0, -28.0, 23.0, 86.0, 144.0, 173.0, 237.0, 109.0, 115.0, 98.0, 109.0, 92.0, 98.0, 80.0, 57.0, 40.0, 40.0, 40.0, 40.0, 40.0, -23.0, -52.0, -80.0, -75.0, -86.0, -86.0, -75.0, -92.0, -52.0, -52.0, -5.0, 34.0, 69.0, 104.0, 144.0, 167.0, 190.0, 237.0, 260.0, 283.0, 306.0, 335.0, 370.0, 364.0, 381.0, 404.0, 416.0, 416.0, 427.0, 404.0, 410.0, 387.0, 387.0, 358.0, 283.0, 265.0, 225.0, 213.0, 144.0, 98.0, 69.0, 28.0, -5.0, -52.0, -80.0, -92.0, -144.0, -156.0, -167.0, -179.0, -190.0, -173.0, -150.0, -121.0, -115.0, -69.0, -46.0, 0.0, 52.0, 80.0, 115.0, 167.0, 208.0, 260.0, 271.0, 300.0, 335.0, 352.0, 381.0, 364.0, 381.0, 381.0, 393.0, 364.0, 352.0, 346.0, 300.0, 283.0, 265.0, 231.0, 202.0, 144.0, 98.0, 57.0, -11.0, -46.0, -86.0, -109.0, -150.0, -179.0, -237.0, -237.0, -271.0, -294.0, -335.0, -364.0, -375.0, -393.0, -398.0, -381.0, -387.0, -375.0, -329.0, -271.0, -254.0, -237.0, -202.0, -173.0, -138.0, -109.0, -92.0, -75.0, -52.0, -40.0, -40.0, -40.0, -23.0, -28.0, -34.0, -28.0, -23.0, -23.0, -11.0, 0.0, 23.0, 75.0, 40.0, -5.0, -28.0, -80.0, -98.0, -132.0, -173.0, -219.0, -248.0, -294.0, -364.0, -375.0, -433.0, -433.0, -416.0, -433.0, -456.0, -439.0, -450.0, -410.0, -410.0, -398.0, -352.0, -335.0, -323.0, -318.0, -318.0, -306.0, -283.0, -271.0, -265.0, -248.0, -242.0, -237.0, -196.0, -202.0, -202.0, -202.0, -231.0, -225.0, -208.0, -173.0, -190.0, -196.0, -202.0, -225.0, -242.0, -237.0, -248.0, -260.0, -271.0, -283.0, -294.0, -294.0, -283.0, -271.0, -242.0, -208.0, -190.0, -179.0, -132.0, -121.0, -69.0, -69.0, -69.0, -23.0, -28.0, -17.0, 11.0, 17.0, 46.0, 86.0, 127.0, 138.0, 150.0, 132.0, 109.0, 86.0, 40.0, -5.0, -23.0, -86.0, -132.0, -173.0, -231.0, -294.0, -335.0, -375.0, -404.0, -433.0, -451.0, -474.0, -514.0, -526.0, -566.0, -601.0, -601.0, -630.0, -664.0, -699.0, -699.0, -734.0, -769.0, -769.0, -780.0, -780.0, -705.0, -699.0, -659.0, -641.0, -583.0, -514.0, -485.0, -445.0, -387.0, -312.0, -242.0, -190.0, -150.0, -104.0, -69.0, -40.0, -23.0, 5.0, 17.0, 11.0, 17.0, 23.0, 46.0, 0.0, -17.0, -28.0, -34.0, -57.0, -80.0, -86.0, -121.0, -115.0, -161.0, -161.0, -185.0, -248.0, -271.0, -283.0, -318.0, -358.0, -375.0, -404.0, -439.0, -474.0, -451.0, -514.0, -520.0, -508.0, -549.0, -549.0, -549.0, -531.0, -531.0, -503.0, -520.0, -514.0, -479.0, -445.0, -439.0, -422.0, -370.0, -323.0, -300.0, -248.0, -248.0, -213.0, -190.0, -185.0, -179.0, -167.0, -179.0, -202.0, -219.0, -260.0, -300.0, -335.0, -381.0, -422.0, -497.0, -531.0, -566.0, -601.0, -630.0, -676.0, -722.0, -745.0, -780.0, -815.0, -815.0, -832.0, -821.0, -815.0, -821.0, -803.0, -774.0, -757.0, -717.0, -676.0, -636.0, -595.0, -560.0, -497.0, -451.0, -398.0, -352.0, -300.0, -242.0, -202.0, -156.0, -121.0, -75.0, -40.0, 0.0, -5.0, 11.0, 11.0, 11.0, 11.0, -34.0, -80.0, -156.0, -202.0, -254.0, -294.0, -346.0, -398.0, -456.0, -526.0, -578.0, -624.0, -659.0, -693.0, -740.0, -769.0, -786.0, -792.0, -809.0, -826.0, -821.0, -821.0, -815.0, -815.0, -815.0, -792.0, -792.0, -763.0, -722.0, -705.0, -670.0, -630.0, -578.0, -531.0, -468.0, -398.0, -346.0, -300.0, -237.0, -208.0, -173.0, -127.0, -104.0, -109.0, -109.0, -115.0, -132.0, -144.0, -150.0, -213.0, -213.0, -265.0, -306.0, -323.0, -358.0, -393.0, -422.0, -451.0, -479.0, -531.0, -531.0, -601.0, -618.0, -647.0, -688.0, -716.0, -728.0, -745.0, -751.0, -751.0, -751.0, -740.0, -757.0, -728.0, -716.0, -688.0, -682.0, -653.0, -612.0, -566.0, -514.0, -485.0, -439.0, -387.0, -358.0, -318.0, -289.0, -265.0, -225.0, -208.0, -196.0, -190.0, -213.0, -219.0, -237.0, -237.0, -277.0, -294.0, -323.0, -370.0, -381.0, -427.0, -468.0, -520.0, -543.0, -560.0, -589.0, -595.0, -630.0, -647.0, -682.0, -717.0, -717.0, -751.0, -751.0, -751.0, -751.0, -722.0, -711.0, -705.0, -670.0, -641.0, -630.0, -595.0, -543.0, -560.0, -537.0, -520.0, -474.0, -508.0, -451.0, -479.0, -433.0, -416.0, -433.0, -387.0, -370.0, -335.0, -329.0, -283.0, -254.0, -237.0, -208.0, -167.0, -132.0, -109.0, -121.0, -121.0, -156.0, -167.0, -185.0, -202.0, -237.0, -294.0, -312.0, -381.0, -427.0, -456.0, -514.0, -549.0, -595.0, -670.0, -676.0, -751.0, -769.0, -803.0, -809.0, -809.0, -832.0, -815.0, -838.0, -815.0, -797.0, -780.0, -763.0, -717.0, -711.0, -653.0, -636.0, -595.0, -555.0, -514.0, -474.0, -433.0, -393.0, -387.0, -370.0, -329.0, -289.0, -277.0, -237.0, -202.0, -179.0, -173.0, -138.0, -127.0, -92.0, -80.0, -57.0, -28.0, -17.0, 5.0, 17.0, 23.0, 5.0, -5.0, -34.0, -17.0, 0.0, 11.0, 5.0, -34.0, -86.0, -132.0, -213.0, -300.0, -387.0, -474.0, -549.0, -630.0, -676.0, -699.0, -705.0, -693.0, -699.0, -693.0, -641.0, -612.0, -572.0, -531.0, -485.0, -485.0, -485.0, -485.0, -144.0, -104.0, -17.0, 5.0, 109.0, 127.0, 173.0, 208.0, 219.0, 254.0, 271.0, 323.0, 346.0, 364.0, 335.0, 352.0, 352.0, 364.0, 335.0, 335.0, 283.0, 213.0, 167.0, 121.0, 34.0, -57.0, -138.0, -219.0, -289.0, -346.0, -404.0, -445.0, -508.0, -549.0, -584.0, -595.0, -630.0, -624.0, -624.0, -624.0, -601.0, -584.0, -543.0, -537.0, -468.0, -422.0, -381.0, -318.0, -271.0, -260.0, -231.0, -208.0, -185.0, -127.0, -132.0, -92.0, -63.0, 0.0, 52.0, 98.0, 150.0, 167.0, 196.0, 219.0, 237.0, 237.0, 208.0, 202.0, 179.0, 121.0, 63.0, 0.0, -86.0, -144.0, -185.0, -283.0, -341.0, -398.0, -422.0, -456.0, -462.0, -468.0, -439.0, -416.0, -398.0, -381.0, -381.0, -346.0, -289.0, -231.0, -185.0, -138.0, -80.0, -28.0, 23.0, 86.0, 144.0, 173.0, 237.0
};

double y[MAX_DATA_LEN] = {
196.0, 196.0, 179.0, 196.0, 190.0, 167.0, 150.0, 150.0, 127.0, 127.0, 127.0, 127.0, 127.0, 127.0, 92.0, 98.0, 127.0, 127.0, 156.0, 179.0, 213.0, 248.0, 271.0, 300.0, 341.0, 346.0, 358.0, 387.0, 404.0, 410.0, 410.0, 422.0, 416.0, 427.0, 410.0, 404.0, 375.0, 358.0, 335.0, 318.0, 277.0, 248.0, 208.0, 173.0, 132.0, 69.0, 28.0, 11.0, -23.0, -34.0, -57.0, -57.0, -57.0, -52.0, -28.0, -17.0, 0.0, 11.0, 69.0, 86.0, 138.0, 185.0, 237.0, 283.0, 318.0, 358.0, 387.0, 387.0, 398.0, 393.0, 416.0, 398.0, 393.0, 404.0, 393.0, 375.0, 364.0, 329.0, 318.0, 271.0, 260.0, 219.0, 185.0, 156.0, 150.0, 121.0, 86.0, 80.0, 40.0, 23.0, 17.0, 0.0, -40.0, -40.0, -63.0, -57.0, -75.0, -115.0, -115.0, -115.0, -115.0, -115.0, -92.0, -75.0, -63.0, -34.0, -28.0, 11.0, 28.0, 52.0, 115.0, 150.0, 173.0, 202.0, 202.0, 237.0, 237.0, 231.0, 213.0, 219.0, 167.0, 138.0, 98.0, 34.0, -17.0, -34.0, -75.0, -75.0, -75.0, -202.0, -254.0, -318.0, -335.0, -346.0, -335.0, -341.0, -341.0, -329.0, -300.0, -294.0, -318.0, -318.0, -318.0, -277.0, -254.0, -225.0, -213.0, -213.0, -213.0, -185.0, -156.0, -121.0, -104.0, -115.0, -80.0, -109.0, -98.0, -92.0, -104.0, -75.0, -75.0, -63.0, -46.0, -5.0, 0.0, -17.0, -17.0, -17.0, -11.0, -11.0, -17.0, -17.0, 11.0, -5.0, -11.0, -11.0, -11.0, 0.0, -11.0, -11.0, -28.0, -63.0, -86.0, -132.0, -179.0, -242.0, -248.0, -277.0, -300.0, -300.0, -300.0, -318.0, -277.0, -260.0, -225.0, -156.0, -115.0, -69.0, 5.0, 69.0, 121.0, 144.0, 185.0, 231.0, 231.0, 271.0, 277.0, 294.0, 289.0, 294.0, 271.0, 265.0, 260.0, 248.0, 213.0, 167.0, 127.0, 63.0, 17.0, -17.0, -69.0, -115.0, -161.0, -208.0, -254.0, -329.0, -335.0, -370.0, -381.0, -398.0, -422.0, -416.0, -398.0, -398.0, -404.0, -364.0, -335.0, -318.0, -260.0, -237.0, -237.0, -173.0, -63.0, -63.0, 63.0, 63.0, 202.0, 248.0, 289.0, 346.0, 387.0, 422.0, 445.0, 474.0, 474.0, 462.0, 508.0, 514.0, 503.0, 497.0, 485.0, 439.0, 404.0, 370.0, 306.0, 265.0, 202.0, 167.0, 75.0, 17.0, -40.0, -80.0, -150.0, -202.0, -260.0, -300.0, -341.0, -364.0, -375.0, -393.0, -398.0, -387.0, -387.0, -364.0, -335.0, -323.0, -306.0, -277.0, -213.0, -190.0, -150.0, -80.0, -17.0, 57.0, 104.0, 138.0, 190.0, 242.0, 300.0, 318.0, 364.0, 404.0, 427.0, 451.0, 503.0, 514.0, 514.0, 526.0, 508.0, 491.0, 451.0, 393.0, 346.0, 260.0, 185.0, 115.0, 57.0, -23.0, -69.0, -115.0, -161.0, -190.0, -225.0, -248.0, -254.0, -242.0, -254.0, -237.0, -237.0, -179.0, -179.0, -127.0, -150.0, -179.0, -179.0, -150.0, -127.0, -80.0, -57.0, -5.0, 40.0, 86.0, 92.0, 156.0, 208.0, 271.0, 306.0, 346.0, 375.0, 410.0, 427.0, 451.0, 456.0, 474.0, 485.0, 485.0, 474.0, 485.0, 479.0, 445.0, 427.0, 410.0, 387.0, 352.0, 329.0, 329.0, 329.0, 329.0, 173.0, 115.0, 63.0, 28.0, 5.0, -23.0, -34.0, -46.0, -52.0, -34.0, -17.0, -28.0, 0.0, 23.0, 40.0, 69.0, 86.0, 115.0, 138.0, 156.0, 167.0, 185.0, 196.0, 231.0, 237.0, 277.0, 289.0, 335.0, 329.0, 352.0, 346.0, 364.0, 370.0, 358.0, 323.0, 300.0, 277.0, 254.0, 242.0, 202.0, 156.0, 121.0, 86.0, 34.0, 23.0, 34.0, 34.0, 46.0, 92.0, 132.0, 167.0, 190.0, 254.0, 254.0, 283.0, 294.0, 300.0, 306.0, 306.0, 283.0, 306.0, 329.0, 323.0, 318.0, 318.0, 294.0, 312.0, 300.0, 323.0, 277.0, 283.0, 254.0, 254.0, 219.0, 213.0, 185.0, 179.0, 179.0, 179.0, 179.0, 138.0, 98.0, 98.0, 115.0, 133.0, 161.0, 179.0, 196.0, 208.0, 237.0, 248.0, 277.0, 289.0, 329.0, 335.0, 346.0, 352.0, 329.0, 346.0, 352.0, 393.0, 381.0, 312.0, 306.0, 294.0, 254.0, 254.0, 248.0, 219.0, 179.0, 150.0, 109.0, 63.0, 28.0, 0.0, 11.0, 23.0, 34.0, 57.0, 80.0, 86.0, 92.0, 115.0, 121.0, 144.0, 167.0, 179.0, 196.0, 219.0, 242.0, 260.0, 277.0, 294.0, 306.0, 329.0, 341.0, 358.0, 381.0, 398.0, 410.0, 427.0, 451.0, 456.0, 479.0, 497.0, 503.0, 537.0, 531.0, 555.0, 555.0, 566.0, 595.0, 584.0, 612.0, 607.0, 578.0, 601.0, 584.0, 555.0, 537.0, 491.0, 433.0, 352.0, 312.0, 265.0, 202.0, 156.0, 121.0, 98.0, 63.0, 46.0, 23.0, -5.0, -28.0, -63.0, -92.0, -80.0, -104.0, -98.0, -86.0, -75.0, -46.0, -28.0, 0.0, 40.0, 98.0, 98.0, 185.0, 185.0, 185.0, 300.0, 364.0, 433.0, 491.0, 543.0, 566.0, 636.0, 670.0, 711.0, 757.0, 769.0, 809.0, 815.0, 826.0, 832.0, 809.0, 803.0, 780.0, 745.0, 693.0, 670.0, 624.0, 589.0, 572.0, 526.0, 451.0, 381.0, 341.0, 289.0, 242.0, 167.0, 121.0, 80.0, 28.0, -28.0, -63.0, -104.0, -144.0, -156.0, -173.0, -202.0, -219.0, -225.0, -213.0, -202.0, -179.0, -132.0, -104.0, -63.0, -40.0, 0.0, 57.0, 98.0, 144.0, 173.0, 231.0, 283.0, 329.0, 352.0, 370.0, 398.0, 427.0, 462.0, 508.0, 520.0, 526.0, 549.0, 572.0, 578.0, 601.0, 595.0, 607.0, 607.0, 618.0, 636.0, 624.0, 612.0, 589.0, 584.0, 578.0, 537.0, 531.0, 514.0, 526.0, 503.0, 445.0, 410.0, 381.0, 375.0, 335.0, 265.0, 260.0, 248.0, 213.0, 196.0, 173.0, 167.0, 185.0, 190.0, 185.0, 173.0, 190.0, 173.0, 173.0, 161.0, 167.0, 156.0, 190.0, 185.0, 173.0, 173.0, 173.0, 190.0, 196.0, 208.0, 231.0, 242.0, 248.0, 260.0, 242.0, 231.0, 213.0, 208.0, 196.0, 190.0, 202.0, 213.0, 202.0, 196.0, 202.0, 196.0, 185.0, 190.0, 196.0, 202.0, 208.0, 196.0, 190.0, 196.0, 196.0, 179.0, 196.0, 190.0, 167.0, 150.0, 150.0, 127.0, 127.0, 127.0, 127.0, 127.0, 127.0, 92.0, 98.0, 127.0, 127.0, 156.0, 179.0, 213.0, 248.0, 271.0, 300.0, 341.0, 346.0, 358.0, 387.0, 404.0, 410.0, 410.0, 422.0, 416.0, 427.0, 410.0, 404.0, 375.0, 358.0, 335.0, 318.0, 277.0, 248.0, 208.0, 173.0, 132.0, 69.0, 28.0, 11.0, -23.0, -34.0, -57.0, -57.0, -57.0, -52.0, -28.0, -17.0, 0.0, 11.0, 69.0, 86.0, 138.0, 185.0, 237.0, 283.0, 318.0, 358.0, 387.0, 387.0, 398.0, 393.0, 416.0, 398.0, 393.0, 404.0, 393.0, 375.0, 364.0, 329.0, 318.0, 271.0, 260.0, 219.0, 185.0, 156.0, 150.0, 121.0, 86.0, 80.0, 40.0, 23.0, 17.0, 0.0, -40.0, -40.0, -63.0, -57.0, -75.0, -115.0, -115.0, -115.0, -115.0, -115.0, -92.0, -75.0, -63.0, -34.0, -28.0, 11.0, 28.0, 52.0, 115.0, 150.0, 173.0, 202.0, 202.0, 237.0, 237.0, 231.0, 213.0, 219.0, 167.0, 138.0, 98.0, 34.0, -17.0, -34.0, -75.0, -75.0, -75.0, -202.0, -254.0, -318.0, -335.0, -346.0, -335.0, -341.0, -341.0, -329.0, -300.0, -294.0, -318.0, -318.0, -318.0, -277.0, -254.0, -225.0, -213.0, -213.0, -213.0, -185.0, -156.0, -121.0, -104.0, -115.0, -80.0, -109.0, -98.0, -92.0, -104.0, -75.0, -75.0, -63.0, -46.0, -5.0, 0.0, -17.0, -17.0, -17.0, -11.0, -11.0, -17.0, -17.0, 11.0, -5.0, -11.0, -11.0, -11.0, 0.0, -11.0, -11.0, -28.0, -63.0, -86.0, -132.0, -179.0, -242.0, -248.0, -277.0, -300.0, -300.0, -300.0, -318.0, -277.0, -260.0, -225.0, -156.0, -115.0, -69.0, 5.0, 69.0, 121.0, 144.0, 185.0, 231.0, 231.0, 271.0, 277.0, 294.0, 289.0, 294.0, 271.0, 265.0, 260.0, 248.0, 213.0, 167.0, 127.0, 63.0, 17.0, -17.0, -69.0, -115.0, -161.0, -208.0, -254.0, -329.0, -335.0, -370.0, -381.0, -398.0, -422.0, -416.0, -398.0, -398.0, -404.0, -364.0, -335.0, -318.0, -260.0, -237.0, -237.0, -173.0, -63.0, -63.0, 63.0, 63.0, 202.0, 248.0, 289.0, 346.0, 387.0, 422.0, 445.0, 474.0, 474.0, 462.0, 508.0, 514.0, 503.0, 497.0, 485.0, 439.0, 404.0, 370.0, 306.0, 265.0, 202.0, 167.0, 75.0, 17.0, -40.0, -80.0, -150.0, -202.0, -260.0, -300.0, -341.0, -364.0, -375.0, -393.0, -398.0, -387.0, -387.0, -364.0, -335.0, -323.0, -306.0, -277.0, -213.0, -190.0, -150.0, -80.0, -17.0, 57.0, 104.0, 138.0, 190.0, 242.0, 300.0, 318.0, 364.0, 404.0, 427.0, 451.0, 503.0, 514.0, 514.0, 526.0, 508.0, 491.0, 451.0, 393.0, 346.0, 260.0, 185.0, 115.0, 57.0, -23.0, -69.0, -115.0, -161.0, -190.0, -225.0, -248.0, -254.0, -242.0, -254.0, -237.0, -237.0, -179.0, -179.0, -127.0, -150.0, -179.0, -179.0, -150.0, -127.0, -80.0, -57.0, -5.0, 40.0, 86.0, 92.0, 156.0, 208.0, 271.0, 306.0, 346.0, 375.0, 410.0, 427.0, 451.0, 456.0, 474.0, 485.0, 485.0, 474.0, 485.0, 479.0, 445.0, 427.0, 410.0, 387.0, 352.0, 329.0, 329.0, 329.0, 329.0, 173.0, 115.0, 63.0, 28.0, 5.0, -23.0, -34.0, -46.0, -52.0, -34.0, -17.0, -28.0, 0.0, 23.0, 40.0, 69.0, 86.0, 115.0, 138.0, 156.0, 167.0, 185.0, 196.0, 231.0, 237.0, 277.0, 289.0, 335.0, 329.0, 352.0, 346.0, 364.0, 370.0, 358.0, 323.0, 300.0, 277.0, 254.0, 242.0, 202.0, 156.0, 121.0, 86.0, 34.0, 23.0, 34.0, 34.0, 46.0, 92.0, 132.0, 167.0, 190.0, 254.0, 254.0, 283.0, 294.0, 300.0, 306.0, 306.0, 283.0, 306.0, 329.0, 323.0, 318.0, 318.0, 294.0, 312.0, 300.0, 323.0, 277.0, 283.0, 254.0, 254.0, 219.0, 213.0, 185.0, 179.0, 179.0, 179.0, 179.0, 138.0, 98.0, 98.0, 115.0, 133.0, 161.0, 179.0, 196.0, 208.0, 237.0, 248.0, 277.0, 289.0, 329.0, 335.0, 346.0, 352.0, 329.0, 346.0, 352.0, 393.0, 381.0, 312.0, 306.0, 294.0, 254.0, 254.0, 248.0, 219.0, 179.0, 150.0, 109.0, 63.0, 28.0, 0.0, 11.0, 23.0, 34.0, 57.0, 80.0, 86.0, 92.0, 115.0, 121.0, 144.0, 167.0, 179.0, 196.0, 219.0, 242.0, 260.0, 277.0, 294.0, 306.0, 329.0, 341.0, 358.0, 381.0, 398.0, 410.0, 427.0, 451.0, 456.0, 479.0, 497.0, 503.0, 537.0, 531.0, 555.0, 555.0, 566.0, 595.0, 584.0, 612.0, 607.0, 578.0, 601.0, 584.0, 555.0, 537.0, 491.0, 433.0, 352.0, 312.0, 265.0, 202.0, 156.0, 121.0, 98.0, 63.0, 46.0, 23.0, -5.0, -28.0, -63.0, -92.0, -80.0, -104.0, -98.0, -86.0, -75.0, -46.0, -28.0, 0.0, 40.0, 98.0, 98.0, 185.0, 185.0, 185.0, 300.0, 364.0, 433.0, 491.0, 543.0, 566.0, 636.0, 670.0, 711.0, 757.0, 769.0, 809.0, 815.0, 826.0, 832.0, 809.0, 803.0, 780.0, 745.0, 693.0, 670.0, 624.0, 589.0, 572.0, 526.0, 451.0, 381.0, 341.0, 289.0, 242.0, 167.0, 121.0, 80.0, 28.0, -28.0, -63.0, -104.0, -144.0, -156.0, -173.0, -202.0, -219.0, -225.0, -213.0, -202.0, -179.0, -132.0, -104.0, -63.0, -40.0, 0.0, 57.0, 98.0, 144.0, 173.0, 231.0, 283.0, 329.0, 352.0, 370.0, 398.0, 427.0, 462.0, 508.0, 520.0, 526.0, 549.0, 572.0, 578.0, 601.0, 595.0, 607.0, 607.0, 618.0, 636.0, 624.0, 612.0, 589.0, 584.0, 578.0, 537.0, 531.0, 514.0, 526.0, 503.0, 445.0, 410.0, 381.0, 375.0, 335.0, 265.0, 260.0, 248.0, 213.0, 196.0, 173.0, 167.0, 185.0, 190.0, 185.0, 173.0, 190.0, 173.0, 173.0, 161.0, 167.0, 156.0, 190.0, 185.0, 173.0, 173.0, 173.0, 190.0, 196.0, 208.0, 231.0, 242.0, 248.0, 260.0, 242.0, 231.0, 213.0, 208.0, 196.0, 190.0, 202.0, 213.0, 202.0, 196.0, 202.0, 196.0, 185.0, 190.0, 196.0, 202.0, 208.0, 196.0, 190.0
};

double z[MAX_DATA_LEN] = {
-759.0, -765.0, -778.0, -753.0, -765.0, -759.0, -765.0, -772.0, -759.0, -759.0, -759.0, -759.0, -759.0, -778.0, -778.0, -791.0, -797.0, -816.0, -810.0, -810.0, -785.0, -791.0, -772.0, -784.0, -778.0, -765.0, -733.0, -714.0, -714.0, -682.0, -657.0, -638.0, -612.0, -599.0, -599.0, -593.0, -568.0, -561.0, -574.0, -574.0, -555.0, -561.0, -568.0, -580.0, -587.0, -606.0, -625.0, -625.0, -638.0, -657.0, -638.0, -682.0, -714.0, -714.0, -740.0, -746.0, -772.0, -778.0, -778.0, -784.0, -778.0, -778.0, -804.0, -797.0, -785.0, -772.0, -791.0, -772.0, -753.0, -772.0, -765.0, -702.0, -721.0, -721.0, -708.0, -682.0, -676.0, -670.0, -657.0, -663.0, -631.0, -612.0, -619.0, -612.0, -631.0, -619.0, -632.0, -632.0, -638.0, -638.0, -676.0, -676.0, -695.0, -702.0, -714.0, -740.0, -727.0, -727.0, -740.0, -727.0, -734.0, -733.0, -746.0, -753.0, -772.0, -772.0, -772.0, -753.0, -766.0, -740.0, -778.0, -772.0, -766.0, -778.0, -772.0, -785.0, -804.0, -810.0, -810.0, -797.0, -804.0, -823.0, -797.0, -791.0, -778.0, -766.0, -759.0, -759.0, -759.0, -682.0, -683.0, -657.0, -631.0, -625.0, -619.0, -612.0, -625.0, -631.0, -644.0, -638.0, -644.0, -644.0, -651.0, -663.0, -689.0, -702.0, -695.0, -715.0, -702.0, -664.0, -695.0, -702.0, -689.0, -714.0, -708.0, -714.0, -708.0, -689.0, -721.0, -721.0, -727.0, -721.0, -740.0, -753.0, -734.0, -753.0, -753.0, -753.0, -753.0, -766.0, -753.0, -772.0, -759.0, -766.0, -772.0, -778.0, -759.0, -772.0, -785.0, -766.0, -778.0, -773.0, -747.0, -734.0, -740.0, -702.0, -676.0, -670.0, -657.0, -632.0, -638.0, -645.0, -632.0, -676.0, -702.0, -708.0, -753.0, -734.0, -772.0, -798.0, -778.0, -785.0, -804.0, -791.0, -785.0, -791.0, -785.0, -785.0, -785.0, -772.0, -791.0, -766.0, -759.0, -740.0, -740.0, -740.0, -753.0, -772.0, -766.0, -766.0, -753.0, -740.0, -727.0, -695.0, -657.0, -632.0, -600.0, -561.0, -523.0, -510.0, -491.0, -472.0, -459.0, -459.0, -440.0, -447.0, -472.0, -466.0, -472.0, -472.0, -485.0, -523.0, -523.0, -587.0, -587.0, -619.0, -632.0, -644.0, -664.0, -638.0, -695.0, -702.0, -727.0, -727.0, -753.0, -753.0, -753.0, -753.0, -747.0, -766.0, -772.0, -766.0, -779.0, -772.0, -785.0, -772.0, -779.0, -740.0, -747.0, -734.0, -721.0, -734.0, -683.0, -670.0, -657.0, -587.0, -606.0, -581.0, -600.0, -581.0, -574.0, -606.0, -606.0, -606.0, -613.0, -632.0, -632.0, -644.0, -683.0, -657.0, -676.0, -702.0, -715.0, -714.0, -727.0, -727.0, -715.0, -715.0, -708.0, -708.0, -734.0, -696.0, -715.0, -715.0, -728.0, -728.0, -747.0, -747.0, -747.0, -759.0, -779.0, -791.0, -772.0, -798.0, -791.0, -804.0, -747.0, -734.0, -747.0, -683.0, -677.0, -651.0, -587.0, -600.0, -587.0, -555.0, -555.0, -530.0, -523.0, -491.0, -498.0, -459.0, -440.0, -453.0, -466.0, -466.0, -479.0, -510.0, -536.0, -549.0, -574.0, -600.0, -625.0, -657.0, -651.0, -683.0, -696.0, -702.0, -728.0, -734.0, -740.0, -747.0, -753.0, -740.0, -759.0, -779.0, -772.0, -727.0, -747.0, -753.0, -753.0, -753.0, -753.0, -779.0, -778.0, -766.0, -727.0, -766.0, -740.0, -715.0, -715.0, -695.0, -664.0, -587.0, -606.0, -593.0, -574.0, -549.0, -549.0, -523.0, -498.0, -504.0, -510.0, -478.0, -472.0, -498.0, -498.0, -498.0, -491.0, -536.0, -536.0, -555.0, -600.0, -619.0, -632.0, -664.0, -676.0, -696.0, -702.0, -734.0, -734.0, -753.0, -759.0, -759.0, -759.0, -759.0, -766.0, -759.0, -760.0, -779.0, -753.0, -766.0, -766.0, -766.0, -766.0, -747.0, -747.0, -753.0, -740.0, -728.0, -721.0, -721.0, -708.0, -645.0, -664.0, -651.0, -638.0, -594.0, -574.0, -542.0, -530.0, -530.0, -504.0, -478.0, -453.0, -479.0, -498.0, -498.0, -511.0, -542.0, -536.0, -561.0, -581.0, -593.0, -625.0, -632.0, -657.0, -638.0, -702.0, -721.0, -760.0, -766.0, -766.0, -772.0, -753.0, -721.0, -772.0, -792.0, -772.0, -734.0, -766.0, -740.0, -721.0, -702.0, -683.0, -670.0, -651.0, -632.0, -625.0, -625.0, -606.0, -619.0, -593.0, -600.0, -562.0, -562.0, -555.0, -555.0, -555.0, -536.0, -555.0, -542.0, -536.0, -530.0, -530.0, -536.0, -536.0, -517.0, -485.0, -466.0, -459.0, -440.0, -434.0, -459.0, -466.0, -485.0, -504.0, -536.0, -562.0, -581.0, -613.0, -664.0, -689.0, -702.0, -721.0, -740.0, -760.0, -760.0, -779.0, -740.0, -766.0, -766.0, -760.0, -760.0, -766.0, -728.0, -709.0, -651.0, -683.0, -651.0, -632.0, -600.0, -574.0, -549.0, -549.0, -466.0, -504.0, -491.0, -479.0, -478.0, -479.0, -479.0, -510.0, -498.0, -536.0, -517.0, -562.0, -574.0, -600.0, -600.0, -638.0, -638.0, -638.0, -670.0, -664.0, -664.0, -670.0, -677.0, -683.0, -683.0, -677.0, -709.0, -683.0, -670.0, -696.0, -683.0, -696.0, -702.0, -689.0, -696.0, -721.0, -715.0, -721.0, -734.0, -734.0, -715.0, -728.0, -728.0, -760.0, -766.0, -753.0, -747.0, -772.0, -779.0, -785.0, -792.0, -779.0, -753.0, -760.0, -747.0, -728.0, -702.0, -645.0, -619.0, -575.0, -574.0, -555.0, -549.0, -523.0, -504.0, -536.0, -568.0, -581.0, -581.0, -581.0, -581.0, -555.0, -536.0, -517.0, -504.0, -459.0, -440.0, -440.0, -415.0, -440.0, -459.0, -479.0, -498.0, -517.0, -542.0, -562.0, -536.0, -581.0, -613.0, -619.0, -657.0, -664.0, -689.0, -709.0, -734.0, -734.0, -734.0, -747.0, -760.0, -753.0, -740.0, -747.0, -721.0, -696.0, -696.0, -702.0, -657.0, -651.0, -651.0, -651.0, -651.0, -657.0, -638.0, -658.0, -683.0, -670.0, -696.0, -683.0, -690.0, -689.0, -670.0, -689.0, -683.0, -677.0, -709.0, -689.0, -677.0, -670.0, -670.0, -638.0, -626.0, -632.0, -619.0, -600.0, -613.0, -638.0, -645.0, -645.0, -664.0, -747.0, -754.0, -773.0, -811.0, -804.0, -747.0, -779.0, -760.0, -766.0, -760.0, -740.0, -754.0, -753.0, -766.0, -753.0, -779.0, -785.0, -804.0, -798.0, -792.0, -792.0, -811.0, -792.0, -811.0, -792.0, -734.0, -792.0, -766.0, -741.0, -722.0, -690.0, -664.0, 759.0, 765.0, 778.0, 753.0, 765.0, 759.0, 765.0, 772.0, 759.0, 759.0, 759.0, 759.0, 759.0, 778.0, 778.0, 791.0, 797.0, 816.0, 810.0, 810.0, 785.0, 791.0, 772.0, 784.0, 778.0, 765.0, 733.0, 714.0, 714.0, 682.0, 657.0, 638.0, 612.0, 599.0, 599.0, 593.0, 568.0, 561.0, 574.0, 574.0, 555.0, 561.0, 568.0, 580.0, 587.0, 606.0, 625.0, 625.0, 638.0, 657.0, 638.0, 682.0, 714.0, 714.0, 740.0, 746.0, 772.0, 778.0, 778.0, 784.0, 778.0, 778.0, 804.0, 797.0, 785.0, 772.0, 791.0, 772.0, 753.0, 772.0, 765.0, 702.0, 721.0, 721.0, 708.0, 682.0, 676.0, 670.0, 657.0, 663.0, 631.0, 612.0, 619.0, 612.0, 631.0, 619.0, 632.0, 632.0, 638.0, 638.0, 676.0, 676.0, 695.0, 702.0, 714.0, 740.0, 727.0, 727.0, 740.0, 727.0, 734.0, 733.0, 746.0, 753.0, 772.0, 772.0, 772.0, 753.0, 766.0, 740.0, 778.0, 772.0, 766.0, 778.0, 772.0, 785.0, 804.0, 810.0, 810.0, 797.0, 804.0, 823.0, 797.0, 791.0, 778.0, 766.0, 759.0, 759.0, 759.0, 682.0, 683.0, 657.0, 631.0, 625.0, 619.0, 612.0, 625.0, 631.0, 644.0, 638.0, 644.0, 644.0, 651.0, 663.0, 689.0, 702.0, 695.0, 715.0, 702.0, 664.0, 695.0, 702.0, 689.0, 714.0, 708.0, 714.0, 708.0, 689.0, 721.0, 721.0, 727.0, 721.0, 740.0, 753.0, 734.0, 753.0, 753.0, 753.0, 753.0, 766.0, 753.0, 772.0, 759.0, 766.0, 772.0, 778.0, 759.0, 772.0, 785.0, 766.0, 778.0, 773.0, 747.0, 734.0, 740.0, 702.0, 676.0, 670.0, 657.0, 632.0, 638.0, 645.0, 632.0, 676.0, 702.0, 708.0, 753.0, 734.0, 772.0, 798.0, 778.0, 785.0, 804.0, 791.0, 785.0, 791.0, 785.0, 785.0, 785.0, 772.0, 791.0, 766.0, 759.0, 740.0, 740.0, 740.0, 753.0, 772.0, 766.0, 766.0, 753.0, 740.0, 727.0, 695.0, 657.0, 632.0, 600.0, 561.0, 523.0, 510.0, 491.0, 472.0, 459.0, 459.0, 440.0, 447.0, 472.0, 466.0, 472.0, 472.0, 485.0, 523.0, 523.0, 587.0, 587.0, 619.0, 632.0, 644.0, 664.0, 638.0, 695.0, 702.0, 727.0, 727.0, 753.0, 753.0, 753.0, 753.0, 747.0, 766.0, 772.0, 766.0, 779.0, 772.0, 785.0, 772.0, 779.0, 740.0, 747.0, 734.0, 721.0, 734.0, 683.0, 670.0, 657.0, 587.0, 606.0, 581.0, 600.0, 581.0, 574.0, 606.0, 606.0, 606.0, 613.0, 632.0, 632.0, 644.0, 683.0, 657.0, 676.0, 702.0, 715.0, 714.0, 727.0, 727.0, 715.0, 715.0, 708.0, 708.0, 734.0, 696.0, 715.0, 715.0, 728.0, 728.0, 747.0, 747.0, 747.0, 759.0, 779.0, 791.0, 772.0, 798.0, 791.0, 804.0, 747.0, 734.0, 747.0, 683.0, 677.0, 651.0, 587.0, 600.0, 587.0, 555.0, 555.0, 530.0, 523.0, 491.0, 498.0, 459.0, 440.0, 453.0, 466.0, 466.0, 479.0, 510.0, 536.0, 549.0, 574.0, 600.0, 625.0, 657.0, 651.0, 683.0, 696.0, 702.0, 728.0, 734.0, 740.0, 747.0, 753.0, 740.0, 759.0, 779.0, 772.0, 727.0, 747.0, 753.0, 753.0, 753.0, 753.0, 779.0, 778.0, 766.0, 727.0, 766.0, 740.0, 715.0, 715.0, 695.0, 664.0, 587.0, 606.0, 593.0, 574.0, 549.0, 549.0, 523.0, 498.0, 504.0, 510.0, 478.0, 472.0, 498.0, 498.0, 498.0, 491.0, 536.0, 536.0, 555.0, 600.0, 619.0, 632.0, 664.0, 676.0, 696.0, 702.0, 734.0, 734.0, 753.0, 759.0, 759.0, 759.0, 759.0, 766.0, 759.0, 760.0, 779.0, 753.0, 766.0, 766.0, 766.0, 766.0, 747.0, 747.0, 753.0, 740.0, 728.0, 721.0, 721.0, 708.0, 645.0, 664.0, 651.0, 638.0, 594.0, 574.0, 542.0, 530.0, 530.0, 504.0, 478.0, 453.0, 479.0, 498.0, 498.0, 511.0, 542.0, 536.0, 561.0, 581.0, 593.0, 625.0, 632.0, 657.0, 638.0, 702.0, 721.0, 760.0, 766.0, 766.0, 772.0, 753.0, 721.0, 772.0, 792.0, 772.0, 734.0, 766.0, 740.0, 721.0, 702.0, 683.0, 670.0, 651.0, 632.0, 625.0, 625.0, 606.0, 619.0, 593.0, 600.0, 562.0, 562.0, 555.0, 555.0, 555.0, 536.0, 555.0, 542.0, 536.0, 530.0, 530.0, 536.0, 536.0, 517.0, 485.0, 466.0, 459.0, 440.0, 434.0, 459.0, 466.0, 485.0, 504.0, 536.0, 562.0, 581.0, 613.0, 664.0, 689.0, 702.0, 721.0, 740.0, 760.0, 760.0, 779.0, 740.0, 766.0, 766.0, 760.0, 760.0, 766.0, 728.0, 709.0, 651.0, 683.0, 651.0, 632.0, 600.0, 574.0, 549.0, 549.0, 466.0, 504.0, 491.0, 479.0, 478.0, 479.0, 479.0, 510.0, 498.0, 536.0, 517.0, 562.0, 574.0, 600.0, 600.0, 638.0, 638.0, 638.0, 670.0, 664.0, 664.0, 670.0, 677.0, 683.0, 683.0, 677.0, 709.0, 683.0, 670.0, 696.0, 683.0, 696.0, 702.0, 689.0, 696.0, 721.0, 715.0, 721.0, 734.0, 734.0, 715.0, 728.0, 728.0, 760.0, 766.0, 753.0, 747.0, 772.0, 779.0, 785.0, 792.0, 779.0, 753.0, 760.0, 747.0, 728.0, 702.0, 645.0, 619.0, 575.0, 574.0, 555.0, 549.0, 523.0, 504.0, 536.0, 568.0, 581.0, 581.0, 581.0, 581.0, 555.0, 536.0, 517.0, 504.0, 459.0, 440.0, 440.0, 415.0, 440.0, 459.0, 479.0, 498.0, 517.0, 542.0, 562.0, 536.0, 581.0, 613.0, 619.0, 657.0, 664.0, 689.0, 709.0, 734.0, 734.0, 734.0, 747.0, 760.0, 753.0, 740.0, 747.0, 721.0, 696.0, 696.0, 702.0, 657.0, 651.0, 651.0, 651.0, 651.0, 657.0, 638.0, 658.0, 683.0, 670.0, 696.0, 683.0, 690.0, 689.0, 670.0, 689.0, 683.0, 677.0, 709.0, 689.0, 677.0, 670.0, 670.0, 638.0, 626.0, 632.0, 619.0, 600.0, 613.0, 638.0, 645.0, 645.0, 664.0, 747.0, 754.0, 773.0, 811.0, 804.0, 747.0, 779.0, 760.0, 766.0, 760.0, 740.0, 754.0, 753.0, 766.0, 753.0, 779.0, 785.0, 804.0, 798.0, 792.0, 792.0, 811.0, 792.0, 811.0, 792.0, 734.0, 792.0, 766.0, 741.0, 722.0, 690.0, 664.0
};

#else
    #error "DATA_MODE must be 1.0, 2, 3 or 4!"
#endif

void setup() {
    Serial.begin(115200);
    
    delay(100);
    
    Serial.println("Starting calibration process...");
    
#ifdef MAGNETO_1
    Serial.println("Using Magneto 1.4 ellipsoid fit calibration method");
    
    if (magneto_mag == NULL) {
        Serial.println("ERROR: Failed to create MagnetoCalibration object");
        return;
    }
    
    // Adding all samples to Magneto calibration
    Serial.print("Adding ");
    Serial.print(MAX_DATA_LEN);
    Serial.println(" samples to Magneto calibration...");
    
    for (int i = 0; i < MAX_DATA_LEN; i++) {

        #if DATA_MODE == 4
        // Without integer substitution division, magneto returns NaN
        magneto_mag->sample(x[i] / 1000.0, y[i] / 1000.0, z[i] / 1000.0);
        #else
        // RAW data - passing each sample to Magneto
        magneto_mag->sample(x[i], y[i], z[i]);
        #endif
        
        if ((i + 1) % 5 == 0 || i == MAX_DATA_LEN - 1) {
            Serial.print("Processed ");
            Serial.print(i + 1);
            Serial.println(" samples");
        }
        
        vTaskDelay(pdMS_TO_TICKS(2));
    }
    
    Serial.println("Magneto calibration completed");

    magneto_mag->current_calibration(M_BAinv);

    Serial.println("Magnetometer calibration matrix:");
    Serial.println("{");

    for (int i = 0; i < 3; i++) {
                
        sprintf(msgString, "  %f, %f, %f, %f",
            #if DATA_MODE == 4
            M_BAinv[0][i] * 1000.0,
            #else
            M_BAinv[0][i],
            #endif
            M_BAinv[1][i],
            M_BAinv[2][i],
            M_BAinv[3][i]
        );

        Serial.print(msgString);
        Serial.println();

    }

    Serial.println("}");

    vTaskDelay(pdMS_TO_TICKS(20));

    Serial.println("Magneto STOP");

    vTaskDelay(pdMS_TO_TICKS(20));
    
    delete magneto_mag;
    
#else // MAGNETO_2 - using standard calibration
    
    Serial.println("Using standard geometry.h calibration method");
    
    // Create vectors based on data arrays
    // We allocate memory on the heap, since the library will try to free() the passed pointer when calling vec_free()
    // and static arrays cannot be freed.
    double* x_dyn = (double*)malloc(MAX_DATA_LEN * sizeof(double));
    double* y_dyn = (double*)malloc(MAX_DATA_LEN * sizeof(double));
    double* z_dyn = (double*)malloc(MAX_DATA_LEN * sizeof(double));

    if (x_dyn == NULL || y_dyn == NULL || z_dyn == NULL) {
        Serial.println("ERROR: Failed to allocate memory for arrays");
        free(x_dyn); free(y_dyn); free(z_dyn);
        return;
    }

    memcpy(x_dyn, x, MAX_DATA_LEN * sizeof(double));
    memcpy(y_dyn, y, MAX_DATA_LEN * sizeof(double));
    memcpy(z_dyn, z, MAX_DATA_LEN * sizeof(double));

    Vector vx = vec_from_array(x_dyn, MAX_DATA_LEN);
    Vector vy = vec_from_array(y_dyn, MAX_DATA_LEN);
    Vector vz = vec_from_array(z_dyn, MAX_DATA_LEN);
    
    if (vx == NULL || vy == NULL || vz == NULL) {
        Serial.println("ERROR: Failed to create vectors");
        if (vx != NULL) vec_free(vx);
        if (vy != NULL) vec_free(vy);
        if (vz != NULL) vec_free(vz);
        // Clearing temporary buffers in case of vector creation error
        free(x_dyn); free(y_dyn); free(z_dyn);
        return;
    }
    
    // Calculate calibration data (offset vector and transformation matrix)
    Callibration_t calib = calib_calibrate_sensor(vx, vy, vz);
    
    // Check if calibration was successful
    Serial.print("Calculating calibration data of data points was ");
    if (calib_calibration_success(calib)) {
        Serial.println("SUCCESSFUL");
    } else {
        Serial.println("UNSUCCESSFUL");
        calib_free(calib);
        vec_free(vx);
        vec_free(vy);
        vec_free(vz);
        return;
    }
    
    // Print variance before calibration
    Serial.print("Variance of distances before calibration: ");
    Serial.println(square_distance_variance(vx, vy, vz), 6);
    
    // Calibrate single point
    Vector dataPoint = vec_new(3);
    if (dataPoint != NULL) {
        VEC_X(dataPoint) = VEC_ELEM(vx, 0);
        VEC_Y(dataPoint) = VEC_ELEM(vy, 0);
        VEC_Z(dataPoint) = VEC_ELEM(vz, 0);
        calib_calibrate_point(calib, dataPoint);
        
        // Vector gravity = vec_new(3);
        // if (gravity != NULL) {
        //     VEC_X(gravity) = 0;
        //     VEC_Y(gravity) = 0;
        //     VEC_Z(gravity) = 1;
        //     wmm_compensate(dataPoint, gravity, 5.9, 68.2);
        //     vec_free(gravity);
        // }
        // vec_free(dataPoint);
    }
    
    // Calibrate all data points
    calib_calibrate_multiple_points(calib, vx, vy, vz);
    
    // Print variance after calibration
    Serial.print("Variance of distances after calibration: ");
    Serial.println(square_distance_variance(vx, vy, vz), 6);

    // Print the result
    vec_print(calib.offset);
    mat_print(calib.transform);
    
    // Free allocated memory
    calib_free(calib);
    vec_free(vx);
    vec_free(vy);
    vec_free(vz);
    
#endif // MAGNETO_1 / MAGNETO_2
    
    Serial.println("=== Calibration completed ===");
}

void loop() {
    vTaskDelay(pdMS_TO_TICKS(1000));
}

I would like to hear your opinion regarding my reasoning and simplification of calibration for the purpose of collecting data for devices with limited spatial movement.

[michal34512]

Hi there! 👋

1. No, it does not make a difference whether you use NED or ENU. For example, if you feed NED data to the algorithm, you will get calibrated vectors in the NED coordinate system. But remember, magnetometer calibration is not sufficient to get a vector pointing at Geographic North. You need to compensate for that using the World Magnetic Model (WMM). Check out the feature-wmm branch for this.

2. Using double or float doesn't matter. After calibration, you will get unit vectors pointing in the direction of the local magnetic field (which, again, is not the same as Geographic North—see the feature-wmm branch).

3. Calibrating in a limited space will still work with this algorithm, just not as well as it does with a full sphere. Try implementing the methodology for assessing sample quality that I wrote about earlier, and you should be fine. The main limitation of this algorithm is that if you, for example, only rotate around the Z-axis (because you only want yaw) and gather samples, there would be an infinite number of possible 3D ellipsoids that could fit that 2D dataset. In that case, you should actually be looking for the best-fitting 2D ellipse. There are articles on how to handle this situation, but it's quite hard to implement, and WMM compensation becomes problematic later. I might try to add this feature one day, though!

4. I know about the dividing by two method ((max + min) / 2). We used to use it on a sounding rocket (HEXA5). It is fine as long as you don't have outliers, but you need to gather a lot of data across the full range of motion because you rely heavily on finding the true maximums and minimums.

5. Regarding the Z-axis scale ("If I point ... it shows +-15 uT"): Again, the problem is that you are not compensating for magnetic inclination and declination.

The Earth's magnetic field is tilted depending on your geographic location, and you have to compensate for that mathematically (this is what the feature-wmm branch is for). That's why you get a deformed, flat ellipse instead of a circle when rotating perfectly flat around the Z-axis.
(https://en.wikipedia.org/wiki/Magnetic_dip, https://en.wikipedia.org/wiki/Magnetic_declination)

6. "So I thought, why not collect data from the lower hemisphere (or upper) and mirror the values along the Z-axis?" The algorithm is prepared to receive data from just the lower hemisphere. It will just have fewer important samples to work with. If you mirror the data, it won't actually improve the mathematical error. Let's say your true ellipsoid is huge, and you just get samples at the bottom of it. Your samples would be basically flat. It is hard for the algorithm to fit the best fitting ellipsoid because both a huge ellipsoid and a "much more huge" ellipsoid could fit those bottom samples quite well. Mirroring just artificially forces symmetry without providing real data about the environment.