Added: new hessian keyword argument in NonLinMPC and MovingHorizonEstimator

src/ModelPredictiveControl.jl

@@ -8,8 +8,10 @@ using RecipesBase
 using ProgressLogging
 
 using DifferentiationInterface: ADTypes.AbstractADType, AutoForwardDiff, AutoSparse
-using DifferentiationInterface: gradient!, jacobian!, prepare_gradient, prepare_jacobian
+using DifferentiationInterface: prepare_gradient, prepare_jacobian, prepare_hessian
+using DifferentiationInterface: gradient!, jacobian!, hessian!
 using DifferentiationInterface: value_and_gradient!, value_and_jacobian!
+using DifferentiationInterface: value_gradient_and_hessian!
 using DifferentiationInterface: Constant, Cache
 using SparseConnectivityTracer: TracerSparsityDetector
 using SparseMatrixColorings: GreedyColoringAlgorithm, sparsity_pattern

src/controller/execute.jl

@@ -405,7 +405,7 @@ function optim_objective!(mpc::PredictiveController{NT}) where {NT<:Real}
             MOIU.reset_optimizer(optim)
             JuMP.optimize!(optim)
         else
-            rethrow(err)
+            rethrow()
         end
     end
     if !issolved(optim)
@@ -488,7 +488,7 @@ Call `periodsleep(mpc.estim.model)`.
 periodsleep(mpc::PredictiveController, busywait=false) = periodsleep(mpc.estim.model, busywait)
 
 """
-    setstate!(mpc::PredictiveController, x̂, P̂=nothing) -> mpc
+    setstate!(mpc::PredictiveController, x̂[, P̂]) -> mpc
 
 Call [`setstate!`](@ref) on `mpc.estim` [`StateEstimator`](@ref).
 """

src/controller/transcription.jl

@@ -604,21 +604,18 @@ end
 
 """
     init_nonlincon!(
-        mpc::PredictiveController, model::LinModel, transcription::TranscriptionMethod, 
-        gfuncs  , ∇gfuncs!,   
-        geqfuncs, ∇geqfuncs!
-    )
+        mpc::PredictiveController, ::LinModel, ::TranscriptionMethod, g_vec_args, geq_vec_args
+    ) -> nothing
 
 Init nonlinear constraints for [`LinModel`](@ref) for all [`TranscriptionMethod`](@ref).
 
 The only nonlinear constraints are the custom inequality constraints `gc`.
 """
 function init_nonlincon!(
-    mpc::PredictiveController, ::LinModel, ::TranscriptionMethod,
-    gfuncs, ∇gfuncs!, 
-    _ , _    
+    mpc::PredictiveController, ::LinModel, ::TranscriptionMethod, g_vec_args, _   
 ) 
     optim, con = mpc.optim, mpc.con
+    gfuncs, ∇gfuncs! = g_vec_args
     nZ̃ = length(mpc.Z̃)
     if length(con.i_g) ≠ 0
         i_base = 0
@@ -634,22 +631,20 @@ end
 
 """
     init_nonlincon!(
-        mpc::PredictiveController, model::NonLinModel, transcription::MultipleShooting, 
-        gfuncs,   ∇gfuncs!,
-        geqfuncs, ∇geqfuncs!
-    )
+        mpc::PredictiveController, ::NonLinModel, ::MultipleShooting, g_vec_args, geq_vec_args
+    ) -> nothing
 
 Init nonlinear constraints for [`NonLinModel`](@ref) and [`MultipleShooting`](@ref).
 
 The nonlinear constraints are the output prediction `Ŷ` bounds, the custom inequality
 constraints `gc` and all the nonlinear equality constraints `geq`.
 """
 function init_nonlincon!(
-    mpc::PredictiveController, ::NonLinModel, ::MultipleShooting, 
-    gfuncs,     ∇gfuncs!,
-    geqfuncs,   ∇geqfuncs!
+    mpc::PredictiveController, ::NonLinModel, ::MultipleShooting, g_vec_args, geq_vec_args
 ) 
     optim, con = mpc.optim, mpc.con
+    gfuncs  , ∇gfuncs!   = g_vec_args
+    geqfuncs, ∇geqfuncs! = geq_vec_args
     ny, nx̂, Hp, nZ̃ = mpc.estim.model.ny, mpc.estim.nx̂, mpc.Hp, length(mpc.Z̃)
     # --- nonlinear inequality constraints ---
     if length(con.i_g) ≠ 0
@@ -691,20 +686,19 @@ end
 
 """
     init_nonlincon!(
-        mpc::PredictiveController, model::NonLinModel, ::SingleShooting, 
-        gfuncs,   ∇gfuncs!,
-        geqfuncs, ∇geqfuncs!
-    )
+        mpc::PredictiveController, ::NonLinModel, ::SingleShooting, g_vec_args, geq_vec_args
+    ) -> nothing
 
 Init nonlinear constraints for [`NonLinModel`](@ref) and [`SingleShooting`](@ref).
 
 The nonlinear constraints are the custom inequality constraints `gc`, the output
 prediction `Ŷ` bounds and the terminal state `x̂end` bounds.
 """
 function init_nonlincon!(
-    mpc::PredictiveController, ::NonLinModel, ::SingleShooting, gfuncs, ∇gfuncs!, _ , _
+    mpc::PredictiveController, ::NonLinModel, ::SingleShooting, g_vec_args, _
 )
     optim, con = mpc.optim, mpc.con
+    gfuncs, ∇gfuncs! = g_vec_args
     ny, nx̂, Hp, nZ̃ = mpc.estim.model.ny, mpc.estim.nx̂, mpc.Hp, length(mpc.Z̃)
     if length(con.i_g) ≠ 0
         i_base = 0

src/estimator/execute.jl

@@ -330,7 +330,7 @@ function validate_args(estim::StateEstimator, ym, d, u=nothing)
 end
 
 """
-    setstate!(estim::StateEstimator, x̂, P̂=nothing) -> estim
+    setstate!(estim::StateEstimator, x̂[, P̂]) -> estim
 
 Set `estim.x̂0` to `x̂ - estim.x̂op` from the argument `x̂`, and `estim.P̂` to `P̂` if applicable. 
 

src/estimator/mhe/construct.jl

@@ -250,33 +250,37 @@ transcription for now.
 - `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}``.
+   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.
+   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.
+   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.
+   ``\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).
+   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).
+   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).
+   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).
+   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).
+   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).
+   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.
 - `optim=default_optim_mhe(model)` : 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).
+- `hessian=nothing` : an `AbstractADType` backend for the Hessian of the objective function 
+   when `model` is not a [`LinModel`](@ref) (see [`DifferentiationInterface` doc](@extref DifferentiationInterface List)),
+   or `nothing` for the LBFGS approximation provided by `optim` (details in Extended Help).
+- `gradient=isnothing(hessian) ? AutoForwardDiff() : nothing` : an `AbstractADType` backend
+   for the gradient of the objective function when `model` is not a [`LinModel`](@ref) (see
+   `hessian` for the options), or `nothing` to retrieve lower-order derivatives from `hessian`. 
 - `jacobian=AutoForwardDiff()` : an `AbstractADType` backend for the Jacobian of the
-   constraints when `model` is not a [`LinModel`](@ref), see `gradient` above for the options.
+   constraints when `model` is not a [`LinModel`](@ref) (see `hessian` for the options).
 - `direct=true`: construct with a direct transmission from ``\mathbf{y^m}`` (a.k.a. current
    estimator, in opposition to the delayed/predictor form).
 
@@ -359,7 +363,9 @@ MovingHorizonEstimator estimator with a sample time Ts = 10.0 s, Ipopt optimizer
       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, 
+      (customizable). The `hessian` argument defaults the LBFGS approximation of `optim`,
+      but see [`NonLinMPC`](@ref) extended help for a sparse backend that would be otherwise
+      recommended. 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)

src/general.jl

@@ -51,14 +51,97 @@ function limit_solve_time(optim::GenericModel, Ts)
         if isa(err, MOI.UnsupportedAttribute{MOI.TimeLimitSec})
             @warn "Solving time limit is not supported by the optimizer."
         else
-            rethrow(err)
+            rethrow()
         end
     end
 end
 
+"Verify that provided 1st and 2nd order differentiation backends are possible and efficient."
+validate_backends(firstOrder::AbstractADType, secondOrder::Nothing) = nothing
+validate_backends(firstOrder::Nothing, secondOrder::AbstractADType) = nothing
+function validate_backends(firstOrder::AbstractADType, secondOrder::AbstractADType) 
+    @warn(
+        """
+        Two AbstractADType backends were provided for the 1st and 2nd order differentiations,
+        meaning that 1st order derivatives will be computed twice. Use nothing for the 1st
+        order backend to retrieve results from the hessian backend, which is more efficient.
+        """
+    )
+    return nothing 
+end
+function validate_backends(firstOrder::Nothing, secondOrder::Nothing)
+    throw(ArgumentError("1st and 2nd order differentiation backends cannot be both nothing."))
+end
+
 "Init a differentiation result matrix as dense or sparse matrix, as required by `backend`."
 init_diffmat(T, backend::AbstractADType, _  , nx , ny) = Matrix{T}(undef, ny, nx)
 init_diffmat(T, backend::AutoSparse    ,prep , _ , _ ) = similar(sparsity_pattern(prep), T)
+init_diffmat(T, backend::Nothing       , _  , nx , ny) = Matrix{T}(undef, ny, nx)
+
+"""
+    update_memoized_diff!(
+        x, y, ∇f, ∇²f, prep_∇f, prep_∇²f, context,
+        gradient::AbstractADType, hessian::Nothing, f!, xarg
+    ) -> nothing
+
+Update `f!` value `y` and and its gradient `∇f` in-place if `x ≠ xarg`.
+
+The method mutates all the arguments before `gradient`. This function is used for the
+memoization of the `f!` function derivatives, to avoid redundant computations with the
+splatting syntax of `JuMP.@operator`.
+"""
+function update_memoized_diff!(
+    x, y, ∇f, _ , prep_∇f, _ , context,
+    gradient::AbstractADType, hessian::Nothing, f!::F, xarg
+) where F <: Function
+    if isdifferent(xarg, x)
+        x .= xarg # more efficient than individual f! and gradient! calls:
+        y[], _ = value_and_gradient!(f!, ∇f, prep_∇f, gradient, x, context...)
+    end
+    return nothing
+end
+
+"Also update the Hessian `∇²f` if `hessian isa AbstractADType` and `isnothing(gradient)`."
+function update_memoized_diff!(
+    x, y, ∇f, ∇²f, _ , prep_∇²f, context,
+    gradient::Nothing, hessian::AbstractADType, f!::F, xarg
+) where F <: Function
+    if isdifferent(xarg, x)
+        x .= xarg # more efficient than individual f!, gradient! and hessian! calls:
+        y[], _ = value_gradient_and_hessian!(f!, ∇f, ∇²f, prep_∇²f, hessian, x, context...)
+    end
+    return nothing
+end 
+
+"Update `∇f` and `∇²f` individually if both backends are `AbstractADType`."
+function update_memoized_diff!(
+    x, y, ∇f, ∇²f, prep_∇f, prep_∇²f, context,
+    gradient::AbstractADType, hessian::AbstractADType, f!::F, xarg
+) where F <: Function
+    if isdifferent(xarg, x)
+        x .= xarg # inefficient, as warned by validate_backends(), but still possible:
+        hessian!(f!, ∇²f, prep_∇²f, hessian, x, context...)
+        y[], _ = value_and_gradient!(f!, ∇f, prep_∇f, gradient, x, context...)
+    end
+    return nothing
+end
+
+"""
+    update_memoized_diff!(x, y, ∇f, prep_∇f, context, jacobian::AbstractADType, f!, xarg) 
+
+Update `f!` value `y` (vector) and and its jacobian `∇f` in-place if `x ≠ xarg`.
+
+This method mutates all the arguments before `jacobian`.
+"""
+function update_memoized_diff!(
+    x, y, ∇f, prep_∇f, context, jacobian::AbstractADType, f!::F, xarg
+) where F <: Function
+    if isdifferent(xarg, x)
+        x .= xarg # more efficient than individual f! and jacobian! calls:
+        value_and_jacobian!(f!, y, ∇f, prep_∇f, jacobian, x, context...)
+    end
+    return nothing
+end
 
 "Verify that x and y elements are different using `!==`."
 isdifferent(x, y) = any(xi !== yi for (xi, yi) in zip(x, y))

test/2_test_state_estim.jl

@@ -1364,7 +1364,7 @@ end
     X̂_mhe = zeros(4, 6)
     X̂_kf  = zeros(4, 6)
     for i in 1:6
-        y = [50,31] #+ randn(2)
+        y = [50,31] + randn(2)
         x̂_mhe = preparestate!(mhe, y, [25])
         x̂_kf  = preparestate!(kf,  y, [25])
         X̂_mhe[:,i] = x̂_mhe