Added a neat EKF package where the EKF code can go