Added 3D coop estimation with magnetometer shared
Has not been debugged yet