Fixed re-initialization of estimator, re-initializes in air now reliably. Does give useful HIL results.