Code used in generating results found in my ICRA2015 paper http://www.zjtaylor.com/welcome/download_pdf?pdf=ICRA2015.pdf&target=_blank