Updates to 7-state 3D coop estimator with UKF. Still does not work