Implements iLQG for Gaussian belief space planning according to:
Motion planning under uncertainty using iterative local optimization in belief space
Jur van den Berg, Sachin Patil, and Ron Alterovitz
Specifically, we implement the 2d holonomic robot with full state observations.
Jeff Walls jmwalls@umich.edu