added specific trust boxes to cfg in slam-state. slam-state only does the vertical path now (because that is the main problem). note that changing the landmark positions changes where the optimization ends, notably that the end point ends near the closest landmark (so local minimum?)