Chanegd flyvslam to use LQG for height