Add BeliefRobotAndDOF and replace RobotAndDOF by the former as a hack. Add belief dynamics constraints. Add sphere/ellipsoid plotting, even though it is not being used currently.