Richard Mason Joel W. Burdick Trajectory Planning using Reachable-State Density Functions. 273-280 2002 conf/icra/2002 ICRA db/conf/icra/icra2002.html#MasonB02