working on test code for EKF SLAM