Clean up some of the EKF code and correct some errors. Hopefully the coordinate reference frames now all match up.