new sonar model (change attitude ekf filter, no mag), not tested yet
rewrite sonar model to update with position, not tested yet