https://123dok.net/document/qvl19pkg-invariant-extended-kalman-filter-application-velocity-attitude-estimation.html