- Use `inpolygon` to check if position is within road and not in or on an obstacle - Add helper function `get_start_idx` to get starting index of Z(i) [states(i), inputs(i)] in Z vector (combined states & inputs for prediction horizon)