Reefwing AHRS - yaw not changing

[Aleperry]

Hi there!
I've spent a lot of time trying to use the Reefwing AHRS library successfully on my Nano 33 BLE sense rev2 but I'm losing hope: I'm using the example code that comes with the library with the addition of some lines for magnetometer calibration with Magneto.
The magnetometer is calibrated (almost perfect sphere centered on the origin).
The roll and pitch values are correct and consistent, but the yaw (and heading) do not change at all when the board rotates around its z axis. The only way I can obtain a change is when the board tilts around.
I am thinking that this has something to do with the different orientation of the sensors on the board, but I have no idea how to fix this.
I'll post the code I'm using.

#include <ReefwingAHRS.h>
#include <Arduino_BMI270_BMM150.h>
#include <BasicLinearAlgebra.h>

using namespace BLA;

#define SAMPLES 1000

ReefwingAHRS ahrs;
SensorData data;


//  Display and Loop Frequency
int loopFrequency = 0;
const long displayPeriod = 1000;
unsigned long previousMillis = 0;
BLA::Matrix<3, 3> A = {1.024214, 0.001741, -0.005501, 0.001741, 1.016293, 0.002423, -0.005501, 0.002423, 1.056541};


BLA::Matrix<3> b = {-44.098118, 12.418187, -22.342269};

void setup() {
  //  Initialise the AHRS
  //  Use default fusion algo and parameters
  ahrs.begin();
  
  ahrs.setFusionAlgorithm(SensorFusion::MADGWICK);
  ahrs.setDeclination(3.567);                      

  //  Start Serial and wait for connection
  Serial.begin(115200);
  while (!Serial);

  Serial.print("Detected Board - ");
  Serial.println(ahrs.getBoardTypeString());

  if (IMU.begin() && ahrs.getBoardType() == BoardType::NANO33BLE_SENSE_R2) {
    Serial.println("BMI270 & BMM150 IMUs Connected."); 
    Serial.print("Gyroscope sample rate = ");
    Serial.print(IMU.gyroscopeSampleRate());
    Serial.println(" Hz");
    Serial.println();
    Serial.println("Gyroscope in degrees/second");
    Serial.print("Accelerometer sample rate = ");
    Serial.print(IMU.accelerationSampleRate());
    Serial.println(" Hz");
    Serial.println();
    Serial.println("Acceleration in G's");
    Serial.print("Magnetic field sample rate = ");
    Serial.print(IMU.magneticFieldSampleRate());
    Serial.println(" Hz");
    Serial.println();
    Serial.println("Magnetic Field in uT");
  } 
  else {
    Serial.println("BMI270 & BMM150 IMUs Not Detected.");
    while(1);
  }
   delay(2000);
}

void loop() {
  if (IMU.gyroscopeAvailable()) {  IMU.readGyroscope(data.gx, data.gy, data.gz);  }
  if (IMU.accelerationAvailable()) {  IMU.readAcceleration(data.ax, data.ay, data.az);  }
  if (IMU.magneticFieldAvailable()) {  IMU.readMagneticField(data.mx, data.my, data.mz);  }

  BLA::Matrix<3> mag_raw = {data.mx, data.my, data.mz};
  BLA::Matrix<3> mag_cal = A * (mag_raw - b);

  data.mx = mag_cal(0);
  data.my = mag_cal(1);
  data.mz = mag_cal(2);

  ahrs.setData(data);
  ahrs.update();
  
  if (millis() - previousMillis >= displayPeriod) {
    //  Display sensor data every displayPeriod, non-blocking.
    Serial.print("--> Roll: ");
    Serial.print(ahrs.angles.roll, 2);
    Serial.print("\tPitch: ");
    Serial.print(ahrs.angles.pitch, 2);
    Serial.print("\tYaw: ");
    Serial.print(ahrs.angles.yaw, 2);
    Serial.print("\tHeading: ");
    Serial.print(ahrs.angles.heading, 2);
    Serial.print("\tLoop Frequency: ");
    Serial.print(loopFrequency);
    Serial.println(" Hz");

    loopFrequency = 0;
    previousMillis = millis();
  }

  loopFrequency++;
}