EKF crashing when flying with VL53 sensor only

[ataffanel]

When flying with the VL53 deck and the kalman filter, the EKF is crashing at random interval on a NaN assert:

SYS: The system resumed after watchdog timeout [WARNING]
SYS: Assert failed at src/modules/src/estimator_kalman.c:789

It corresponds to this line:

crazyflie-firmware/src/modules/src/estimator_kalman.c

Line 789 in 72146a6

configASSERT(!isnan(HPHR));

Further investigation seems to indicate that in at least one case PHTd[STATE_X] was NaN.

Tested with the version 72146a6 and the config.mk:

ESTIMATOR=kalman
CFLAGS+=-DUPDATE_KALMAN_WITH_RANGING

[ataffanel]

Found that H of the tof update could contain a NaN:

crazyflie-firmware/src/modules/src/estimator_kalman.c

Line 958 in 72146a6

h[STATE_Z] = 1 / cosf(alpha);

h[STATE_Z] = 1 / cosf(alpha);

This is a good reason for the ekf to crash.

[ataffanel]

The problem comes from R[2][2] > 1.0. Since R is a rotation matrix it is not supposed to be possible.

[ataffanel]

I can confirm that there is rounding errors when calculating the rotation matrix R, I printed R[2][2] when it is > 1 and I get R[2][2] = 1.0. Which means that the value is really close to 1 but just over when compared to 1.

This should be fixable by making sure all element of R are within [-1, 1].

[ataffanel]

Fixed by removing the VL53 aperture angle compensation: the effect was very small (I cannot see any difference) and it removes the acos() that was causing the NaN.