Nonlinear Moving-Horizon Estimation Using State- and Control-Dependent Models

Abstract

This paper presents a state- and control-dependent moving-horizon estimation (SCD-MHE) algorithm for nonlinear discrete-time systems. The nonlinear dynamics and measurement maps are written exactly in pseudo-linear form using state- and control-dependent coefficient (SCDC) matrices. At each time step, the moving-horizon estimation problem is solved by a sequence of sparse quadratic programs, where the SCDC matrices are refrozen along the trajectory computed by the preceding iteration; the estimator requires no Jacobians and retains the nonlinear model exactly. We show that each quadratic program has a unique solution, characterize the fixed points of the iteration, establish geometric convergence under a contraction condition, and prove that the estimation error of the computed estimate is uniformly bounded under uniform observability, bounded disturbances, a bounded arrival-cost error, and iterates confined to a compact set, for all finite iteration counts. In a quadrotor benchmark with a saturating rangefinder, the altitude RMSE of SCD-MHE is 18 times smaller than that of a nonlinear moving-horizon estimator that solves the nonlinear program and 58 times smaller than that of the extended and unscented Kalman filters, and its per-step computation time is 34 times smaller than that of the nonlinear estimator.