Added comments to the main EKF function.
Changed the tolerance to 1e-4 and changing the objective to minimize chi2 of residuals.
made the ftol 1e-5
Added the final_ekf submission.
Removed the out file.