EKF SLAM now agrees with the output format of SLAM, and can deal correctly with both online and offline SLAM

IssueID #10