Changed bag_load_cmdvel to use Matlab structures