Source: http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17.pdf