https://5dok.org/document/nq72dooz-efficient-implementation-second-order-extended-kalman-filter.html