Changed EKF back, moved predict_feature_appearance and calculate_derivatives back into update.
Merged with current updated version. All I actually did was update EKF lol