Implemented scan to map matching, but not working yet.