gtsam/gtsam_unstable/dynamics/SimpleHelicopter.cpp