Added script for creating the math in the EKF and small fixes to the EKF