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. Within this framework, a pseudo-linear representation of nonlinear dynamics is leveraged utilizing state- and control-dependent coefficients, where the solution to a moving-horizon estimation problem is iteratively refined. At each discrete time step, a quadratic program is executed over a sliding window of historical measurements. Moreover, system matrices are consecutively updated based upon prior iterates to capture nonlinear regimes. In contrast to the extended Kalman filter (EKF) and the unscented Kalman filter (UKF), nonlinearities and bounds are accommodated within a structured optimization framework, thereby circumventing the reliance on local Jacobian matrices. Furthermore, theoretical analysis is presented to establish the convergence of the iterative sequence, and bounded estimation errors are mathematically guaranteed under uniform observability conditions. Finally, comparative numerical experiments utilizing a quadrotor vertical kinematics system demonstrate that the SCD-MHE achieves superior estimation accuracy relative to the EKF, the UKF, and a fully nonlinear moving-horizon estimator, while reducing per-step computational latency by over an order of magnitude.

• The paper presents an SCD-MHE method that replaces conventional linearization with state- and control-dependent pseudo-linear models to achieve robust estimation in nonlinear systems.
• It leverages a warm-start strategy and sequential arrival cost updates to ensure rapid convergence and maintain computational efficiency.
• Numerical evaluations demonstrate significant improvements in estimation accuracy and latency, outperforming EKF, UKF, and fully nonlinear MHE in challenging scenarios.

Nonlinear Moving-Horizon Estimation Using State- and Control-Dependent Models: An Expert Review

Problem Formulation and Motivation

This work proposes a state- and control-dependent moving-horizon estimation (SCD-MHE) framework for nonlinear discrete-time systems, extending canonical approaches to nonlinear state estimation beyond the limitations of local linearization. Classical methods such as the EKF and UKF operate by local linearization or distribution transforms, which inherently sacrifice accuracy and robustness in the presence of significant nonlinearities, non-Gaussian noise, or severe initial miscalibration. The moving-horizon estimation (MHE) paradigm overcomes these drawbacks by framing estimation as a constrained optimization over sliding windows, naturally accommodating nonlinear dynamics and state/input/output constraints. However, fully nonlinear MHE methods require the repeated solution of non-convex programs, which are typically computationally prohibitive for real-time applications.

The proposed SCD-MHE algorithm exploits a pseudo-linear representation of the nonlinear system wherein dynamics and measurement maps are locally replaced with state- and control-dependent coefficient (SCDC) matrices, effectively capturing nonlinearities without explicit linearization and avoiding truncation errors. Optimization at each time step reduces to a sequence of quadratic programs, utilizing iterative updating of system matrices evaluated at prior estimates. This structure permits the direct use of efficient, sparse solvers, yielding a scalable estimator with substantially reduced computation.

SCD-MHE Algorithmic Framework

The SCD-MHE approach parameterizes the nonlinear system dynamics as

$$x_{k+1} = A(x_k, u_k, k) x_k + B(x_k, u_k, k) u_k + w_k,$$

$y_k = C(x_k, k)x_k + v_k,$

with general SCDC matrices $A(\cdot), B(\cdot), C(\cdot)$, as opposed to constant or Jacobian-based matrices. At each moving-horizon window, a constrained quadratic program is formulated over the state and noise sequences. The SCDC matrices are re-evaluated after each optimization on the latest trajectory; this loop iterates until convergence (measured by trajectory changes below tolerance or a hard iteration cap), employing a "warm start" seeded by the shifted (converged) estimate from the previous step.

Two algorithmic features are critical:

• Warm-Starting/Trajectory Shifting: Facilitates rapid convergence and robust contractivity, leveraging the temporal continuity of the optimal state trajectory.
• Sequential Arrival Cost Update: Historical data outside the MHE window are statistically compressed into a recursively updated arrival cost and covariance (using a discrete Riccati equation), anchoring the estimator and preventing information loss and divergence.

These components maintain strong convexity and structural sparsity, resulting in per-iteration computational complexity scaling linearly with the window length.

Theoretical Guarantees

A comprehensive stability analysis underpins the SCD-MHE framework:

• Under uniform observability (with respect to the true SCDC trajectories), boundedness of process and sensor noise, and Lipschitz continuity of SCDC matrices, estimation error is strictly bounded by a quantifiable constant for all time. Analytical development includes non-asymptotic upper bounds, employing observability gramians and contraction mapping theory.
• The iterative solution scheme is shown to constitute a contraction in neighborhood of the true trajectory, with convergence (in the Banach fixed-point sense) to a stationary point of the full moving-horizon cost. This result is algorithmically enforced via regularization of the Hessian (through weighting matrix inflation).
• The Riccati-based arrival cost update guarantees uniformly bounded information matrices and thus estimator regularity and avoidance of singularities.

No assumptions are made regarding system linearizability or noise isotropy beyond compact input/state sets and positive definite covariances, ensuring broad applicability.

Numerical Evaluation

A comprehensive simulation study investigates the performance of SCD-MHE, EKF, UKF, and fully nonlinear MHE (N-MHE) on a benchmark nonlinear system: quadrotor vertical kinematics with strong measurement saturation. The experiment is deliberately configured with highly ill-conditioned initial priors, resulting in an unobservable region for Jacobian-based methods (EKF, UKF) due to vanishing measurement sensitivity.

Key findings:

• Estimation Performance: SCD-MHE achieves an altitude RMSE of $0.56$ m, outperforming N-MHE ($10.26$ m RMSE) by an order of magnitude, and dramatically surpassing EKF/UKF ($\sim32.3$ m RMSE), which remain trapped in spurious or biased estimates due to measurement Jacobian collapse.
• Computational Cost: SCD-MHE's per-step latency is $1.96$ ms, substantially lower than N-MHE ($66.16$ ms), making it suitable for real-time implementation well within the 50 ms sampling constraint.
• The fully nonlinear MHE method recovers the true state after significant initial delay but is impractical for real-time due to computational cost; EKF/UKF do not recover in most runs.
• The core advantage arises from SCD-MHE's pseudo-linear parameterization, which structurally prevents loss of information through vanishing Jacobians and enables immediate post-horizon recovery upon sufficient measurement excitation.

Implications and Future Directions

The SCD-MHE formulation provides a computationally efficient and, under reasonable assumptions, provably stable method for nonlinear state estimation in feedback- and constraint-critical applications. By avoiding Jacobian dependence and enabling robust warm-starting and regularization, the framework extends practical MHE to systems and operating regimes previously considered infeasible in real time.

Potential future research directions include:

• Adaptive/hybrid horizon selection for further latency-accuracy tradeoff optimization.
• Online adjustment of estimation window in response to observability or disturbance changes.
• Systematic enforcement of state/output constraints directly in the quadratic program.
• Analysis of estimator robustness under non-Gaussian and time-varying noise models.
• Integration with advanced control, e.g., real-time MPC schemes where the estimator is tightly coupled with predictive optimization.

Conclusion

SCD-MHE constitutes a significant advance in nonlinear moving-horizon state estimation by leveraging SCDC structure for accuracy and speed. It combines strong theoretical guarantees with demonstrated empirical superiority in challenging regimes. The work provides a scalable estimation architecture especially well-suited for real-time control of nonlinear, constrained, or safety-critical systems, and serves as a foundation for further research in high-performance estimation and control algorithms for modern embedded applications (2604.00309).