update_imu return NaN quaternion

[MauroMombelli]

let mut ahrs = Madgwick::<f32>::new(1.0/1000.0, 0.1);

let gyro = Vector3::new(0.0, 0.0, 0.0);
let acce = Vector3::new(0.0, 0.0, 1.0);

let quat = ahrs.update_imu(
  &gyro,
  &acce,
).unwrap();

quat will be NaN. The issue seems to be let step = (J_t * F).normalize(); in particular in the multiplication J_t * F

[peterkrull]

The multiplication seems fine to me, but normalizing a vector where all entries are zeros is what seems to return NaN. However it seems to me that the entries of F are only zero in very specific cases, such as when the first accelerometer reading only contain a non-zero value in the positive z-direction. If the filter is initialized to anything else, it does not seem to return NaN.

Perhaps normalizing directly using (J_t * F).normalize() is not a good idea then, even if the documentation for the algorithm pretty much does this as well. Instead either using .try_normalize() and then handle the fault case, or simply add a small number to the divisor of the normalization.

let neta: N = nalgebra::convert(1e-3);

let gradient = J_t * F;
let step = gradient.unscale(gradient.norm() + neta);

I am using this crate in my drone project, and have twice experienced that the drone went crazy after a few minutes of flight, but at seemingly random times when I am flying calmly. I do not know if this is related in any way, or if the crate is even to blame at all, but this division by zero (or nearly zero) does spook me out a bit. Does anyone else have experience with if this has affected their use of the crate?

Edit: This crate was not to blame for my crashes. I am however now flying with the change described above