Improved sonar tracking filter. Completely stable.

Sonar filter: added sonar reading delay compensation before calculating
accelerometer bias.

Needs work on:
-IMU rotation matrix
-Tuning
-Replace MPU6050 by the one with magnetometer.