EKF filter update

This updates the EKF to the latest version of Paul Riseborough.