Updated 3D cooperative SLAM files
Currently still debugging
Seems to almost work, but covariances are too low and position estimation does not converge well
Added compiled versions of the assocaited UKF function for speed