Completed nonlinear EKF model code - first draft