Fixed EKF and added UKF (now used, slightly more accurate)