Kalman and pitch

[salviador]

Hi, I use Nano 33 BLE rev 2 setting in KALMAN mode, but when I write roll , pitch in serial I only get roll, pitch in nan

void setup() {
  ahrs.begin();

  ahrs.setFusionAlgorithm(SensorFusion::KALMAN  );
  ahrs.setDeclination(12.717);                     //  Sydney, Australia
}

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);  }

  ahrs.setData(data);
  ahrs.update();

  Serial.print(ahrs.angles.roll , 2);
  Serial.print(",");
  Serial.print(ahrs.angles.pitch, 2);
  Serial.println();
}

[reefwing]

There are a few points to consider that might lead to NaN (Not a Number) results under certain conditions in the Kalman filter:

Initial Conditions and Parameters:

• The filter is initialized with certain parameters (Q_angle, Q_bias, and R_measure). If these parameters are not suitable for your specific application or sensor characteristics, they could lead to instability in the filter's performance. You might need to experiment with these parameters to find the optimal values for your setup.

Division by Zero in Kalman Gain Calculation:

• In the line float S = P[0][0] + R_measure;, there is a risk of division by zero when calculating the Kalman gain (K[0] = P[0][0] / S; and K[1] = P[1][0] / S;). If S becomes zero or very close to zero, it could result in NaN values. A check can be added to ensure S is not zero before performing the division.