addes state constraints for angle turnaround
roll and yaw measurements need to be corrected ASAP