Incremental changes for AMCL and navigation