@@ -6,15 +6,16 @@ const DEFAULT_NONLINMHE_OPTIMIZER = optimizer_with_attributes(Ipopt.Optimizer,"s
66
77Construct a moving horizon estimator (MHE) based on `model` ([`LinModel`](@ref) or [`NonLinModel`](@ref)).
88
9- This estimator can handle constraints on the estimates, see [`setconstraint!`](@ref).
10- Additionally, `model` is not linearized like the [`ExtendedKalmanFilter`](@ref), and the
11- probability distribution is not approximated like the [`UnscentedKalmanFilter`](@ref). The
12- computational costs are drastically higher, however, since it minimizes the following
13- objective function at each discrete time ``k``:
9+ It can handle constraints on the estimates, see [`setconstraint!`](@ref). Additionally,
10+ `model` is not linearized like the [`ExtendedKalmanFilter`](@ref), and the probability
11+ distribution is not approximated like the [`UnscentedKalmanFilter`](@ref). The computational
12+ costs are drastically higher, however, since it minimizes the following objective function
13+ at each discrete time ``k``:
1414```math
15- \m in_{\m athbf{x̂}_k(k-N_k+1), \m athbf{Ŵ}} \m athbf{x̄}' \m athbf{P̄}^{-1} \m athbf{x̄}
16- + \m athbf{Ŵ}' \m athbf{Q̂}_{N_k}^{-1} \m athbf{Ŵ}
17- + \m athbf{V̂}' \m athbf{R̂}_{N_k}^{-1} \m athbf{V̂}
15+ \m in_{\m athbf{x̂}_k(k-N_k+1), \m athbf{Ŵ}, ϵ} \m athbf{x̄}' \m athbf{P̄}^{-1} \m athbf{x̄}
16+ + \m athbf{Ŵ}' \m athbf{Q̂}_{N_k}^{-1} \m athbf{Ŵ}
17+ + \m athbf{V̂}' \m athbf{R̂}_{N_k}^{-1} \m athbf{V̂}
18+ + C ϵ^2
1819```
1920in which the arrival costs are evaluated from the states estimated at time ``k-N_k``:
2021```math
@@ -38,22 +39,20 @@ N_k = \begin{cases}
3839```
3940The vectors ``\m athbf{Ŵ}`` and ``\m athbf{V̂}`` encompass the estimated process noise
4041``\m athbf{ŵ}(k-j)`` and sensor noise ``\m athbf{v̂}(k-j)`` from ``j=N_k-1`` to ``0``. The
41- Extended Help explicitly defines the two vectors. See [`SteadyKalmanFilter`](@ref) for
42- details on ``\m athbf{R̂}, \m athbf{Q̂}`` covariances and model augmentation. The process
43- model is identical to the one in [`UnscentedKalmanFilter`](@ref) documentation. The matrix
42+ Extended Help defines the two vectors. See [`UnscentedKalmanFilter`](@ref) for details on
43+ the augmented process model and ``\m athbf{R̂}, \m athbf{Q̂}`` covariances. The matrix
4444``\m athbf{P̂}_{k-N_k}(k-N_k+1)`` is estimated with an [`ExtendedKalmanFilter`](@ref).
4545
4646!!! warning
47- See the Extended Help of [`NonLinMPC`](@ref) function if you get an error like:
47+ See the Extended Help if you get an error like:
4848 `MethodError: no method matching (::var"##")(::Vector{ForwardDiff.Dual})`.
4949
5050# Arguments
5151- `model::SimModel` : (deterministic) model for the estimations.
52- - `He=nothing`: estimation horizon ``H_e``, must be specified.
53- - `optim=default_optim_mhe(model)` : quadratic or nonlinear optimizer used in the moving
54- horizon estimator, provided as a [`JuMP.Model`](https://jump.dev/JuMP.jl/stable/api/JuMP/#JuMP.Model)
55- (default to [`Ipopt`](https://github.com/jump-dev/Ipopt.jl), or [`OSQP`](https://osqp.org/docs/parsers/jump.html)
56- if `model` is a [`LinModel`](@ref)).
52+ - `He=nothing` : estimation horizon ``H_e``, must be specified.
53+ - `optim=default_optim_mhe(model)` : a [`JuMP.Model`](https://jump.dev/JuMP.jl/stable/api/JuMP/#JuMP.Model)
54+ with a quadratic/nonlinear optimizer for solving (default to [`Ipopt`](https://github.com/jump-dev/Ipopt.jl),
55+ or [`OSQP`](https://osqp.org/docs/parsers/jump.html) if `model` is a [`LinModel`](@ref)).
5756- `<keyword arguments>` of [`SteadyKalmanFilter`](@ref) constructor.
5857- `<keyword arguments>` of [`KalmanFilter`](@ref) constructor.
5958
@@ -97,6 +96,13 @@ MovingHorizonEstimator estimator with a sample time Ts = 10.0 s, Ipopt optimizer
9796 \m athbf{x̂}_k(k-j+1) &= \m athbf{f̂}\B ig(\m athbf{x̂}_k(k-j), \m athbf{u}(k-j), \m athbf{d}(k-j)\B ig) + \m athbf{ŵ}(k-j)
9897 \e nd{aligned}
9998 ```
99+
100+ For [`LinModel`](@ref), the optimization is treated as a quadratic program with a
101+ time-varying Hessian, which is generally cheaper than nonlinear programming. For
102+ [`NonLinModel`](@ref), the optimization relies on automatic differentiation (AD).
103+ Optimizers generally benefit from exact derivatives like AD. However, the `f` and `h`
104+ functions must be compatible with this feature. See [Automatic differentiation](https://jump.dev/JuMP.jl/stable/manual/nlp/#Automatic-differentiation)
105+ for common mistakes when writing these functions.
100106"""
101107function MovingHorizonEstimator (
102108 model:: SM ;
520526
521527Set the constraint parameters of the [`MovingHorizonEstimator`](@ref) `estim`.
522528
523- The estimator supports both soft and hard constraints on the estimated state ``\m athbf{x̂}``,
524- process noise ``\m athbf{ŵ}`` and sensor noise ``\m athbf{v̂}``:
529+ It supports both soft and hard constraints on the estimated state ``\m athbf{x̂}``, process
530+ noise ``\m athbf{ŵ}`` and sensor noise ``\m athbf{v̂}``:
525531```math
526532\b egin{alignat*}{3}
527533 \m athbf{x̂_{min} - c_{x̂_{min}}} ϵ ≤&&\ \m athbf{x̂}_k(k-j+1) &≤ \m athbf{x̂_{max} + c_{x̂_{max}}} ϵ &&\q quad j = N_k, N_k - 1, ... , 0 \\
@@ -533,11 +539,9 @@ and also ``ϵ ≥ 0``. All the constraint parameters are vector. Use `±Inf` val
533539is no bound. The constraint softness parameters ``\m athbf{c}``, also called equal concern
534540for relaxation, are non-negative values that specify the softness of the associated bound.
535541Use `0.0` values for hard constraints. The process and sensor noise constraints are all soft
536- by default. Note that the state and process noise constraints are applied on the augmented
537- vectors (see the extended help of [`SteadyKalmanFilter`](@ref) for details on augmentation).
538- Also note that constraining the estimated sensor noises is equivalent to bounding the
539- innovation term, since ``\m athbf{v̂}(k) = \m athbf{y^m}(k) - \m athbf{ŷ^m}(k)``. See Extended
540- Help for time-varying constraints.
542+ by default. Notice that constraining the estimated sensor noises is equivalent to bounding
543+ the innovation term, since ``\m athbf{v̂}(k) = \m athbf{y^m}(k) - \m athbf{ŷ^m}(k)``. See
544+ Extended Help for details on model augmentation and time-varying constraints.
541545
542546# Arguments
543547!!! info
@@ -577,6 +581,9 @@ MovingHorizonEstimator estimator with a sample time Ts = 1.0 s, OSQP optimizer,
577581
578582# Extended Help
579583!!! details "Extended Help"
584+ Note that the state ``\m athbf{x̂}`` and process noise ``\m athbf{ŵ}`` constraints are
585+ applied on the augmented model, detailed in [`SteadyKalmanFilter`](@ref) Extended Help.
586+
580587 For variable constraints, the bounds can be modified after calling [`updatestate!`](@ref),
581588 that is, at runtime, except for `±Inf` bounds. Time-varying constraints over the
582589 estimation horizon ``H_e`` are also possible, mathematically defined as:
0 commit comments