implemented L-BFGS. also, add checker to pr2_sim for data stamps (in case moving and getting last data measurement)
working on is_in_fov version
tried splitting up fov obs from Z_DIM=1 to 4, didn't work