construct.jl

const DEFAULT_LINMHE_OPTIMIZER    = OSQP.MathOptInterfaceOSQP.Optimizer
const DEFAULT_NONLINMHE_OPTIMIZER = optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes")
const DEFAULT_NONLINMHE_GRADIENT  = AutoForwardDiff()
const DEFAULT_NONLINMHE_JACOBIAN  = AutoForwardDiff()
const DEFAULT_NONLINMHE_HESSIAN   = AutoSparse(
    AutoForwardDiff();
    sparsity_detector=TracerSparsityDetector(),
    coloring_algorithm=GreedyColoringAlgorithm(ALL_COLORING_ORDERS, postprocessing=true),
) 

@doc raw"""
Include all the data for the constraints of [`MovingHorizonEstimator`](@ref).

The bounds on the estimated state at arrival ``\mathbf{x̂}_k(k-N_k+1)`` is separated from
the other state constraints ``\mathbf{x̂}_k(k-N_k+2), \mathbf{x̂}_k(k-N_k+3), ...`` since
the former is always a linear inequality constraint (it's a decision variable). The fields
`x̃min` and `x̃max` refer to the bounds at the arrival (augmented with the slack variable
ε), and `X̂min` and `X̂max`, the others.
"""
struct EstimatorConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}}
    # matrices for the estimated state constraints:
    Ẽx̂      ::Matrix{NT}
    Fx̂      ::Vector{NT}
    Gx̂      ::Matrix{NT}
    Jx̂      ::Matrix{NT}
    Bx̂      ::Vector{NT}
    # bounds over the estimation windows (deviation vectors from operating points):
    x̃0min   ::Vector{NT}
    x̃0max   ::Vector{NT}
    X̂0min   ::Vector{NT}
    X̂0max   ::Vector{NT}
    Ŵmin    ::Vector{NT}
    Ŵmax    ::Vector{NT}
    V̂min    ::Vector{NT}
    V̂max    ::Vector{NT}
    # A matrcies for the linear inequality constraints:
    A_x̃min  ::Matrix{NT}
    A_x̃max  ::Matrix{NT}
    A_X̂min  ::Matrix{NT}
    A_X̂max  ::Matrix{NT}
    A_Ŵmin  ::Matrix{NT}
    A_Ŵmax  ::Matrix{NT}
    A_V̂min  ::Matrix{NT}
    A_V̂max  ::Matrix{NT}
    A       ::Matrix{NT}
    # b vectir for the linear inequality constraints:
    b       ::Vector{NT}
    # constraint softness parameter vectors needing seperate strorage:
    C_x̂min  ::Vector{NT}
    C_x̂max  ::Vector{NT}
    C_v̂min  ::Vector{NT}
    C_v̂max  ::Vector{NT}
    # indices of finite numbers in the b vector (linear inequality constraints):
    i_b     ::BitVector
    # indices of finite numbers in the g vectors (nonlinear inequality constraints):
    i_g     ::BitVector
    # custom nonlinear inequality constraints:
    gc!     ::GCfunc
    nc      ::Int
end

struct MovingHorizonEstimator{
    NT<:Real, 
    SM<:SimModel,
    KC<:KalmanCovariances,
    JM<:JuMP.GenericModel,
    GB<:AbstractADType,
    JB<:AbstractADType,
    HB<:Union{AbstractADType, Nothing},
    PT<:Any,
    GCfunc<:Function,
    CE<:KalmanEstimator,
} <: StateEstimator{NT}
    model::SM
    cov  ::KC
    # note: `NT` and the number type `JNT` in `JuMP.GenericModel{JNT}` can be
    # different since solvers that support non-Float64 are scarce.
    optim::JM
    con::EstimatorConstraint{NT, GCfunc}
    gradient::GB
    jacobian::JB
    hessian::HB
    covestim::CE
    Z̃::Vector{NT}
    lastu0::Vector{NT}
    x̂op::Vector{NT}
    f̂op::Vector{NT}
    x̂0 ::Vector{NT}
    He::Int
    nε::Int
    i_ym::Vector{Int}
    nx̂ ::Int
    nym::Int
    nyu::Int
    nxs::Int
    p::PT
    As  ::Matrix{NT}
    Cs_u::Matrix{NT}
    Cs_y::Matrix{NT}
    nint_u ::Vector{Int}
    nint_ym::Vector{Int}
    Â   ::Matrix{NT}
    B̂u  ::Matrix{NT}
    Ĉ   ::Matrix{NT}
    B̂d  ::Matrix{NT}
    D̂d  ::Matrix{NT}
    Ĉm  ::Matrix{NT}
    D̂dm ::Matrix{NT}
    Ẽ ::Matrix{NT}
    F ::Vector{NT}
    G ::Matrix{NT}
    J ::Matrix{NT}
    B ::Vector{NT}
    ẽx̄::Matrix{NT}
    fx̄::Vector{NT}
    H̃::Hermitian{NT, Matrix{NT}}
    q̃::Vector{NT}
    r::Vector{NT}
    C::NT
    X̂op::Vector{NT}
    Y0m::Vector{NT}
    Yem::Vector{NT}
    U0 ::Vector{NT}
    Ue ::Vector{NT}
    D0 ::Vector{NT}
    De ::Vector{NT}
    Ŵ  ::Vector{NT}
    X̂0_old   ::Vector{NT}
    x̂0arr_old::Vector{NT}
    P̂arr_old ::Hermitian{NT, Matrix{NT}}
    Nk::Vector{Int}
    direct::Bool
    corrected::Vector{Bool}
    buffer::StateEstimatorBuffer{NT}
    function MovingHorizonEstimator{NT}(
        model::SM, 
        He, i_ym, nint_u, nint_ym, cov::KC, Cwt, 
        gc!::GCfunc, nc, p::PT,
        optim::JM, gradient::GB, jacobian::JB, hessian::HB, covestim::CE;
        direct=true
    ) where {
            NT<:Real, 
            SM<:SimModel{NT}, 
            KC<:KalmanCovariances,
            JM<:JuMP.GenericModel, 
            GB<:AbstractADType,
            JB<:AbstractADType,
            HB<:Union{AbstractADType, Nothing},
            PT<:Any,
            GCfunc<:Function,
            CE<:KalmanEstimator{NT}
        }
        nu, ny, nd, nk = model.nu, model.ny, model.nd, model.nk
        He < 1  && throw(ArgumentError("Estimation horizon He should be ≥ 1"))
        Cwt < 0 && throw(ArgumentError("Cwt weight should be ≥ 0"))
        nym, nyu = validate_ym(model, i_ym)
        As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym)
        nxs = size(As, 1)
        nx̂  = model.nx + nxs
        Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model(model, As, Cs_u, Cs_y)
        Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :]
        lastu0 = zeros(NT, nu)
        x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)]
        E, G, J, B, ex̄, Ex̂, Gx̂, Jx̂, Bx̂ = init_predmat_mhe(
            model, He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, direct
        )
        # dummy values (updated just before optimization):
        F, fx̄, Fx̂ = zeros(NT, nym*He), zeros(NT, nx̂), zeros(NT, nx̂*He)
        con, nε, Ẽ, ẽx̄ = init_defaultcon_mhe(
            model, He, Cwt, nx̂, nym, E, ex̄, Ex̂, Fx̂, Gx̂, Jx̂, Bx̂, gc!, nc
        )
        nZ̃ = size(Ẽ, 2)
        # dummy values, updated before optimization:
        H̃, q̃, r = Hermitian(zeros(NT, nZ̃, nZ̃), :L), zeros(NT, nZ̃), zeros(NT, 1)
        Z̃ = zeros(NT, nZ̃)
        X̂op  = repeat(x̂op, He)
        Y0m, Yem = fill(NT(NaN), nym*He),    fill(NT(NaN), nym*(He+1))
        U0,  Ue  = fill(NT(NaN), nu*He),     fill(NT(NaN),  nu*(He+1))
        D0,  De  = fill(NT(NaN), nd*(He+1)), fill(NT(NaN),  nd*(He+1))
        Ŵ        = fill(NT(NaN), nx̂*He)
        X̂0_old   = fill(NT(NaN), nx̂*He)
        D0[1:nd] .= 0 # D0 start with d0(-1) and it should not be NaN
        x̂0arr_old = zeros(NT, nx̂)
        P̂arr_old = copy(cov.P̂_0)
        Nk = [0]
        corrected = [false]
        test_custom_function_mhe(NT, model, i_ym, He, gc!, nc, x̂op, p, direct)
        buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk, He, nε)
        estim = new{NT, SM, KC, JM, GB, JB, HB, PT, GCfunc, CE}(
            model,
            cov,
            optim, con, 
            gradient, jacobian, hessian,
            covestim,  
            Z̃, lastu0, x̂op, f̂op, x̂0, 
            He, nε,
            i_ym, nx̂, nym, nyu, nxs, 
            p,
            As, Cs_u, Cs_y, nint_u, nint_ym,
            Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm,
            Ẽ, F, G, J, B, ẽx̄, fx̄,
            H̃, q̃, r,
            Cwt,
            X̂op, 
            Y0m, Yem, U0, Ue, D0, De, Ŵ, 
            X̂0_old, x̂0arr_old, P̂arr_old, Nk,
            direct, corrected,
            buffer
        )
        init_optimization!(estim, model, optim)
        return estim
    end
end

@doc raw"""
    MovingHorizonEstimator(model::SimModel; <keyword arguments>)

Construct a moving horizon estimator (MHE) based on `model` ([`LinModel`](@ref) or [`NonLinModel`](@ref)).

It can handle constraints on the estimates. Additionally, `model` is not linearized like the
[`ExtendedKalmanFilter`](@ref), and the probability  distribution is not approximated like
the [`UnscentedKalmanFilter`](@ref). The computational costs are drastically higher, 
however, since it minimizes the following objective function at each discrete time ``k``:
```math
\min_{\mathbf{x̂}_k(k-N_k+p), \mathbf{Ŵ}, ε}   \mathbf{x̄}' \mathbf{P̄}^{-1}       \mathbf{x̄} 
                                            + \mathbf{Ŵ}' \mathbf{Q̂}_{N_k}^{-1} \mathbf{Ŵ}  
                                            + \mathbf{V̂}' \mathbf{R̂}_{N_k}^{-1} \mathbf{V̂}
                                            + C ε^2
```
subject to [`setconstraint!`](@ref) bounds and the custom nonlinear inequality constraints:
```math
\mathbf{g_c}(\mathbf{X̂_e, V̂_e, Ŵ_e, U_e, Y_e^m, D_e, P̄, x̄, p}, ε) ≤ \mathbf{0}
```
and in which the arrival costs are evaluated from the states estimated at time ``k-N_k``:
```math
\begin{aligned}
    \mathbf{x̄} &= \mathbf{x̂}_{k-N_k}(k-N_k+p) - \mathbf{x̂}_k(k-N_k+p) \\
    \mathbf{P̄} &= \mathbf{P̂}_{k-N_k}(k-N_k+p)
\end{aligned}
```
The covariances are repeated ``N_k`` times:
```math
\begin{aligned}
    \mathbf{Q̂}_{N_k} &= \text{diag}\mathbf{(Q̂,Q̂,...,Q̂)}  \\
    \mathbf{R̂}_{N_k} &= \text{diag}\mathbf{(R̂,R̂,...,R̂)} 
\end{aligned}
```
The estimation horizon ``H_e`` limits the window length:
```math
N_k =                     \begin{cases}
    k + 1   &  k < H_e    \\
    H_e     &  k ≥ H_e    \end{cases}
```
The vectors ``\mathbf{Ŵ}`` and ``\mathbf{V̂}`` respectively encompass the estimated process
noises ``\mathbf{ŵ}(k-j+p)`` and sensor noises ``\mathbf{v̂}(k-j+1)`` from ``j=N_k`` to ``1``.
The arguments of ``\mathbf{g_c}`` include the extended vectors of the estimated states 
``\mathbf{X̂_e}``, estimated sensor noises ``\mathbf{V̂_e}``,  estimated process noises
``\mathbf{Ŵ_e}``, manipulated inputs ``\mathbf{U_e}``, measured outputs ``\mathbf{Y_e^m}``
and measured disturbances ``\mathbf{D_e}``. The Extended Help details all these vectors, the
slack variable ``ε`` and the estimation of the covariance at arrival 
``\mathbf{P̂}_{k-N_k}(k-N_k+p)``. If the keyword argument `direct=true` (default value), the
constant ``p=0`` in the equations above, and the MHE is in the current form. Else ``p=1``,
leading to the prediction form.

See [`UnscentedKalmanFilter`](@ref) for details on the augmented process model and 
``\mathbf{R̂}, \mathbf{Q̂}`` covariances. This estimator allocates a fair amount of memory 
at each time step for the optimization, which is hard-coded as a single shooting
transcription for now.

!!! warning
    See the Extended Help if you get an error like:    
    `MethodError: no method matching (::var"##")(::Vector{ForwardDiff.Dual})`.

# Arguments
!!! info
    Keyword arguments with *`emphasis`* are non-Unicode alternatives.

- `model::SimModel` : (deterministic) model for the estimations.
- `He=nothing` : estimation horizon ``H_e``, must be specified.
- `i_ym=1:model.ny` : `model` output indices that are measured ``\mathbf{y^m}``, the rest 
    are unmeasured ``\mathbf{y^u}``.
- `σP_0=fill(1/model.nx,model.nx)` or *`sigmaP_0`* : main diagonal of the initial estimate
    covariance ``\mathbf{P}(0)``, specified as a standard deviation vector.
- `σQ=fill(1/model.nx,model.nx)` or *`sigmaQ`* : main diagonal of the process noise
    covariance ``\mathbf{Q}`` of `model`, specified as a standard deviation vector.
- `σR=fill(1,length(i_ym))` or *`sigmaR`* : main diagonal of the sensor noise covariance
    ``\mathbf{R}`` of `model` measured outputs, specified as a standard deviation vector.
- `nint_u=0`: integrator quantity for the stochastic model of the unmeasured disturbances at
    the manipulated inputs (vector), use `nint_u=0` for no integrator (see Extended Help).
- `nint_ym=default_nint(model,i_ym,nint_u)` : same than `nint_u` but for the unmeasured 
    disturbances at the measured outputs, use `nint_ym=0` for no integrator (see Extended Help).
- `σQint_u=fill(1,sum(nint_u))` or *`sigmaQint_u`* : same than `σQ` but for the unmeasured
    disturbances at manipulated inputs ``\mathbf{Q_{int_u}}`` (composed of integrators).
- `σPint_u_0=fill(1,sum(nint_u))` or *`sigmaPint_u_0`* : same than `σP_0` but for the unmeasured
    disturbances at manipulated inputs ``\mathbf{P_{int_u}}(0)`` (composed of integrators).
- `σQint_ym=fill(1,sum(nint_ym))` or *`sigmaQint_u`* : same than `σQ` for the unmeasured
    disturbances at measured outputs ``\mathbf{Q_{int_{ym}}}`` (composed of integrators).
- `σPint_ym_0=fill(1,sum(nint_ym))` or *`sigmaPint_ym_0`* : same than `σP_0` but for the unmeasured
    disturbances at measured outputs ``\mathbf{P_{int_{ym}}}(0)`` (composed of integrators).
- `Cwt=Inf` : slack variable weight ``C``, default to `Inf` meaning hard constraints only.
- `gc=(_,_,_,_,_,_,_,_,_,_,_)->nothing` or `gc!` : custom nonlinear inequality constraint function 
   ``\mathbf{g_c}(\mathbf{X̂_e, V̂_e, Ŵ_e, U_e, Y_e^m, D_e, P̄, x̄, p}, ε)``, mutating or not 
   (details in Extended Help).
- `nc=0` : number of custom nonlinear inequality constraints.
- `p=model.p` : ``\mathbf{g_c}`` functions parameter ``\mathbf{p}`` (any type).
- `optim=default_optim_mhe(model,nc)` : a [`JuMP.Model`](@extref) object with a quadratic or
   nonlinear optimizer for solving (default to [`Ipopt`](https://github.com/jump-dev/Ipopt.jl),
   or [`OSQP`](https://osqp.org/docs/parsers/jump.html) if `model` is a [`LinModel`](@ref)).
- `gradient=AutoForwardDiff()` : an `AbstractADType` backend for the gradient of the objective
   function when `model` is not a [`LinModel`](@ref), see [`DifferentiationInterface` doc](@extref DifferentiationInterface List).
- `jacobian=AutoForwardDiff()` : an `AbstractADType` backend for the Jacobian of the
   constraints when `model` is not a [`LinModel`](@ref), see `gradient` above for the options.
- `hessian=false` : an `AbstractADType` backend for the Hessian of the Lagrangian, see 
   `gradient` above for the options. The default `false` skip it and use the quasi-Newton
   method of `optim` (see Extended Help).
- `direct=true`: construct with a direct transmission from ``\mathbf{y^m}`` (a.k.a. current
   estimator, in opposition to the delayed/predictor form).

# Examples
```jldoctest
julia> model = NonLinModel((x,u,_,_)->0.1x+u, (x,_,_)->2x, 10.0, 1, 1, 1, solver=nothing);

julia> estim = MovingHorizonEstimator(model, He=5, σR=[1], σP_0=[0.01])
MovingHorizonEstimator estimator with a sample time Ts = 10.0 s:
├ model: NonLinModel
├ optimizer: Ipopt 
├ gradient: AutoForwardDiff
├ jacobian: AutoForwardDiff
├ hessian: nothing
├ arrival covariance: UnscentedKalmanFilter 
├ direct: true
└ dimensions:
  ├ 5 estimation steps He
  ├ 0 slack variable ε (estimation constraints)
  ├ 1 manipulated inputs u (0 integrating states)
  ├ 2 estimated states x̂
  ├ 1 measured outputs ym (1 integrating states)
  ├ 0 unmeasured outputs yu
  └ 0 measured disturbances d
```

# Extended Help
!!! details "Extended Help"
    The estimated process and sensor noises are defined as:
    ```math
    \mathbf{Ŵ} = 
    \begin{bmatrix}
        \mathbf{ŵ}(k-N_k+p+0)     \\
        \mathbf{ŵ}(k-N_k+p+1)     \\
        \vdots                  \\
        \mathbf{ŵ}(k+p-1)
    \end{bmatrix} , \quad
    \mathbf{V̂} =
    \begin{bmatrix}
        \mathbf{v̂}(k-N_k+1)     \\
        \mathbf{v̂}(k-N_k+2)     \\
        \vdots                  \\
        \mathbf{v̂}(k)
    \end{bmatrix}
    ```
    based on the augmented model functions ``\mathbf{f̂, ĥ^m}``:
    ```math
    \begin{aligned}
        \mathbf{v̂}(k-j)     &= \mathbf{y^m}(k-j) - \mathbf{ĥ^m}\Big(\mathbf{x̂}_k(k-j), \mathbf{d}(k-j)\Big) \\
        \mathbf{x̂}_k(k-j+1) &= \mathbf{f̂}\Big(\mathbf{x̂}_k(k-j), \mathbf{u}(k-j), \mathbf{d}(k-j)\Big) + \mathbf{ŵ}(k-j)
    \end{aligned}
    ```
    The constant ``p`` equals to `!direct`. In other words, ``\mathbf{Ŵ}`` and ``\mathbf{V̂}``
    are shifted by one time step if `direct==true`. The non-default prediction form
    with ``p=1`` is particularly useful for the MHE since it moves its expensive
    computations after the MPC optimization. That is, [`preparestate!`](@ref) will solve the
    optimization by default, but it can be postponed to [`updatestate!`](@ref) with
    `direct=false`.

    The Extended Help of [`SteadyKalmanFilter`](@ref) details the tuning of the covariances
    and the augmentation with `nint_ym` and `nint_u` arguments. The default augmentation
    scheme is identical, that is `nint_u=0` and `nint_ym` computed by [`default_nint`](@ref).
    Note that the constructor does not validate the observability of the resulting augmented
    [`NonLinModel`](@ref). In such cases, it is the user's responsibility to ensure that it
    is still observable.

    The argument ``\mathbf{p}`` in the ``\mathbf{g_c}`` function is a custom parameter
    object of any type, but use a mutable one if you want to modify it later e.g.: a vector.
    The slack variable ``ε`` relaxes the constraints if enabled, see [`setconstraint!`](@ref). 
    It is disabled thus always zero by default for the MHE (from `Cwt=Inf`) but it should be
    activated for problems with two or more types of bounds, to ensure feasibility (e.g. on
    ``\mathbf{x̂}`` and ``\mathbf{v̂}``). The following table details the arguments of 
    ``\mathbf{g_c}``, including the time steps of the first and last sample in them. 

    !!! warning
        The vectors will grows with time until ``N_k = H_e`` is reached. The time series are
        also *artificially aligned* to ease the user life, but some data at boundaries are
        unavailable e.g.: ``\mathbf{u}(k)`` with ``p=0``. They are filled with `NaN` values.
        The exact time steps of the `NaN`s are detailed in the last column below.

    | ARGUMENT           | SIZE            | FIRST SAMPLE    | LAST SAMPLE     | MISSING SAMPLES    |
    | :---------------   | :-------------- | :-------------- | :-------------- | :----------------- |
    | ``\mathbf{X̂_e}``   | `((Nk+1)*nx̂,)`  | ``k - N_k + p`` | ``k + p``       | —                  |
    | ``\mathbf{V̂_e}``   | `((Nk+1)*nym,)` | ``k - N_k + p`` | ``k + p``       | ``k - N_k, k + 1`` |
    | ``\mathbf{Ŵ_e}``   | `((Nk+1)*nx̂,)`  | ``k - N_k + p`` | ``k + p``       | ``k + p``          |
    | ``\mathbf{U_e}``   | `((Nk+1)*nu,)`  | ``k - N_k + p`` | ``k + p``       | ``k + p``          |
    | ``\mathbf{Y_e^m}`` | `((Nk+1)*nym,)` | ``k - N_k + p`` | ``k + p``       | ``k + 1``          |
    | ``\mathbf{D_e}``   | `((Nk+1)*nd,)`  | ``k - N_k + p`` | ``k + p``       | ``k + 1``          |
    | ``\mathbf{P̄}``     | `(nx̂, nx̂)`      | ``k - N_k + p`` | ``k - N_k + p`` | —                  |
    | ``\mathbf{x̄}``     | `(nx̂,)`         | ``k - N_k + p`` | ``k - N_k + p`` | —                  |
    | ``\mathbf{p}``     | var.            | —               | —               | —                  |
    | ``ε``              | `()`            | —               | —               | —                  |

    If `LHS` represents the result of the left-hand side in the inequality 
    ``\mathbf{g_c}(\mathbf{X̂_e, V̂_e, Ŵ_e, U_e, Y_e^m, D_e, P̄, x̄, p}, ε) ≤ \mathbf{0}``,
    the function `gc` can be implemented in two possible ways:
    
    1. **Non-mutating function** (out-of-place): define it as `gc(X̂e, V̂e, Ŵe, Ue, Yem, De, 
       P̄, x̄, p, ε) -> LHS`. This syntax is simple and intuitive but it allocates more memory.
    2. **Mutating function** (in-place): define it as `gc!(LHS, X̂e, V̂e, Ŵe, Ue, Yem, De, P̄,
       x̄, p, ε) -> nothing`. This syntax reduces the allocations and potentially the
       computational burden as well.

    The keyword argument `nc` is the number of elements in `LHS`, and `gc!`, an alias for
    the `gc` argument (both `gc` and `gc!` accepts non-mutating and mutating functions).

    The estimation covariance at arrival ``\mathbf{P̂}_{k-N_k}(k-N_k+p)`` gives an uncertainty
    on the state estimate at the beginning of the window ``k-N_k+p``, that is, in the past.
    It is not the same as the current estimate covariance ``\mathbf{P̂}_k(k)``, a value not
    computed by the MHE (contrarily to e.g. the [`KalmanFilter`](@ref)). Three keyword
    arguments specify its initial value with ``\mathbf{P̂_i} =  \mathrm{diag}\{ \mathbf{P}(0),
    \mathbf{P_{int_{u}}}(0), \mathbf{P_{int_{ym}}}(0) \}``. The initial state estimate
    ``\mathbf{x̂_i}`` can be manually specified with [`setstate!`](@ref), or automatically 
    with [`initstate!`](@ref) for [`LinModel`](@ref). Note the MHE with ``p=0`` is slightly
    inconsistent with all the other estimators here. It interprets the initial values as
    ``\mathbf{x̂_i} = \mathbf{x̂}_{-1}(-1)`` and  ``\mathbf{P̂_i} = \mathbf{P̂}_{-1}(-1)``, an 
    *a posteriori* estimate[^2] from the last time step. The MHE with ``p=1`` is consistent,
    interpreting them as  ``\mathbf{x̂_i} = \mathbf{x̂}_{-1}(0)`` and
    ``\mathbf{P̂_i} = \mathbf{P̂}_{-1}(0)``.

    [^2]: M. Hovd (2012), "A Note On The Smoothing Formulation Of Moving Horizon Estimation",
          *Facta Universitatis*, Vol. 11 №2.

    The optimization and the update of the arrival covariance depend on `model`:

    - If `model` is a [`LinModel`](@ref), the optimization is treated as a quadratic program
      with a time-varying Hessian, which is generally cheaper than nonlinear programming. By
      default, a [`KalmanFilter`](@ref) estimates the arrival covariance (customizable).
    - Else, a nonlinear program with dense [`ForwardDiff`](@extref ForwardDiff) automatic
      differentiation (AD) compute the objective and constraint derivatives by default 
      (customizable). Optimizers generally benefit from exact derivatives like AD. However, 
      the `f` and `h` functions must be compatible with this feature. See the 
      [`JuMP` documentation](@extref JuMP Common-mistakes-when-writing-a-user-defined-operator)
      for common mistakes when writing these functions. Also, an [`UnscentedKalmanFilter`](@ref)
      estimates the arrival covariance by default.

    One exception about AD: the selected backend for the Hessian of the Lagrangian function
    with `hessian=true` options is sparse:
    ```julia
    AutoSparse(
        AutoForwardDiff(); 
        sparsity_detector  = TracerSparsityDetector(), 
        coloring_algorithm = GreedyColoringAlgorithm(
            (
                NaturalOrder(),
                LargestFirst(),
                SmallestLast(),
                IncidenceDegree(),
                DynamicLargestFirst(),
                RandomOrder(StableRNG(0), 0)
            ), 
        postprocessing = true
        )
    )
    ```
    that is, it will test many coloring orders at preparation and keep the best. 

    Note that if `Cwt≠Inf`, the attribute `nlp_scaling_max_gradient` of `Ipopt` is set to 
    `10/Cwt` (if not already set), to scale the small values of ``ε``. Use the second
    constructor to specify the arrival covariance estimation method.
"""
function MovingHorizonEstimator(
    model::SM;
    He::Union{Int, Nothing} = nothing,
    i_ym::AbstractVector{Int} = 1:model.ny,
    sigmaP_0 = fill(1/model.nx, model.nx),
    sigmaQ   = fill(1/model.nx, model.nx),
    sigmaR   = fill(1, length(i_ym)),
    nint_u ::IntVectorOrInt = 0,
    nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u),
    sigmaPint_u_0  = fill(1, max(sum(nint_u),  0)),
    sigmaQint_u    = fill(1, max(sum(nint_u),  0)),
    sigmaPint_ym_0 = fill(1, max(sum(nint_ym), 0)),
    sigmaQint_ym   = fill(1, max(sum(nint_ym), 0)),
    Cwt::Real = Inf,
    gc!::Function = (_,_,_,_,_,_,_,_,_,_,_) -> nothing,
    gc ::Function = gc!,
    nc ::Int = 0,
    p = model.p,
    optim::JM = default_optim_mhe(model, nc),
    gradient::AbstractADType = DEFAULT_NONLINMHE_GRADIENT,
    jacobian::AbstractADType = DEFAULT_NONLINMHE_JACOBIAN,
    hessian::Union{AbstractADType, Bool, Nothing} = false,
    direct = true,
    σP_0       = sigmaP_0,
    σQ         = sigmaQ,
    σR         = sigmaR,
    σPint_u_0  = sigmaPint_u_0,
    σQint_u    = sigmaQint_u,
    σPint_ym_0 = sigmaPint_ym_0,
    σQint_ym   = sigmaQint_ym,
) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel}
    # estimated covariances matrices (variance = σ²) :
    P̂_0 = Diagonal([σP_0; σPint_u_0; σPint_ym_0].^2)
    Q̂   = Diagonal([σQ;  σQint_u;  σQint_ym ].^2)
    R̂   = Diagonal([σR;].^2)
    isnothing(He) && throw(ArgumentError("Estimation horizon He must be explicitly specified")) 
    return MovingHorizonEstimator(
        model, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt;
        gc, gc!, nc, p, direct, optim, gradient, jacobian, hessian
    )
end

"Default optimizer for MHE, depending on the model and the number of custom NL constraints."
function default_optim_mhe(model::SimModel, nc)
    if model isa LinModel && iszero(nc)
        return JuMP.Model(DEFAULT_LINMHE_OPTIMIZER, add_bridges=false)
    else
        return JuMP.Model(DEFAULT_NONLINMHE_OPTIMIZER, add_bridges=false)
    end
end

@doc raw"""
    MovingHorizonEstimator(
        model, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt=Inf;
        gc!=(_,_,_,_,_,_,_,_,_,_,_) -> nothing,
        gc=gc!,
        nc=0,
        optim=default_optim_mhe(model, nc), 
        gradient=AutoForwardDiff(),
        jacobian=AutoForwardDiff(),
        hessian=false,
        direct=true,
        covestim=default_covestim_mhe(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct)
    )

Construct the estimator from the augmented covariance matrices `P̂_0`, `Q̂` and `R̂`.

This syntax allows nonzero off-diagonal elements in ``\mathbf{P̂_i}, \mathbf{Q̂, R̂}``,
where ``\mathbf{P̂_i}`` is the initial estimation covariance, provided by `P̂_0` argument. The
keyword argument `covestim` also allows specifying a custom [`StateEstimator`](@ref) object
for the estimation of covariance at the arrival ``\mathbf{P̂}_{k-N_k}(k-N_k+p)``. The
supported types are [`SteadyKalmanFilter`](@ref), [`KalmanFilter`](@ref), 
[`UnscentedKalmanFilter`](@ref) and [`ExtendedKalmanFilter`](@ref). A constant arrival
covariance is supported with [`SteadyKalmanFilter`](@ref), and by setting the `P̂` argument 
of [`setstate!`](@ref) at the desired value.
"""
function MovingHorizonEstimator(
    model::SM, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt=Inf;
    gc!::Function = (_,_,_,_,_,_,_,_,_,_,_) -> nothing,
    gc ::Function = gc!,
    nc = 0,
    p = model.p,
    optim::JM = default_optim_mhe(model, nc),
    gradient::AbstractADType = DEFAULT_NONLINMHE_GRADIENT,
    jacobian::AbstractADType = DEFAULT_NONLINMHE_JACOBIAN,
    hessian::Union{AbstractADType, Bool, Nothing} = false,
    direct = true,
    covestim::CE = default_covestim_mhe(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct)
) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel, CE<:StateEstimator{NT}}
    P̂_0, Q̂, R̂ = to_mat(P̂_0), to_mat(Q̂), to_mat(R̂)
    cov = KalmanCovariances(model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0, He)
    gc! = get_mutating_gc_mhe(NT, gc)
    hessian = validate_hessian(hessian, gradient, DEFAULT_NONLINMHE_HESSIAN)
    validate_covestim(cov, covestim)
    return MovingHorizonEstimator{NT}(
        model, 
        He, i_ym, nint_u, nint_ym, cov, Cwt,
        gc!, nc, p,
        optim, gradient, jacobian, hessian, covestim; 
        direct
    )
end

function default_covestim_mhe(model::LinModel, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct)
    return KalmanFilter(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct)
end
function default_covestim_mhe(model::SimModel, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct)
    return UnscentedKalmanFilter(model,  i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct)
end

"Validate covestim type and dimensions."
function validate_covestim(cov::KalmanCovariances, covestim::KalmanEstimator)
    if size(cov.P̂) != size(covestim.cov.P̂)
        throw(ArgumentError("estimation covariance covestim.cov.P̂ size does not match the MHE"))
    end
    return nothing
end
function validate_covestim(::KalmanCovariances, ::StateEstimator)
    error(  "covestim argument must be a SteadyKalmanFilter, KalmanFilter, "*
            "ExtendedKalmanFilter or UnscentedKalmanFilter")
end

"""
    validate_gc_mhe(NT, gc) -> ismutating

Validate `gc` function argument signature for MHE and return `true` if it is mutating.
"""
function validate_gc_mhe(NT, gc)
    ismutating = hasmethod(
        gc, 
        Tuple{
        #   LHS,      , X̂e        , V̂e         , Ŵe
            Vector{NT}, Vector{NT}, Vector{NT}, Vector{NT}, 
        #   Ue        , Yem       , De         , P̄                 , x̄         , p  , ε    
            Vector{NT}, Vector{NT}, Vector{NT}, AbstractMatrix{NT}, Vector{NT}, Any, NT
        }
    )
    isnonmutating = hasmethod(
        gc, 
        Tuple{
        #   X̂e        , V̂e        , Ŵe
            Vector{NT}, Vector{NT}, Vector{NT}, 
        #   Ue        , Yem       , De        , P̄                 , x̄         , p  , ε
            Vector{NT}, Vector{NT}, Vector{NT}, AbstractMatrix{NT}, Vector{NT}, Any, NT
        }
    )
    if !(ismutating || isnonmutating)
        error(
            "the custom constraint function has no method with type signature "*
            "gc(X̂e::Vector{$(NT)}, V̂e::Vector{$(NT)}, Ŵe::Vector{$(NT)}, "*
            "Ue::Vector{$(NT)}, Yem::Vector{$(NT)}, De::Vector{$(NT)}, "*
            "P̄::Vector{$(NT)}, x̄::Vector{$(NT)}, p::Any, ϵ::$(NT)) "*
            "or mutating form gc!(LHS::Vector{$(NT)}, "*
            "X̂e::Vector{$(NT)}, V̂e::Vector{$(NT)}, Ŵe::Vector{$(NT)}, "*
            "Ue::Vector{$(NT)}, Yem::Vector{$(NT)}, De::Vector{$(NT)}, "*
            "P̄::Vector{$(NT)}, x̄::Vector{$(NT)}, p::Any, ϵ::$(NT))"
        )
    end
    return ismutating
end

"Get mutating custom constraint function `gc!` from the provided function in argument."
function get_mutating_gc_mhe(NT, gc)
    ismutating_gc = validate_gc_mhe(NT, gc)
    gc! = if ismutating_gc
        gc
    else
        function gc!(LHS, X̂e, V̂e, Ŵe, Ue, Yem, De, P̄, x̄, p, ϵ)
            LHS .= gc(X̂e, V̂e, Ŵe, Ue, Yem, De, P̄, x̄, p, ϵ)
            return nothing
        end
    end
    return gc!
end

"""
    test_custom_function_mhe(NT, model::SimModel, i_ym, He, gc!, nc, x̂op, p, direct) -> nothing

Test the custom functions `gc!` at the operating points.

This function is called at the end of `MovingHorizonEstimator` construction. It warns the
user if the custom constraint `gc!` function crashes at `model` operating points. It
will also verify the custom function work with the growing windows, and with the `NaN` 
values at the boundaries (see [`MovingHorizonEstimator`](@ref) Extended Help for details on
the data windows). It should ease troubleshooting of simple bugs e.g.: the user forgets to
set the `nc` argument.
"""
function test_custom_function_mhe(NT, model::SimModel, i_ym, He, gc!, nc, x̂op, p, direct)
    nx̂, nŵ, nym = length(x̂op), length(x̂op), length(i_ym)
    nu, nd = model.nu, model.nd
    uop, dop, yop = model.uop, model.dop, model.yop
    yopm = yop[i_ym]
    X̂e_He, V̂e_He,  Ŵe_He = repeat(x̂op, He+1), zeros(NT, (He+1)*nym), zeros(NT, (He+1)*nŵ)
    Ue_He, Yem_He, De_He = repeat(uop, He+1), repeat(yopm, He+1),    repeat(dop, He+1)
    x̄ = zeros(NT, nx̂)
    P̄ = Hermitian(Matrix{NT}(I, nx̂, nx̂), :L)
    ε = zero(NT)
    gc = Vector{NT}(undef, nc) 
    try
        for i in 2:He+1
            X̂e, V̂e, Ŵe  = X̂e_He[1:(i*nx̂)], V̂e_He[1:(i*nym)],  Ŵe_He[1:(i*nŵ)]
            Ue, Yem, De = Ue_He[1:(i*nu)], Yem_He[1:(i*nym)], De_He[1:(i*nd)]
            if direct
                V̂e[1:nym] .= NaN
            else
                V̂e[end-nym+1:end]  .= NaN
                Yem[end-nym+1:end] .= NaN
                De[end-nd+1:end]   .= NaN
            end
            Ŵe[end-nŵ+1:end] .= NaN
            Ue[end-nu+1:end] .= NaN
            gc!(gc, X̂e, V̂e, Ŵe, Ue, Yem, De, P̄, x̄, p, ε)
            all(isfinite, gc) || error("the gc function returned non-finite values: gc = $gc")
        end
    catch err
        @warn(
            """
            Calling the gc function with X̂e, V̂e, Ŵe, Ue, Yem, De, P̄, x̄, ε arguments
            fixed at x̂op=$x̂op, uop=$uop, yop=$yop, dop=$dop, 
            P̄=I, x̄=0, p=$p, ϵ=0 failed with the following stacktrace. 
            Did you forget to set the keyword argument p or nc? 
            Did you handle the growing data windows with the NaN values at the boundaries?
            See the Extended Help of MovingHorizonEstimator for details on the arguments and
            the data windows.
            """, 
            exception=(err, catch_backtrace())
        )
    end
    return nothing
end

@doc raw"""
    setconstraint!(estim::MovingHorizonEstimator; <keyword arguments>) -> estim

Set the bound constraint parameters of the [`MovingHorizonEstimator`](@ref) `estim`.
   
It supports both soft and hard constraints on the estimated state ``\mathbf{x̂}``, process 
noise ``\mathbf{ŵ}`` and sensor noise ``\mathbf{v̂}``:
```math 
\begin{alignat*}{3}
    \mathbf{x̂_{min} - c_{x̂_{min}}} ε ≤&&\   \mathbf{x̂}_k(k-j+p) &≤ \mathbf{x̂_{max} + c_{x̂_{max}}} ε &&\qquad  j = N_k, N_k - 1, ... , 0    \\
    \mathbf{ŵ_{min} - c_{ŵ_{min}}} ε ≤&&\     \mathbf{ŵ}(k-j+p) &≤ \mathbf{ŵ_{max} + c_{ŵ_{max}}} ε &&\qquad  j = N_k, N_k - 1, ... , 1    \\
    \mathbf{v̂_{min} - c_{v̂_{min}}} ε ≤&&\     \mathbf{v̂}(k-j+1) &≤ \mathbf{v̂_{max} + c_{v̂_{max}}} ε &&\qquad  j = N_k, N_k - 1, ... , 1
\end{alignat*}
```
and also ``ε ≥ 0``. All the constraint parameters are vector. Use `±Inf` values when there
is no bound. The constraint softness parameters ``\mathbf{c}``, also called equal concern
for relaxation, are non-negative values that specify the softness of the associated bound.
Use `0.0` values for hard constraints (default for all of them). Notice that constraining
the estimated sensor noises is equivalent to bounding the innovation term, since 
``\mathbf{v̂}(k) = \mathbf{y^m}(k) - \mathbf{ŷ^m}(k)``. See Extended Help for details on
the constant ``p``, on model augmentation and on time-varying constraints.

# Arguments
!!! info
    All the keyword arguments have non-Unicode alternatives e.g. *`xhatmin`* or *`Vhatmax`*. 

    The default constraints are mentioned here for clarity but omitting a keyword argument 
    will not re-assign to its default value (defaults are set at construction only).

- `estim::MovingHorizonEstimator` : moving horizon estimator to set constraints
- `x̂min=fill(-Inf,nx̂)` / `x̂max=fill(+Inf,nx̂)` : estimated state bound ``\mathbf{x̂_{min/max}}``
- `ŵmin=fill(-Inf,nx̂)` / `ŵmax=fill(+Inf,nx̂)` : estimated process noise bound ``\mathbf{ŵ_{min/max}}``
- `v̂min=fill(-Inf,nym)` / `v̂max=fill(+Inf,nym)` : estimated sensor noise bound ``\mathbf{v̂_{min/max}}``
- `c_x̂min=fill(0.0,nx̂)` / `c_x̂max=fill(0.0,nx̂)` : `x̂min` / `x̂max` softness weight ``\mathbf{c_{x̂_{min/max}}}``
- `c_ŵmin=fill(0.0,nx̂)` / `c_ŵmax=fill(0.0,nx̂)` : `ŵmin` / `ŵmax` softness weight ``\mathbf{c_{ŵ_{min/max}}}``
- `c_v̂min=fill(0.0,nym)` / `c_v̂max=fill(0.0,nym)` : `v̂min` / `v̂max` softness weight ``\mathbf{c_{v̂_{min/max}}}``
-  all the keyword arguments above but with a first capital letter, e.g. `X̂max` or `C_ŵmax`:
   for time-varying constraints (see Extended Help)

# Examples
```jldoctest
julia> estim = MovingHorizonEstimator(LinModel(ss(0.5,1,1,0,1)), He=3);

julia> estim = setconstraint!(estim, x̂min=[-50, -50], x̂max=[50, 50])
MovingHorizonEstimator estimator with a sample time Ts = 1.0 s:
├ model: LinModel
├ optimizer: OSQP
├ arrival covariance: KalmanFilter
├ direct: true
└ dimensions:
  ├ 3 estimation steps He
  ├ 0 slack variable ε (estimation constraints)
  ├ 1 manipulated inputs u (0 integrating states)
  ├ 2 estimated states x̂
  ├ 1 measured outputs ym (1 integrating states)
  ├ 0 unmeasured outputs yu
  └ 0 measured disturbances d
```

# Extended Help
!!! details "Extended Help"
    The constant ``p=0`` if `estim.direct==true` (current form), else ``p=1`` (prediction
    form). Note that the state ``\mathbf{x̂}`` and process noise ``\mathbf{ŵ}`` constraints
    are applied on the augmented model, detailed in [`SteadyKalmanFilter`](@ref) Extended
    Help. For variable constraints, the bounds can be modified after calling [`updatestate!`](@ref),
    that is, at runtime, except for `±Inf` bounds. Time-varying constraints over the
    estimation horizon ``H_e`` are also possible, mathematically defined as:
    ```math 
    \begin{alignat*}{3}
        \mathbf{X̂_{min} - C_{x̂_{min}}} ε ≤&&\ \mathbf{X̂} &≤ \mathbf{X̂_{max} + C_{x̂_{max}}} ε \\
        \mathbf{Ŵ_{min} - C_{ŵ_{min}}} ε ≤&&\ \mathbf{Ŵ} &≤ \mathbf{Ŵ_{max} + C_{ŵ_{max}}} ε \\
        \mathbf{V̂_{min} - C_{v̂_{min}}} ε ≤&&\ \mathbf{V̂} &≤ \mathbf{V̂_{max} + C_{v̂_{max}}} ε
    \end{alignat*}
    ```
    For this, use the same keyword arguments as above but with a first capital letter:
    - `X̂min` / `X̂max` / `C_x̂min` / `C_x̂max` : ``\mathbf{X̂}`` constraints `(nx̂*(He+1),)`.
    - `Ŵmin` / `Ŵmax` / `C_ŵmin` / `C_ŵmax` : ``\mathbf{Ŵ}`` constraints `(nx̂*He,)`.
    - `V̂min` / `V̂max` / `C_v̂min` / `C_v̂max` : ``\mathbf{V̂}`` constraints `(nym*He,)`.
"""
function setconstraint!(
    estim::MovingHorizonEstimator; 
    xhatmin   = nothing, xhatmax   = nothing,
    whatmin   = nothing, whatmax   = nothing,
    vhatmin   = nothing, vhatmax   = nothing,
    c_xhatmin = nothing, c_xhatmax = nothing,
    c_whatmin = nothing, c_whatmax = nothing,
    c_vhatmin = nothing, c_vhatmax = nothing,
    Xhatmin   = nothing, Xhatmax   = nothing,
    Whatmin   = nothing, Whatmax   = nothing,
    Vhatmin   = nothing, Vhatmax   = nothing,
    C_xhatmin = nothing, C_xhatmax = nothing,
    C_whatmin = nothing, C_whatmax = nothing,
    C_vhatmin = nothing, C_vhatmax = nothing,
    x̂min   = xhatmin,   x̂max   = xhatmax,
    ŵmin   = whatmin,   ŵmax   = whatmax,
    v̂min   = vhatmin,   v̂max   = vhatmax,
    c_x̂min = c_xhatmin, c_x̂max = c_xhatmax,
    c_ŵmin = c_whatmin, c_ŵmax = c_whatmax,
    c_v̂min = c_vhatmin, c_v̂max = c_vhatmax,
    X̂min   = Xhatmin,   X̂max   = Xhatmax,
    Ŵmin   = Whatmin,   Ŵmax   = Whatmax,
    V̂min   = Vhatmin,   V̂max   = Vhatmax,
    C_x̂min = C_xhatmin, C_x̂max = C_xhatmax,
    C_ŵmin = C_whatmin, C_ŵmax = C_whatmax,
    C_v̂min = C_vhatmin, C_v̂max = C_vhatmax,
)
    model, optim, con = estim.model, estim.optim, estim.con
    nx̂, nŵ, nym, He = estim.nx̂, estim.nx̂, estim.nym, estim.He
    nX̂con = nx̂*(He+1)
    notSolvedYet = (JuMP.termination_status(optim) == JuMP.OPTIMIZE_NOT_CALLED)
    C = estim.C
    if isnothing(X̂min) && !isnothing(x̂min)
        size(x̂min) == (nx̂,) || throw(DimensionMismatch("x̂min size must be $((nx̂,))"))
        con.x̃0min[end-nx̂+1:end] .= x̂min .- estim.x̂op # if C is finite : x̃ = [ε; x̂]
        for i in 1:nx̂*He
            con.X̂0min[i] = x̂min[(i-1) % nx̂ + 1] - estim.X̂op[i]
        end
    elseif !isnothing(X̂min)
        size(X̂min) == (nX̂con,) || throw(DimensionMismatch("X̂min size must be $((nX̂con,))"))
        con.x̃0min[end-nx̂+1:end] .= X̂min[1:nx̂] .- estim.x̂op
        con.X̂0min .= @views X̂min[nx̂+1:end] .- estim.X̂op
    end
    if isnothing(X̂max) && !isnothing(x̂max)
        size(x̂max) == (nx̂,) || throw(DimensionMismatch("x̂max size must be $((nx̂,))"))
        con.x̃0max[end-nx̂+1:end] .= x̂max .- estim.x̂op # if C is finite : x̃ = [ε; x̂]
        for i in 1:nx̂*He
            con.X̂0max[i] = x̂max[(i-1) % nx̂ + 1] - estim.X̂op[i]
        end
    elseif !isnothing(X̂max)
        size(X̂max) == (nX̂con,) || throw(DimensionMismatch("X̂max size must be $((nX̂con,))"))
        con.x̃0max[end-nx̂+1:end] .= X̂max[1:nx̂] .- estim.x̂op
        con.X̂0max .= @views X̂max[nx̂+1:end] .- estim.X̂op
    end
    if isnothing(Ŵmin) && !isnothing(ŵmin)
        size(ŵmin) == (nŵ,) || throw(DimensionMismatch("ŵmin size must be $((nŵ,))"))
        for i in 1:nŵ*He
            con.Ŵmin[i] = ŵmin[(i-1) % nŵ + 1]
        end
    elseif !isnothing(Ŵmin)
        size(Ŵmin) == (nŵ*He,) || throw(DimensionMismatch("Ŵmin size must be $((nŵ*He,))"))
        con.Ŵmin .= Ŵmin
    end
    if isnothing(Ŵmax) && !isnothing(ŵmax)
        size(ŵmax) == (nŵ,) || throw(DimensionMismatch("ŵmax size must be $((nŵ,))"))
        for i in 1:nŵ*He
            con.Ŵmax[i] = ŵmax[(i-1) % nŵ + 1]
        end
    elseif !isnothing(Ŵmax)
        size(Ŵmax) == (nŵ*He,) || throw(DimensionMismatch("Ŵmax size must be $((nŵ*He,))"))
        con.Ŵmax .= Ŵmax
    end
    if isnothing(V̂min) && !isnothing(v̂min)
        size(v̂min) == (nym,) || throw(DimensionMismatch("v̂min size must be $((nym,))"))
        for i in 1:nym*He
            con.V̂min[i] = v̂min[(i-1) % nym + 1]
        end
    elseif !isnothing(V̂min)
        size(V̂min) == (nym*He,) || throw(DimensionMismatch("V̂min size must be $((nym*He,))"))
        con.V̂min .= V̂min
    end
    if isnothing(V̂max) && !isnothing(v̂max)
        size(v̂max) == (nym,) || throw(DimensionMismatch("v̂max size must be $((nym,))"))
        for i in 1:nym*He
            con.V̂max[i] = v̂max[(i-1) % nym + 1]
        end
    elseif !isnothing(V̂max)
        size(V̂max) == (nym*He,) || throw(DimensionMismatch("V̂max size must be $((nym*He,))"))
        con.V̂max .= V̂max
    end
    allECRs = (
        c_x̂min, c_x̂max, c_ŵmin, c_ŵmax, c_v̂min, c_v̂max,
        C_x̂min, C_x̂max, C_ŵmin, C_ŵmax, C_v̂min, C_v̂max,
    )
    if any(ECR -> !isnothing(ECR), allECRs)
        !isinf(C) || throw(ArgumentError("Slack variable weight Cwt must be finite to set softness parameters"))
        notSolvedYet || error("Cannot set softness parameters after calling updatestate!")
    end
    if notSolvedYet
        isnothing(C_x̂min) && !isnothing(c_x̂min) && (C_x̂min = repeat(c_x̂min, He+1))
        isnothing(C_x̂max) && !isnothing(c_x̂max) && (C_x̂max = repeat(c_x̂max, He+1))
        isnothing(C_ŵmin) && !isnothing(c_ŵmin) && (C_ŵmin = repeat(c_ŵmin, He))
        isnothing(C_ŵmax) && !isnothing(c_ŵmax) && (C_ŵmax = repeat(c_ŵmax, He))
        isnothing(C_v̂min) && !isnothing(c_v̂min) && (C_v̂min = repeat(c_v̂min, He))
        isnothing(C_v̂max) && !isnothing(c_v̂max) && (C_v̂max = repeat(c_v̂max, He))
        if !isnothing(C_x̂min)
            size(C_x̂min) == (nX̂con,) || throw(DimensionMismatch("C_x̂min size must be $((nX̂con,))"))
            any(C_x̂min .< 0) && error("C_x̂min weights should be non-negative")
            # if C is finite : x̃ = [ε; x̂] 
            con.A_x̃min[end-nx̂+1:end, end] .= @views -C_x̂min[1:nx̂] 
            con.C_x̂min .= @views C_x̂min[nx̂+1:end]
            size(con.A_X̂min, 1) ≠ 0 && (con.A_X̂min[:, end] = -con.C_x̂min) # for LinModel
        end
        if !isnothing(C_x̂max)
            size(C_x̂max) == (nX̂con,) || throw(DimensionMismatch("C_x̂max size must be $((nX̂con,))"))
            any(C_x̂max .< 0) && error("C_x̂max weights should be non-negative")
            # if C is finite : x̃ = [ε; x̂] :
            con.A_x̃max[end-nx̂+1:end, end] .= @views -C_x̂max[1:nx̂]
            con.C_x̂max .= @views C_x̂max[nx̂+1:end]
            size(con.A_X̂max, 1) ≠ 0 && (con.A_X̂max[:, end] = -con.C_x̂max) # for LinModel
        end
        if !isnothing(C_ŵmin)
            size(C_ŵmin) == (nŵ*He,) || throw(DimensionMismatch("C_ŵmin size must be $((nŵ*He,))"))
            any(C_ŵmin .< 0) && error("C_ŵmin weights should be non-negative")
            con.A_Ŵmin[:, end] .= -C_ŵmin
        end
        if !isnothing(C_ŵmax)
            size(C_ŵmax) == (nŵ*He,) || throw(DimensionMismatch("C_ŵmax size must be $((nŵ*He,))"))
            any(C_ŵmax .< 0) && error("C_ŵmax weights should be non-negative")
            con.A_Ŵmax[:, end] .= -C_ŵmax
        end
        if !isnothing(C_v̂min)
            size(C_v̂min) == (nym*He,) || throw(DimensionMismatch("C_v̂min size must be $((nym*He,))"))
            any(C_v̂min .< 0) && error("C_v̂min weights should be non-negative")
            con.C_v̂min .= C_v̂min
            size(con.A_V̂min, 1) ≠ 0 && (con.A_V̂min[:, end] = -con.C_v̂min) # for LinModel
        end
        if !isnothing(C_v̂max)
            size(C_v̂max) == (nym*He,) || throw(DimensionMismatch("C_v̂max size must be $((nym*He,))"))
            any(C_v̂max .< 0) && error("C_v̂max weights should be non-negative")
            con.C_v̂max .= C_v̂max
            size(con.A_V̂max, 1) ≠ 0 && (con.A_V̂max[:, end] = -con.C_v̂max) # for LinModel
        end
    end
    i_x̃min, i_x̃max  = .!isinf.(con.x̃0min), .!isinf.(con.x̃0max)
    i_X̂min, i_X̂max  = .!isinf.(con.X̂0min), .!isinf.(con.X̂0max)
    i_Ŵmin, i_Ŵmax  = .!isinf.(con.Ŵmin),  .!isinf.(con.Ŵmax)
    i_V̂min, i_V̂max  = .!isinf.(con.V̂min),  .!isinf.(con.V̂max)
    if notSolvedYet
        con.i_b[:], con.i_g[:], con.A[:] = init_matconstraint_mhe(
            model, con.nc,
            i_x̃min, i_x̃max, i_X̂min, i_X̂max, i_Ŵmin, i_Ŵmax, i_V̂min, i_V̂max,
            con.A_x̃min, con.A_x̃max, con.A_X̂min, con.A_X̂max, 
            con.A_Ŵmin, con.A_Ŵmax, con.A_V̂min, con.A_V̂max
        )
        A = con.A[con.i_b, :]
        b = zeros(count(con.i_b)) # dummy value, updated before optimization (avoid ±Inf)
        Z̃var = optim[:Z̃var]
        JuMP.delete(optim, optim[:linconstraint])
        JuMP.unregister(optim, :linconstraint)
        @constraint(optim, linconstraint, A*Z̃var .≤ b)
        reset_nonlincon!(estim, model)
    else
        i_b, i_g = init_matconstraint_mhe(
            model, con.nc,
            i_x̃min, i_x̃max, i_X̂min, i_X̂max, i_Ŵmin, i_Ŵmax, i_V̂min, i_V̂max
        )
        if i_b ≠ con.i_b || i_g ≠ con.i_g
            error("Cannot modify ±Inf constraints after first solve of estimation problem")
        end
    end
    return estim
end

"By default, no nonlinear constraints or only custom ones, do and return nothing."
reset_nonlincon!(::MovingHorizonEstimator, ::SimModel) = nothing

"""
    reset_nonlincon!(estim::MovingHorizonEstimator, model::NonLinModel)

Re-construct nonlinear constraints and add them to `estim.optim`.
"""
function reset_nonlincon!(estim::MovingHorizonEstimator, model::NonLinModel)
    g_oracle = get_nonlincon_oracle(estim, estim.optim)
    set_nonlincon!(estim, estim.optim, g_oracle)
end

@doc raw"""
    init_matconstraint_mhe(
        model::LinModel, nc::Int,
        i_x̃min, i_x̃max, i_X̂min, i_X̂max, i_Ŵmin, i_Ŵmax, i_V̂min, i_V̂max, args...
    ) -> i_b, i_g, A

Init `i_b`, `i_g` and `A` matrices for the MHE linear inequality constraints.

The linear and nonlinear inequality constraints are respectively defined as:
```math
\begin{aligned} 
    \mathbf{A Z̃ } &≤ \mathbf{b} \\ 
    \mathbf{g(Z̃)} &≤ \mathbf{0}
\end{aligned}
```
The argument `nc` is the number of custom nonlinear inequality constraints in
``\mathbf{g_c}``. `i_b` is a `BitVector` including the indices of ``\mathbf{b}`` that are
finite numbers. `i_g` is a similar vector but for the indices of ``\mathbf{g}`` (empty if
`model` is a [`LinModel`](@ref)). The method also returns the ``\mathbf{A}`` matrix if
`args` is provided. In such a case, `args`  needs to contain all the inequality constraint
matrices: `A_x̃min, A_x̃max, A_X̂min, A_X̂max, A_Ŵmin, A_Ŵmax, A_V̂min, A_V̂max`.
"""
function init_matconstraint_mhe(
    ::LinModel{NT}, nc::Int,
    i_x̃min, i_x̃max, i_X̂min, i_X̂max, i_Ŵmin, i_Ŵmax, i_V̂min, i_V̂max, args...
) where {NT<:Real}
    i_b = [i_x̃min; i_x̃max; i_X̂min; i_X̂max; i_Ŵmin; i_Ŵmax; i_V̂min; i_V̂max]
    i_g = trues(nc)
    if isempty(args)
        A = zeros(NT, length(i_b), 0)
    else
        A_x̃min, A_x̃max, A_X̂min, A_X̂max, A_Ŵmin, A_Ŵmax, A_V̂min, A_V̂max = args
        A = [A_x̃min; A_x̃max; A_X̂min; A_X̂max; A_Ŵmin; A_Ŵmax; A_V̂min; A_V̂max]
    end
    return i_b, i_g, A
end

"Init `i_b, A` without state and sensor noise constraints if `model` is not a [`LinModel`](@ref)."
function init_matconstraint_mhe(
    ::SimModel{NT}, nc::Int,
    i_x̃min, i_x̃max, i_X̂min, i_X̂max, i_Ŵmin, i_Ŵmax, i_V̂min, i_V̂max, args...
) where {NT<:Real}
    i_b = [i_x̃min; i_x̃max; i_Ŵmin; i_Ŵmax]
    i_g = [i_X̂min; i_X̂max; i_V̂min; i_V̂max; trues(nc)]
    if isempty(args)
        A = zeros(NT, length(i_b), 0)
    else
        A_x̃min, A_x̃max, _ , _ , A_Ŵmin, A_Ŵmax, _ , _ = args
        A = [A_x̃min; A_x̃max; A_Ŵmin; A_Ŵmax]
    end
    return i_b, i_g, A
end

"""
    init_defaultcon_mhe(
        model::SimModel, He, C, nx̂, nym, E, ex̄, Ex̂, Fx̂, Gx̂, Jx̂, Bx̂
    ) -> con, Ẽ, ẽx̄

    Init `EstimatatorConstraint` struct with default parameters based on model `model`.

Also return `Ẽ` and `ẽx̄` matrices for the the augmented decision vector `Z̃`.
"""
function init_defaultcon_mhe(
    model::SimModel{NT}, He, C, nx̂, nym, E, ex̄, Ex̂, Fx̂, Gx̂, Jx̂, Bx̂, 
    gc!::GCfunc = nothing, nc = 0
) where {NT<:Real, GCfunc<:Union{Nothing, Function}}
    nŵ = nx̂
    nZ̃, nX̂, nŴ, nYm = nx̂+nŵ*He, nx̂*He, nŵ*He, nym*He
    nε = isinf(C) ? 0 : 1
    x̂min, x̂max = fill(convert(NT,-Inf), nx̂),  fill(convert(NT,+Inf), nx̂)
    X̂min, X̂max = fill(convert(NT,-Inf), nX̂),  fill(convert(NT,+Inf), nX̂)
    Ŵmin, Ŵmax = fill(convert(NT,-Inf), nŴ),  fill(convert(NT,+Inf), nŴ)
    V̂min, V̂max = fill(convert(NT,-Inf), nYm), fill(convert(NT,+Inf), nYm)
    c_x̂min, c_x̂max = fill(0.0, nx̂),  fill(0.0, nx̂)
    C_x̂min, C_x̂max = fill(0.0, nX̂),  fill(0.0, nX̂)
    C_ŵmin, C_ŵmax = fill(0.0, nŴ),  fill(0.0, nŴ)
    C_v̂min, C_v̂max = fill(0.0, nYm), fill(0.0, nYm)
    A_x̃min, A_x̃max, x̃min, x̃max, ẽx̄ = relaxarrival(model, nε, c_x̂min, c_x̂max, x̂min, x̂max, ex̄)
    A_X̂min, A_X̂max, Ẽx̂ = relaxX̂(model, nε, C_x̂min, C_x̂max, Ex̂)
    A_Ŵmin, A_Ŵmax = relaxŴ(model, nε, C_ŵmin, C_ŵmax, nx̂)
    A_V̂min, A_V̂max, Ẽ = relaxV̂(model, nε, C_v̂min, C_v̂max, E)
    i_x̃min, i_x̃max = .!isinf.(x̃min), .!isinf.(x̃max)
    i_X̂min, i_X̂max = .!isinf.(X̂min), .!isinf.(X̂max)
    i_Ŵmin, i_Ŵmax = .!isinf.(Ŵmin), .!isinf.(Ŵmax)
    i_V̂min, i_V̂max = .!isinf.(V̂min), .!isinf.(V̂max)
    i_b, i_g, A = init_matconstraint_mhe(
        model, nc,
        i_x̃min, i_x̃max, i_X̂min, i_X̂max, i_Ŵmin, i_Ŵmax, i_V̂min, i_V̂max,
        A_x̃min, A_x̃max, A_X̂min, A_X̂max, A_Ŵmin, A_Ŵmax, A_V̂min, A_V̂max
    )
    b = zeros(NT, size(A, 1)) # dummy b vector (updated just before optimization)
    con = EstimatorConstraint{NT, GCfunc}(
        Ẽx̂, Fx̂, Gx̂, Jx̂, Bx̂,
        x̃min, x̃max, X̂min, X̂max, Ŵmin, Ŵmax, V̂min, V̂max,
        A_x̃min, A_x̃max, A_X̂min, A_X̂max, A_Ŵmin, A_Ŵmax, A_V̂min, A_V̂max,
        A, b,
        C_x̂min, C_x̂max, C_v̂min, C_v̂max,
        i_b, i_g,
        gc!, nc
    )
    return con, nε, Ẽ, ẽx̄
end

@doc raw"""
    relaxarrival(
        model::SimModel, nε, c_x̂min, c_x̂max, x̂min, x̂max, ex̄
    ) -> A_x̃min, A_x̃max, x̃min, x̃max, ẽx̄

Augment arrival state constraints with slack variable ε for softening the MHE.

Denoting the MHE decision variable augmented with the slack variable ``\mathbf{Z̃} = 
[\begin{smallmatrix} ε \\ \mathbf{Z} \end{smallmatrix}]``, it returns the ``\mathbf{ẽ_x̄}``
matrix that appears in the estimation error at arrival equation ``\mathbf{x̄} =
\mathbf{ẽ_x̄ Z̃ + f_x̄}``. It also returns the augmented constraints ``\mathbf{x̃_{min}}`` and
``\mathbf{x̃_{max}}``, and the ``\mathbf{A}`` matrices for the inequality constraints:
```math
\begin{bmatrix} 
    \mathbf{A_{x̃_{min}}} \\ 
    \mathbf{A_{x̃_{max}}}
\end{bmatrix} \mathbf{Z̃} ≤
\begin{bmatrix}
    - \mathbf{(x̃_{min} - x̃_{op})} \\
    + \mathbf{(x̃_{max} - x̃_{op})}
\end{bmatrix}
```
in which
``\mathbf{x̃_{min}} = [\begin{smallmatrix} 0 \\ \mathbf{x̂_{min}} \end{smallmatrix}]``, 
``\mathbf{x̃_{max}} = [\begin{smallmatrix} ∞ \\ \mathbf{x̂_{max}} \end{smallmatrix}]`` and
``\mathbf{x̃_{op}}  = [\begin{smallmatrix} 0 \\ \mathbf{x̂_{op}}  \end{smallmatrix}]``
"""
function relaxarrival(::SimModel{NT}, nε, c_x̂min, c_x̂max, x̂min, x̂max, ex̄) where {NT<:Real}
    ex̂ = -ex̄
    if nε ≠ 0 # Z̃ = [ε; Z]
        x̃min, x̃max = [NT[0.0]; x̂min], [NT[Inf]; x̂max]
        A_ε = [NT[1.0] zeros(NT, 1, size(ex̂, 2))]
        # ε impacts arrival state constraint calculations:
        A_x̃min, A_x̃max = -[A_ε; c_x̂min ex̂], [A_ε; -c_x̂max ex̂]
        # ε has no impact on estimation error at arrival:
        ẽx̄ = [zeros(NT, size(ex̄, 1), 1) ex̄] 
    else # Z̃ = Z (only hard constraints)
        x̃min, x̃max = x̂min, x̂max
        ẽx̄ = ex̄
        A_x̃min, A_x̃max = -ex̂, ex̂
    end
    return A_x̃min, A_x̃max, x̃min, x̃max, ẽx̄
end

@doc raw"""
    relaxX̂(model::SimModel, nε, C_x̂min, C_x̂max, Ex̂) -> A_X̂min, A_X̂max, Ẽx̂

Augment estimated state constraints with slack variable ε for softening the MHE.

Denoting the MHE decision variable augmented with the slack variable ``\mathbf{Z̃} = 
[\begin{smallmatrix} ε \\ \mathbf{Z} \end{smallmatrix}]``, it returns the ``\mathbf{Ẽ_x̂}``
matrix that appears in estimated states equation ``\mathbf{X̂} = \mathbf{Ẽ_x̂ Z̃ + F_x̂}``. It
also returns the ``\mathbf{A}`` matrices for the inequality constraints:
```math
\begin{bmatrix} 
    \mathbf{A_{X̂_{min}}} \\ 
    \mathbf{A_{X̂_{max}}}
\end{bmatrix} \mathbf{Z̃} ≤
\begin{bmatrix}
    - \mathbf{(X̂_{min} - X̂_{op}) + F_x̂} \\
    + \mathbf{(X̂_{max} - X̂_{op}) - F_x̂}
\end{bmatrix}
```
in which ``\mathbf{X̂_{min}, X̂_{max}}`` and ``\mathbf{X̂_{op}}`` vectors respectively contains
``\mathbf{x̂_{min}, x̂_{max}}`` and ``\mathbf{x̂_{op}}`` repeated ``H_e`` times.
"""
function relaxX̂(::LinModel{NT}, nε, C_x̂min, C_x̂max, Ex̂) where {NT<:Real}
    if nε ≠ 0 # Z̃ = [ε; Z]
        # ε impacts estimated process noise constraint calculations:
        A_X̂min, A_X̂max = -[C_x̂min Ex̂], [-C_x̂max Ex̂]
        # ε has no impact on estimated process noises:
        Ẽx̂ = [zeros(NT, size(Ex̂, 1), 1) Ex̂] 
    else # Z̃ = Z (only hard constraints)
        Ẽx̂ = Ex̂
        A_X̂min, A_X̂max = -Ex̂, Ex̂
    end
    return A_X̂min, A_X̂max, Ẽx̂
end

"Return empty matrices if model is not a [`LinModel`](@ref)"
function relaxX̂(::SimModel{NT}, nε, C_x̂min, C_x̂max, Ex̂) where {NT<:Real}
    Ẽx̂ = [zeros(NT, 0, nε) Ex̂]
    A_X̂min, A_X̂max = -Ẽx̂,  Ẽx̂
    return A_X̂min, A_X̂max, Ẽx̂
end

@doc raw"""
    relaxŴ(model::SimModel, nε, C_ŵmin, C_ŵmax, nx̂) -> A_Ŵmin, A_Ŵmax

Augment estimated process noise constraints with slack variable ε for softening the MHE.

Denoting the MHE decision variable augmented with the slack variable ``\mathbf{Z̃} = 
[\begin{smallmatrix} ε \\ \mathbf{Z} \end{smallmatrix}]``, it returns the ``\mathbf{A}`` 
matrices for the inequality constraints:
```math
\begin{bmatrix} 
    \mathbf{A_{Ŵ_{min}}} \\ 
    \mathbf{A_{Ŵ_{max}}}
\end{bmatrix} \mathbf{Z̃} ≤
\begin{bmatrix}
    - \mathbf{Ŵ_{min}} \\
    + \mathbf{Ŵ_{max}}
\end{bmatrix}
```
"""
function relaxŴ(::SimModel{NT}, nε, C_ŵmin, C_ŵmax, nx̂) where {NT<:Real}
    A = [zeros(NT, length(C_ŵmin), nx̂) I]
    if nε ≠ 0 # Z̃ = [ε; Z]
        A_Ŵmin, A_Ŵmax = -[C_ŵmin A], [-C_ŵmax A]
    else # Z̃ = Z (only hard constraints)
        A_Ŵmin, A_Ŵmax = -A, A
    end
    return A_Ŵmin, A_Ŵmax
end

@doc raw"""
    relaxV̂(model::SimModel, nε, C_v̂min, C_v̂max, E) -> A_V̂min, A_V̂max, Ẽ

Augment estimated sensor noise constraints with slack variable ε for softening the MHE.

Denoting the MHE decision variable augmented with the slack variable ``\mathbf{Z̃} = 
[\begin{smallmatrix} ε \\ \mathbf{Z} \end{smallmatrix}]``, it returns the ``\mathbf{Ẽ}``
matrix that appears in estimated sensor noise equation ``\mathbf{V̂} = \mathbf{Ẽ Z̃ + F}``. It
also returns the ``\mathbf{A}`` matrices for the inequality constraints:
```math
\begin{bmatrix} 
    \mathbf{A_{V̂_{min}}} \\ 
    \mathbf{A_{V̂_{max}}}
\end{bmatrix} \mathbf{Z̃} ≤
\begin{bmatrix}
    - \mathbf{V̂_{min} + F} \\
    + \mathbf{V̂_{max} - F}
\end{bmatrix}
```
"""
function relaxV̂(::LinModel{NT}, nε, C_v̂min, C_v̂max, E) where {NT<:Real}
    if nε ≠ 0 # Z̃ = [ε; Z]
        # ε impacts estimated sensor noise constraint calculations:
        A_V̂min, A_V̂max = -[C_v̂min E], [-C_v̂max E]
        # ε has no impact on estimated sensor noises:
        Ẽ = [zeros(NT, size(E, 1), 1) E] 
    else # Z̃ = Z (only hard constraints)
        Ẽ = E
        A_V̂min, A_V̂max = -Ẽ, Ẽ
    end
    return A_V̂min, A_V̂max, Ẽ
end

"Return empty matrices if model is not a [`LinModel`](@ref)"
function relaxV̂(::SimModel{NT}, nε, C_v̂min, C_v̂max, E) where {NT<:Real}
    Ẽ = [zeros(NT, 0, nε) E]
    A_V̂min, A_V̂max = -Ẽ, Ẽ
    return A_V̂min, A_V̂max, Ẽ
end

@doc raw"""
    init_predmat_mhe(
        model::LinModel, He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, direct
    ) -> E, G, J, B, ex̄, Ex̂, Gx̂, Jx̂, Bx̂

Construct the [`MovingHorizonEstimator`](@ref) prediction matrices for [`LinModel`](@ref) `model`.

We first introduce the deviation vector of the estimated state at arrival 
``\mathbf{x̂_0}(k-N_k+p) = \mathbf{x̂}_k(k-N_k+p) - \mathbf{x̂_{op}}`` (see [`setop!`](@ref)),
and the vector ``\mathbf{Z} = [\begin{smallmatrix} \mathbf{x̂_0}(k-N_k+p)
\\ \mathbf{Ŵ} \end{smallmatrix}]`` with the decision variables. Setting the constant ``p=0``
produces an estimator in the current form, while the prediction form is obtained with
``p=1``. The estimated sensor noises from time ``k-N_k+1`` to ``k`` are computed by:
```math
\begin{aligned}
    \mathbf{V̂} = \mathbf{Y_0^m - Ŷ_0^m} &= \mathbf{E Z + G U_0 + J D_0 + Y_0^m + B}     \\
                                        &= \mathbf{E Z + F}
\end{aligned}
```
in which ``\mathbf{U_0}`` and ``\mathbf{Y_0^m}`` respectively include the deviation values
of the manipulated inputs ``\mathbf{u_0}(k-j+p)`` from ``j=N_k`` to ``1`` and measured
outputs ``\mathbf{y_0^m}(k-j+1)`` from ``j=N_k`` to ``1``. The vector ``\mathbf{D_0}``
includes the the measured disturbance deviation values ``\mathbf{d_0}(k-j)`` from from
``j=N_k`` to ``0``. The constant ``\mathbf{B}`` is the contribution for non-zero state
``\mathbf{x̂_{op}}`` and state update ``\mathbf{f̂_{op}}`` operating points (for linearization,
see [`augment_model`](@ref) and [`linearize`](@ref)). The method also returns the matrices
for the estimation error at arrival:
```math
    \mathbf{x̄} = \mathbf{x̂_0^†}(k-N_k+p) - \mathbf{x̂_0}(k-N_k+p) = \mathbf{e_x̄ Z + f_x̄}
```
in which ``\mathbf{e_x̄} = [\begin{smallmatrix} -\mathbf{I} & \mathbf{0} & \cdots & \mathbf{0} \end{smallmatrix}]``,
and ``\mathbf{f_x̄} = \mathbf{x̂_0^†}(k-N_k+p)``. The latter is the deviation vector of the
state at arrival, estimated at time ``k-N_k``, i.e. ``\mathbf{x̂_0^†}(k-N_k+p) = 
\mathbf{x̂}_{k-N_k}(k-N_k+p) - \mathbf{x̂_{op}}``. Lastly, the estimates ``\mathbf{x̂_0}(k-j+p)``
from ``j=N_k-1`` to ``0``, also in deviation form, are computed with:
```math
\begin{aligned}
    \mathbf{X̂_0}  &= \mathbf{E_x̂ Z + G_x̂ U_0 + J_x̂ D_0 + B_x̂} \\
                  &= \mathbf{E_x̂ Z + F_x̂}
\end{aligned}
```
The matrices ``\mathbf{E, G, J, B, E_x̂, G_x̂, J_x̂, B_x̂}`` are defined in the Extended Help 
section. The vectors ``\mathbf{F, F_x̂, f_x̄}`` are recalculated at each discrete time step, 
see [`initpred!(::MovingHorizonEstimator, ::LinModel)`](@ref) and [`linconstraint!(::MovingHorizonEstimator, ::LinModel)`](@ref).

# Extended Help
!!! details "Extended Help"
    Using the augmented process model matrices ``\mathbf{Â, B̂_u, Ĉ^m, B̂_d, D̂_d^m}``, and the
    function ``\mathbf{S}(j) = ∑_{i=0}^j \mathbf{Â}^i``, the prediction matrices for the
    sensor noises depend on the constant ``p``. For ``p=0``, the matrices are computed by
    (notice the minus signs after the equalities):
    ```math
    \begin{aligned}
    \mathbf{E} &= - \begin{bmatrix}
        \mathbf{Ĉ^m}\mathbf{Â}^{1}                  & \mathbf{Ĉ^m}\mathbf{Â}^{0}                    & \cdots & \mathbf{0}                               \\ 
        \mathbf{Ĉ^m}\mathbf{Â}^{2}                  & \mathbf{Ĉ^m}\mathbf{Â}^{1}                    & \cdots & \mathbf{0}                               \\ 
        \vdots                                      & \vdots                                        & \ddots & \vdots                                   \\
        \mathbf{Ĉ^m}\mathbf{Â}^{H_e}                & \mathbf{Ĉ^m}\mathbf{Â}^{H_e-1}                & \cdots & \mathbf{Ĉ^m}\mathbf{Â}^{0}               \end{bmatrix} \\
    \mathbf{G} &= - \begin{bmatrix}
        \mathbf{Ĉ^m}\mathbf{Â}^{0}\mathbf{B̂_u}      & \mathbf{0}                                    & \cdots & \mathbf{0}                               \\ 
        \mathbf{Ĉ^m}\mathbf{Â}^{1}\mathbf{B̂_u}      & \mathbf{Ĉ^m}\mathbf{Â}^{0}\mathbf{B̂_u}        & \cdots & \mathbf{0}                               \\ 
        \vdots                                      & \vdots                                        & \ddots & \vdots                                   \\
        \mathbf{Ĉ^m}\mathbf{Â}^{H_e-1}\mathbf{B̂_u}  & \mathbf{Ĉ^m}\mathbf{Â}^{H_e-2}\mathbf{B̂_u}    & \cdots & \mathbf{Ĉ^m}\mathbf{Â}^{0}\mathbf{B̂_u}   \end{bmatrix} \\
    \mathbf{J} &= - \begin{bmatrix}
        \mathbf{Ĉ^m}\mathbf{Â}^{0}\mathbf{B̂_d}      & \mathbf{D̂_d^m}                                & \cdots & \mathbf{0}                               \\ 
        \mathbf{Ĉ^m}\mathbf{Â}^{1}\mathbf{B̂_d}      & \mathbf{Ĉ^m}\mathbf{Â}^{0}\mathbf{B̂_d}        & \cdots & \mathbf{0}                               \\ 
        \vdots                                      & \vdots                                        & \ddots & \vdots                                   \\
        \mathbf{Ĉ^m}\mathbf{Â}^{H_e-1}\mathbf{B̂_d}  & \mathbf{Ĉ^m}\mathbf{Â}^{H_e-2}\mathbf{B̂_d}    & \cdots & \mathbf{D̂_d^m}                           \end{bmatrix} \\
    \mathbf{B} &= - \begin{bmatrix}
        \mathbf{Ĉ^m S}(0)                    \\
        \mathbf{Ĉ^m S}(1)                    \\
        \vdots                               \\
        \mathbf{Ĉ^m S}(H_e-1) \end{bmatrix}  \mathbf{\big(f̂_{op} - x̂_{op}\big)}
    \end{aligned}
    ```
    or, for ``p=1``, the matrices are given by:
    ```math
    \begin{aligned}
    \mathbf{E} &= - \begin{bmatrix}
        \mathbf{Ĉ^m}\mathbf{Â}^{0}                  & \mathbf{0}                                    & \cdots & \mathbf{0}   \\ 
        \mathbf{Ĉ^m}\mathbf{Â}^{1}                  & \mathbf{Ĉ^m}\mathbf{Â}^{0}                    & \cdots & \mathbf{0}   \\ 
        \vdots                                      & \vdots                                        & \ddots & \vdots       \\
        \mathbf{Ĉ^m}\mathbf{Â}^{H_e-1}              & \mathbf{Ĉ^m}\mathbf{Â}^{H_e-2}                & \cdots & \mathbf{0}   \end{bmatrix} \\
    \mathbf{G} &= - \begin{bmatrix}
        \mathbf{0}                                  & \mathbf{0}                                    & \cdots & \mathbf{0}   \\ 
        \mathbf{Ĉ^m}\mathbf{Â}^{0}\mathbf{B̂_u}      & \mathbf{0}                                    & \cdots & \mathbf{0}   \\ 
        \vdots                                      & \vdots                                        & \ddots & \vdots       \\
        \mathbf{Ĉ^m}\mathbf{Â}^{H_e-2}\mathbf{B̂_u}  & \mathbf{Ĉ^m}\mathbf{Â}^{H_e-3}\mathbf{B̂_u}    & \cdots & \mathbf{0}   \end{bmatrix} \\
    \mathbf{J} &= - \begin{bmatrix}
        \mathbf{0}  & \mathbf{D̂_d^m}                              & \mathbf{0}                                    & \cdots & \mathbf{0}   \\ 
        \mathbf{0}  & \mathbf{Ĉ^m}\mathbf{Â}^{0}\mathbf{B̂_d}      & \mathbf{D̂_d^m}                                & \cdots & \mathbf{0}   \\ 
        \vdots      & \vdots                                      & \vdots                                        & \ddots & \vdots       \\
        \mathbf{0}  & \mathbf{Ĉ^m}\mathbf{Â}^{H_e-2}\mathbf{B̂_d}  & \mathbf{Ĉ^m}\mathbf{Â}^{H_e-3}\mathbf{B̂_d}    & \cdots & \mathbf{D̂_d^m} \end{bmatrix} \\
    \mathbf{B} &= - \begin{bmatrix}
        \mathbf{0}                           \\  
        \mathbf{Ĉ^m S}(0)                    \\
        \vdots                               \\
        \mathbf{Ĉ^m S}(H_e-2) \end{bmatrix}  \mathbf{\big(f̂_{op} - x̂_{op}\big)}
    \end{aligned}
    ```
    The matrices for the estimated states are computed by:
    ```math
    \begin{aligned}
    \mathbf{E_x̂} &= \begin{bmatrix}
        \mathbf{Â}^{1}                      & \mathbf{A}^{0}                    & \cdots & \mathbf{0}                   \\
        \mathbf{Â}^{2}                      & \mathbf{Â}^{1}                    & \cdots & \mathbf{0}                   \\ 
        \vdots                              & \vdots                            & \ddots & \vdots                       \\
        \mathbf{Â}^{H_e}                    & \mathbf{Â}^{H_e-1}                & \cdots & \mathbf{Â}^{0}               \end{bmatrix} \\
    \mathbf{G_x̂} &= \begin{bmatrix}
        \mathbf{Â}^{0}\mathbf{B̂_u}          & \mathbf{0}                        & \cdots & \mathbf{0}                   \\ 
        \mathbf{Â}^{1}\mathbf{B̂_u}          & \mathbf{Â}^{0}\mathbf{B̂_u}        & \cdots & \mathbf{0}                   \\ 
        \vdots                              & \vdots                            & \ddots & \vdots                       \\
        \mathbf{Â}^{H_e-1}\mathbf{B̂_u}      & \mathbf{Â}^{H_e-2}\mathbf{B̂_u}    & \cdots & \mathbf{Â}^{0}\mathbf{B̂_u}   \end{bmatrix} \\
    \mathbf{J_x̂^†} &= \begin{bmatrix}
        \mathbf{Â}^{0}\mathbf{B̂_d}          & \mathbf{0}                        & \cdots & \mathbf{0}                   \\ 
        \mathbf{Â}^{1}\mathbf{B̂_d}          & \mathbf{Â}^{0}\mathbf{B̂_d}        & \cdots & \mathbf{0}                   \\ 
        \vdots                              & \vdots                            & \ddots & \vdots                       \\
        \mathbf{Â}^{H_e-1}\mathbf{B̂_d}      & \mathbf{Â}^{H_e-2}\mathbf{B̂_d}    & \cdots & \mathbf{Â}^{0}\mathbf{B̂_d}   \end{bmatrix} \ , \quad
    \mathbf{J_x̂} = \begin{cases}
        [\begin{smallmatrix} \mathbf{J_x̂^†} & \mathbf{0}      \end{smallmatrix}]   & p=0                                \\
        [\begin{smallmatrix} \mathbf{0}     & \mathbf{J_x̂^†}  \end{smallmatrix}]   & p=1                                \end{cases}   \\
    \mathbf{B_x̂} &= \begin{bmatrix}
        \mathbf{S}(0)                    \\
        \mathbf{S}(1)                    \\
        \vdots                           \\
        \mathbf{S}(H_e-1) \end{bmatrix}  \mathbf{\big(f̂_{op} - x̂_{op}\big)}
    \end{aligned}
    ```
    All these matrices are truncated when ``N_k < H_e`` (at the beginning).
"""
function init_predmat_mhe(
    model::LinModel{NT}, He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, direct
) where {NT<:Real}
    nu, nd = model.nu, model.nd
    nym, nx̂ = length(i_ym), size(Â, 2)
    nŵ = nx̂
    p = direct ? 0 : 1
    # --- pre-compute matrix powers ---
    # Apow3D array : Apow[:,:,1] = A^0, Apow[:,:,2] = A^1, ... , Apow[:,:,He+1] = A^He
    Âpow3D = Array{NT}(undef, nx̂, nx̂, He+1)
    Âpow3D[:,:,1] = I(nx̂)
    for j=2:He+1
        Âpow3D[:,:,j] = @views Âpow3D[:,:,j-1]*Â
    end
    # nĈm_Âpow3D array : similar indices as Apow3D
    nĈm_Âpow3D = Array{NT}(undef, nym, nx̂, He+1)
    nĈm_Âpow3D[:,:,1] = -Ĉm
    for j=2:He+1
        nĈm_Âpow3D[:,:,j] = @views -Ĉm*Âpow3D[:,:,j]
    end
    # helper function to improve code clarity and be similar to eqs. in docstring:
    getpower(array3D, power) = @views array3D[:,:, power+1]
    # --- decision variables Z ---
    nĈm_Âpow = reduce(vcat, getpower(nĈm_Âpow3D, i) for i=0:He)
    E = zeros(NT, nym*He, nx̂ + nŵ*He)
    col_begin = iszero(p) ? 1    : 0
    col_end   = iszero(p) ? He : He-1
    i = 0
    for j=col_begin:col_end
        iRow = (1 + i*nym):(nym*He)
        iCol = (1:nŵ) .+ j*nŵ
        E[iRow, iCol] = @views nĈm_Âpow[1:length(iRow) ,:]
        i += 1
    end
    iszero(p) && @views (E[:, 1:nx̂] = @views nĈm_Âpow[nym+1:end, :])
    ex̄ = [-I zeros(NT, nx̂, nŵ*He)]
    Âpow_vec = reduce(vcat, getpower(Âpow3D, i) for i=0:He)
    Ex̂ = zeros(NT, nx̂*He, nx̂ + nŵ*He)
    i=0
    for j=1:He
        iRow = (1 + i*nx̂):(nx̂*He)
        iCol = (1:nŵ) .+ j*nŵ
        Ex̂[iRow, iCol] = @views Âpow_vec[1:length(iRow) ,:]
        i+=1
    end
    Ex̂[:, 1:nx̂] = @views Âpow_vec[nx̂+1:end, :] 
    # --- manipulated inputs U ---
    nĈm_Âpow_B̂u = reduce(vcat, getpower(nĈm_Âpow3D, i)*B̂u for i=0:He-1)
    nĈm_Âpow_B̂u = [zeros(nym, nu) ; nĈm_Âpow_B̂u]
    G = zeros(NT, nym*He, nu*He)
    i=0
    col_begin = iszero(p) ? 1    : 0
    col_end   = iszero(p) ? He-1 : He-2
    for j=col_begin:col_end
        iRow = (1 + i*nym):(nym*He)
        iCol = (1:nu) .+ j*nu
        G[iRow, iCol] = @views nĈm_Âpow_B̂u[1:length(iRow) ,:]
        i+=1
    end
    iszero(p) && @views (G[:, 1:nu] = nĈm_Âpow_B̂u[nym+1:end, :])
    Âpow_B̂u = reduce(vcat, getpower(Âpow3D, i)*B̂u for i=0:He-1)
    Gx̂ = zeros(NT, nx̂*He, nu*He)
    for j=0:He-1
        iRow = (1 + j*nx̂):(nx̂*He)
        iCol = (1:nu) .+ j*nu
        Gx̂[iRow, iCol] = @views Âpow_B̂u[1:length(iRow) ,:]
    end
    # --- measured disturbances D ---
    nĈm_Âpow_B̂d = reduce(vcat, getpower(nĈm_Âpow3D, i)*B̂d for i=0:He-1)
    nĈm_Âpow_B̂d = [-D̂dm; nĈm_Âpow_B̂d]
    J = zeros(NT, nym*He, nd*(He+1))
    i = 0
    for j=1:He
        iRow = (1 + i*nym):(nym*He)
        iCol = (1:nd) .+ j*nd
        J[iRow, iCol] = nĈm_Âpow_B̂d[1:length(iRow) ,:]
        i+=1
    end
    iszero(p) && @views (J[:, 1:nd] = nĈm_Âpow_B̂d[nym+1:end, :])
    Âpow_B̂d = reduce(vcat, getpower(Âpow3D, i)*B̂d for i=0:He-1)
    Jx̂ = zeros(NT, nx̂*He, nd*(He+1))
    for j=0:He-1
        iRow = (1 + j*nx̂):(nx̂*He)
        iCol = (1:nd) .+ j*nd .+ p
        Jx̂[iRow, iCol] = Âpow_B̂d[1:length(iRow) ,:]
    end
    # --- state x̂op and state update f̂op operating points ---
    # Apow_csum 3D array : Apow_csum[:,:,1] = A^0, Apow_csum[:,:,2] = A^1 + A^0, ...
    Âpow_csum  = cumsum(Âpow3D, dims=3)
    # helper function to improve code clarity and be similar to eqs. in docstring:
    S(j) = @views Âpow_csum[:,:, j+1]
    f̂_op_n_x̂op = (f̂op - x̂op)
    coef_B  = zeros(NT, nym*He, nx̂)
    row_begin = iszero(p) ? 0    : 1
    row_end   = iszero(p) ? He-1 : He-2
    j=0
    for i=row_begin:row_end
        iRow = (1:nym) .+ nym*i
        coef_B[iRow,:] = -Ĉm*S(j)
        j+=1
    end
    B = coef_B*f̂_op_n_x̂op
    coef_Bx̂ = Matrix{NT}(undef, nx̂*He, nx̂)
    for j=0:He-1
        iRow = (1:nx̂)  .+ nx̂*j
        coef_Bx̂[iRow,:] = S(j)
    end
    Bx̂ = coef_Bx̂*f̂_op_n_x̂op
    return E, G, J, B, ex̄, Ex̂, Gx̂, Jx̂, Bx̂
end

"Return empty matrices if `model` is not a [`LinModel`](@ref), except for `ex̄`."
function init_predmat_mhe(
    model::SimModel{NT}, He, i_ym, Â, _ , _ , _ , _ , _ , _ , direct
) where {NT<:Real}
    nym, nx̂ = length(i_ym), size(Â, 2)
    nŵ = nx̂
    p = direct ? 0 : 1
    E  = zeros(NT, 0, nx̂ + nŵ*He)
    ex̄ = [-I zeros(NT, nx̂, nŵ*He)]
    Ex̂ = zeros(NT, 0, nx̂ + nŵ*He)
    G  = zeros(NT, 0, model.nu*He)
    Gx̂ = zeros(NT, 0, model.nu*He)
    J  = zeros(NT, 0, model.nd*(He+1-p))
    Jx̂ = zeros(NT, 0, model.nd*He)
    B  = zeros(NT, nym*He)
    Bx̂ = zeros(NT, nx̂*He)
    return E, G, J, B, ex̄, Ex̂, Gx̂, Jx̂, Bx̂
end

"""
    init_optimization!(
        estim::MovingHorizonEstimator, model::LinModel, optim::JuMP.GenericModel
    )

Init the quadratic optimization of [`MovingHorizonEstimator`](@ref).
"""
function init_optimization!(
    estim::MovingHorizonEstimator, model::LinModel, optim::JuMP.GenericModel,
)
    C, con = estim.C, estim.con
    nZ̃ = length(estim.Z̃)
    # --- variables and linear constraints ---
    JuMP.num_variables(optim) == 0 || JuMP.empty!(optim)
    JuMP.set_silent(optim)
    limit_solve_time(optim, model.Ts)
    @variable(optim, Z̃var[1:nZ̃])
    A = con.A[con.i_b, :]
    b = con.b[con.i_b]
    @constraint(optim, linconstraint, A*Z̃var .≤ b)
    @objective(optim, Min, obj_quadprog(Z̃var, estim.H̃, estim.q̃))
    if con.nc > 0
        # --- nonlinear optimization init for the custom NL constraints ---
        set_scaling_gradient!(optim, C)
        # constraints with vector nonlinear oracle 
        g_oracle = get_nonlincon_oracle(estim, optim)  
        set_nonlincon!(estim, optim, g_oracle)
    end
    return nothing
end

"""
    init_optimization!(
        estim::MovingHorizonEstimator, model::SimModel, optim::JuMP.GenericModel,
    ) -> nothing

Init the nonlinear optimization of [`MovingHorizonEstimator`](@ref).
"""
function init_optimization!(
    estim::MovingHorizonEstimator, model::SimModel, optim::JuMP.GenericModel{JNT}
) where JNT<:Real
    C, con = estim.C, estim.con
    nZ̃ = length(estim.Z̃)
    # --- variables and linear constraints ---
    JuMP.num_variables(optim) == 0 || JuMP.empty!(optim)
    JuMP.set_silent(optim)
    limit_solve_time(optim, model.Ts)
    @variable(optim, Z̃var[1:nZ̃])
    A = con.A[con.i_b, :]
    b = con.b[con.i_b]
    @constraint(optim, linconstraint, A*Z̃var .≤ b)
    # --- nonlinear optimization init ---
    set_scaling_gradient!(optim, C)
    # constraints with vector nonlinear oracle, objective function with splatting:    
    J_op = get_nonlinobj_op(estim, optim)
    g_oracle = get_nonlincon_oracle(estim, optim)  
    @objective(optim, Min, J_op(Z̃var...))
    set_nonlincon!(estim, optim, g_oracle)
    return nothing
end

"""
    get_nonlinobj_op(estim::MovingHorizonEstimator, optim) -> J_op

Return the nonlinear operator for the objective of `estim` [`MovingHorizonEstimator`](@ref).

It is based on the splatting syntax. This method is really intricate and that's because of:

- These functions are used inside the nonlinear optimization, so they must be type-stable
  and as efficient as possible. All the function outputs and derivatives are cached and
  updated in-place if required to use the efficient [`value_and_gradient!`](@extref DifferentiationInterface DifferentiationInterface.value_and_jacobian!).
- The splatting syntax for objective functions implies the use of `Vararg{T,N}` (see the [performance tip](@extref Julia Be-aware-of-when-Julia-avoids-specializing))
  and memoization to avoid redundant computations. This is already complex, but it's even
  worse knowing that the automatic differentiation tools do not support splatting.
"""
function get_nonlinobj_op(
    estim::MovingHorizonEstimator, optim::JuMP.GenericModel{JNT}
) where JNT<:Real
    model, con = estim.model, estim.con
    grad, hess = estim.gradient, estim.hessian
    nx̂, nym, nŷ, nu, nk, nc = estim.nx̂, estim.nym, model.ny, model.nu, model.nk, con.nc
    He = estim.He
    ng = length(con.i_g)
    nŴ, nV̂, nX̂, ng, nZ̃ = He*nx̂, He*nym, He*nx̂, length(con.i_g), length(estim.Z̃)
    nŴe, nX̂e, nV̂e = (He+1)*nx̂, (He+1)*nx̂, (He+1)*nym
    strict = Val(true)
    myNaN                               = convert(JNT, NaN)
    J::Vector{JNT}                      = zeros(JNT, 1)
    x̂0arr::Vector{JNT}, x̄::Vector{JNT}  = zeros(JNT, nx̂),  zeros(JNT, nx̂)
    Ŵ::Vector{JNT}                      = zeros(JNT, nŴ)
    V̂::Vector{JNT},     X̂0::Vector{JNT} = zeros(JNT, nV̂),  zeros(JNT, nX̂)
    Ŵe::Vector{JNT}                     = zeros(JNT, nŴe)
    V̂e::Vector{JNT},    X̂e::Vector{JNT} = zeros(JNT, nV̂e), zeros(JNT, nX̂e)
    k::Vector{JNT}                      = zeros(JNT, nk)
    û0::Vector{JNT},    ŷ0::Vector{JNT} = zeros(JNT, nu),  zeros(JNT, nŷ)
    gc::Vector{JNT},    g::Vector{JNT}  = zeros(JNT, nc),  zeros(JNT, ng) 
    function J!(Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, û0, k, ŷ0, gc, g)
        update_prediction!(x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, û0, k, ŷ0, gc, g, estim, Z̃)
        return obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃)
    end
    Z̃_J = fill(myNaN, nZ̃)      # NaN to force update_predictions! at first call
    J_cache = (
        Cache(x̂0arr), Cache(x̄), 
        Cache(Ŵ), Cache(V̂), Cache(X̂0), 
        Cache(Ŵe), Cache(V̂e), Cache(X̂e),
        Cache(û0), Cache(k), Cache(ŷ0), Cache(gc), Cache(g),
    )
    # temporarily "fill" the estimation window for the preparation of the gradient: 
    estim.Nk[] = He
    ∇J_prep = prepare_gradient(J!, grad, Z̃_J, J_cache...; strict)
    estim.Nk[] = 0
    ∇J = Vector{JNT}(undef, nZ̃)
    if !isnothing(hess)
        estim.Nk[] = He # see comment above
        ∇²J_prep = prepare_hessian(J!, hess, Z̃_J, J_cache...; strict)
        estim.Nk[] = 0
        ∇²J = init_diffmat(JNT, hess, ∇²J_prep, nZ̃, nZ̃)
        ∇²J_structure = lowertriangle_indices(init_diffstructure(∇²J))
    end
    update_objective! = if !isnothing(hess)
        function (J, ∇J, ∇²J, Z̃_∇J, Z̃_arg)
            if isdifferent(Z̃_arg, Z̃_∇J)
                Z̃_∇J .= Z̃_arg
                J[], _ = value_gradient_and_hessian!(
                    J!, ∇J, ∇²J, ∇²J_prep, hess, Z̃_J, J_cache...
                )
            end
        end    
    else
        function (J, ∇J, Z̃_∇J, Z̃_arg)
            if isdifferent(Z̃_arg, Z̃_∇J)
                Z̃_∇J .= Z̃_arg
                J[], _ = value_and_gradient!(J!, ∇J, ∇J_prep, grad, Z̃_J, J_cache...)
            end
        end
    end
    J_func = if !isnothing(hess)
        function (Z̃_arg::Vararg{T, N}) where {N, T<:Real}
            update_objective!(J, ∇J, ∇²J, Z̃_J, Z̃_arg)
            return J[]::T
        end
    else
        function (Z̃_arg::Vararg{T, N}) where {N, T<:Real}
            update_objective!(J, ∇J, Z̃_J, Z̃_arg)
            return J[]::T
        end
    end
    # only the multivariate syntax of JuMP.@operator, univariate is impossible for MHE
    # since Z̃ comprises the arrival state estimate AND the estimated process noise:
    ∇J_func! = if !isnothing(hess)
        function (∇J_arg::AbstractVector{T}, Z̃_arg::Vararg{T, N}) where {N, T<:Real}
            update_objective!(J, ∇J, ∇²J, Z̃_J, Z̃_arg)
            return ∇J_arg .= ∇J
        end
    else
        function (∇J_arg::AbstractVector{T}, Z̃_arg::Vararg{T, N}) where {N, T<:Real}
            update_objective!(J, ∇J, Z̃_J, Z̃_arg)
            return ∇J_arg .= ∇J
        end
    end
    function ∇²J_func!(∇²J_arg::AbstractMatrix{T}, Z̃_arg::Vararg{T, N}) where {N, T<:Real}
        update_objective!(J, ∇J, ∇²J, Z̃_J, Z̃_arg)
        return fill_diffstructure!(∇²J_arg, ∇²J, ∇²J_structure)
    end
    if !isnothing(hess)
        @operator(optim, J_op, nZ̃, J_func, ∇J_func!, ∇²J_func!)
    else
        @operator(optim, J_op, nZ̃, J_func, ∇J_func!)
    end
    return J_op
end

"""
    get_nonlincon_oracle(estim::MovingHorizonEstimator, optim) -> g_oracle, geq_oracle

Return the nonlinear constraint oracles for [`MovingHorizonEstimator`](@ref) `estim`.

Return `g_oracle` and `geq_oracle`, the inequality and equality [`VectorNonlinearOracle`](@extref MathOptInterface MathOptInterface.VectorNonlinearOracle)
for the two respective constraints. Note that `g_oracle` only includes the non-`Inf`
inequality constraints, thus it must be re-constructed if they change. This method is really
intricate because the oracles are used inside the nonlinear optimization, so they must be
type-stable and as efficient as possible. All the function outputs and derivatives are 
ached and updated in-place if required to use the efficient [`value_and_jacobian!`](@extref DifferentiationInterface DifferentiationInterface.value_and_jacobian!).
"""
function get_nonlincon_oracle(
    estim::MovingHorizonEstimator, ::JuMP.GenericModel{JNT}
) where JNT<:Real
    # ----------- common cache for all functions  ----------------------------------------
    model, con = estim.model, estim.con
    jac, hess = estim.jacobian, estim.hessian
    nx̂, nym, nŷ, nu, nk = estim.nx̂, estim.nym, model.ny, model.nu, model.nk
    He = estim.He
    i_g = findall(con.i_g) # convert to non-logical indices for non-allocating @views
    ng, ngi = length(con.i_g), sum(con.i_g)
    nc = con.nc
    nŴ, nV̂, nX̂, nZ̃ = He*nx̂, He*nym, He*nx̂, length(estim.Z̃)
    nŴe, nX̂e, nV̂e = (He+1)*nx̂, (He+1)*nx̂, (He+1)*nym
    strict = Val(true)
    myNaN, myInf                          = convert(JNT, NaN), convert(JNT, Inf)
    x̂0arr::Vector{JNT}, x̄::Vector{JNT}    = zeros(JNT, nx̂), zeros(JNT, nx̂)
    Ŵ::Vector{JNT}                        = zeros(JNT, nŴ)
    V̂::Vector{JNT},     X̂0::Vector{JNT}   = zeros(JNT, nV̂),  zeros(JNT, nX̂)
    Ŵe::Vector{JNT}                       = zeros(JNT, nŴe)
    V̂e::Vector{JNT},    X̂e::Vector{JNT}   = zeros(JNT, nV̂e), zeros(JNT, nX̂e)
    k::Vector{JNT}                        = zeros(JNT, nk)
    û0::Vector{JNT},    ŷ0::Vector{JNT}   = zeros(JNT, nu), zeros(JNT, nŷ)
    gc::Vector{JNT},    g::Vector{JNT}    = zeros(JNT, nc), zeros(JNT, ng)
    gi::Vector{JNT}                       = zeros(JNT, ngi)
    λi::Vector{JNT}                       = rand(JNT, ngi)
    # -------------- inequality constraint: nonlinear oracle -------------------------
    function gi!(gi, Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, û0, k, ŷ0, gc, g)
        update_prediction!(x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, û0, k, ŷ0, gc, g, estim, Z̃)
        gi .= @views g[i_g]
        return nothing
    end
    function ℓ_gi(Z̃, λi, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, û0, k, ŷ0, gc, g, gi)
        update_prediction!(x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, û0, k, ŷ0, gc, g, estim, Z̃)
        gi .= @views g[i_g]
        return dot(λi, gi)
    end
    Z̃_∇gi = fill(myNaN, nZ̃)      # NaN to force update_predictions! at first call
    ∇gi_cache = (
        Cache(x̂0arr), Cache(x̄), 
        Cache(Ŵ), Cache(V̂), Cache(X̂0), 
        Cache(Ŵe), Cache(V̂e), Cache(X̂e),
        Cache(û0), Cache(k), Cache(ŷ0), Cache(gc), Cache(g),
    )
    # temporarily "fill" the estimation window for the preparation of the gradient: 
    estim.Nk[] = He
    ∇gi_prep = prepare_jacobian(gi!, gi, jac, Z̃_∇gi, ∇gi_cache...; strict)
    estim.Nk[] = 0
    ∇gi = init_diffmat(JNT, jac, ∇gi_prep, nZ̃, ngi)
    ∇gi_structure = init_diffstructure(∇gi)
    if !isnothing(hess)
        ∇²gi_cache = (
            Cache(x̂0arr), Cache(x̄), 
            Cache(Ŵ), Cache(V̂), Cache(X̂0), 
            Cache(Ŵe), Cache(V̂e), Cache(X̂e),    
            Cache(û0), Cache(k), Cache(ŷ0), Cache(gc), Cache(g), Cache(gi)
        )
        estim.Nk[] = He # see comment above
        ∇²gi_prep = prepare_hessian(
            ℓ_gi, hess, Z̃_∇gi, Constant(λi), ∇²gi_cache...; strict
        )
        estim.Nk[] = 0
        ∇²ℓ_gi    = init_diffmat(JNT, hess, ∇²gi_prep, nZ̃, nZ̃)
        ∇²gi_structure = lowertriangle_indices(init_diffstructure(∇²ℓ_gi))
    end
    function update_con!(gi, ∇gi, Z̃_∇gi, Z̃_arg)
        if isdifferent(Z̃_arg, Z̃_∇gi)
            Z̃_∇gi .= Z̃_arg
            value_and_jacobian!(gi!, gi, ∇gi, ∇gi_prep, jac, Z̃_∇gi, ∇gi_cache...)
        end
        return nothing
    end
    function gi_func!(gi_arg, Z̃_arg)
        update_con!(gi, ∇gi, Z̃_∇gi, Z̃_arg)
        return gi_arg .= gi
    end
    function ∇gi_func!(∇gi_arg, Z̃_arg)
        update_con!(gi, ∇gi, Z̃_∇gi, Z̃_arg)
        return fill_diffstructure!(∇gi_arg, ∇gi, ∇gi_structure)
    end
    function ∇²gi_func!(∇²ℓ_arg, Z̃_arg, λ_arg)
        Z̃_∇gi  .= Z̃_arg
        λi     .= λ_arg
        hessian!(ℓ_gi, ∇²ℓ_gi, ∇²gi_prep, hess, Z̃_∇gi, Constant(λi), ∇²gi_cache...)
        return fill_diffstructure!(∇²ℓ_arg, ∇²ℓ_gi, ∇²gi_structure)
    end
    gi_min = fill(-myInf, ngi)
    gi_max = zeros(JNT,   ngi)
    g_oracle = MOI.VectorNonlinearOracle(;
        dimension = nZ̃,
        l = gi_min,
        u = gi_max,
        eval_f = gi_func!,
        jacobian_structure = ∇gi_structure,
        eval_jacobian = ∇gi_func!,
        hessian_lagrangian_structure = isnothing(hess) ? Tuple{Int,Int}[] : ∇²gi_structure,
        eval_hessian_lagrangian      = isnothing(hess) ? nothing          : ∇²gi_func!
    )
    return g_oracle
end

"""
    set_nonlincon!(estim::MovingHorizonEstimator, optim, g_oracle)

Set the nonlinear inequality constraints of `estim`, if any.
"""
function set_nonlincon!(
    estim::MovingHorizonEstimator, optim::JuMP.GenericModel{JNT}, g_oracle
) where JNT<:Real
    Z̃var = optim[:Z̃var]
    nonlin_constraints = JuMP.all_constraints(
        optim, JuMP.Vector{JuMP.VariableRef}, MOI.VectorNonlinearOracle{JNT}
    )
    map(con_ref -> JuMP.delete(optim, con_ref), nonlin_constraints)
    JuMP.unregister(optim, :nonlinconstraint)
    any(estim.con.i_g) && @constraint(optim, nonlinconstraint, Z̃var in g_oracle)
    return nothing
end