Added state and measurement updates to Multimodalrobotmodel