Changed the signed element distance which fixed the error in dU
Need to fix the kalman filter update for quaternion based on discussion with
Ali