Fixed EKF function

Prediction step based on f(x) not linearisation
Included Jacobian, cleaned up code, implement EKF

Added model_JR function to either transition the model or return the
Jacobian, suitable for EKF and PCRLB caclualations