Add update pose and odom stuff