An efficient implementation of the second order extended Kalman filter

The second order extended Kalman filter (EKF2) is based on a second order Taylor expansion of a nonlinear system, in contrast to the more common (first order) extended Kalman filter (EKF1). Despite a solid theoretical ground for its approximation, it is seldom used in applications, where the EKF and the unscented Kalman filter (UKF) are the standard algorithms. One reason for this might be the requirement for analytical Jacobian and Hessian of the system equations, and the high complexity that scales with the state order n_x as n⁵_x. We propose a numerical algorithm which is based on an extended set of sigma points (compared to the UKF) that needs neither Jacobian nor Hessian (or numerical approximations of these). Further, it scales as n⁴_x, which is an order of magnitude better than the EKF2 algorithm presented in literature.