Changed from lmmvlib to the px4 math implementation