search

bitcraze / crazyflie-firmware

The main firmware for the Crazyflie Nano Quadcopter, Crazyflie Bolt Quadcopter and Roadrunner Positioning Tag.
GNU General Public License v3.0
1.19k stars 1.06k forks source link

EKF crashing when flying with VL53 sensor only #182

Closed ataffanel closed 7 years ago

ataffanel commented 7 years ago

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: https://github.com/bitcraze/crazyflie-firmware/blob/72146a6a46e319f3af38eab6c64dcdd504466ba9/src/modules/src/estimator_kalman.c#L789

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

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

ESTIMATOR=kalman
CFLAGS+=-DUPDATE_KALMAN_WITH_RANGING
ataffanel commented 7 years ago

Found that H of the tof update could contain a NaN: https://github.com/bitcraze/crazyflie-firmware/blob/72146a6a46e319f3af38eab6c64dcdd504466ba9/src/modules/src/estimator_kalman.c#L958

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

This is a good reason for the ekf to crash.

ataffanel commented 7 years ago

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

ataffanel commented 7 years ago

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 commented 7 years ago

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.