https://github.com/maxibooksiyi/Prometheus/blob/pr-experiment-TX2/Modules/control/src/px4_pos_estimator.cpp