Added LM and fixed indexing. The landmark positions seem good, but the errors are still high