Switched to the rotation vector sensor, removing all other sensors. Also removed filter to smooth sensor data, not needed anymore. This fixes azimuth swaying issue.