Resolve map issue.
Clean up motion model code slightly.