test: new tests with AutoFiniteDiff

debug: temporarily fake a "filled" window for MHE gradient preparation (needed for some backend like FiniteDiff)

Author: franckgaga

src/estimator/mhe/construct.jl

@@ -1356,7 +1356,10 @@ function get_optim_functions(
         Cache(g),
         Cache(x̄),
     )
+    # temporarily "fill" the estimation window for the preperation of the gradient: 
+    estim.Nk[] = He
     ∇J_prep = prepare_gradient(Jfunc!, estim.gradient, Z̃_∇J, ∇J_context...; strict)
+    estim.Nk[] = 0
     ∇J = Vector{JNT}(undef, nZ̃)
     ∇Jfunc! = function (∇J::AbstractVector{T}, Z̃arg::Vararg{T, N}) where {N, T<:Real}
         # only the multivariate syntax of JuMP.@operator, univariate is impossible for MHE

src/estimator/mhe/execute.jl

@@ -522,14 +522,21 @@ Objective function of the MHE when `model` is not a [`LinModel`](@ref).
 The function `dot(x, A, x)` is a performant way of calculating `x'*A*x`. This method mutates
 `x̄` vector arguments.
 """
-function obj_nonlinprog!(x̄, estim::MovingHorizonEstimator, ::SimModel, V̂, Z̃) 
+function obj_nonlinprog!(
+    x̄, estim::MovingHorizonEstimator, ::SimModel, V̂, Z̃::AbstractVector{NT}
+) where NT<:Real
     nϵ, Nk = estim.nϵ, estim.Nk[] 
     nYm, nŴ, nx̂, invP̄ = Nk*estim.nym, Nk*estim.nx̂, estim.nx̂, estim.invP̄
     nx̃ = nϵ + nx̂
     invQ̂_Nk, invR̂_Nk = @views estim.invQ̂_He[1:nŴ, 1:nŴ], estim.invR̂_He[1:nYm, 1:nYm]
     x̂0arr, Ŵ, V̂ = @views Z̃[nx̃-nx̂+1:nx̃], Z̃[nx̃+1:nx̃+nŴ], V̂[1:nYm]
     x̄ .= estim.x̂0arr_old .- x̂0arr
-    Jϵ = nϵ ≠ 0 ? estim.C*Z̃[begin]^2 : 0
+    Jϵ = nϵ ≠ 0 ? estim.C*Z̃[begin]^2 : zero(NT)
+    #println(x̂0arr)
+    #println(invP̄)
+    #println(dot(x̄, invP̄, x̄))
+    #println(dot(Ŵ, invQ̂_Nk, Ŵ))
+    #println(dot(V̂, invR̂_Nk, V̂))
     return dot(x̄, invP̄, x̄) + dot(Ŵ, invQ̂_Nk, Ŵ) + dot(V̂, invR̂_Nk, V̂) + Jϵ
 end
 

test/2_test_state_estim.jl

@@ -781,11 +781,13 @@ end
 end
 
 @testitem "MovingHorizonEstimator construction" setup=[SetupMPCtests] begin
-    using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, JuMP, Ipopt
+    using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
+    using JuMP, Ipopt, DifferentiationInterface
+    import FiniteDiff
     linmodel1 = LinModel(sys,Ts,i_d=[3])
-    f(x,u,d,_) = linmodel1.A*x + linmodel1.Bu*u + linmodel1.Bd*d
-    h(x,d,_)   = linmodel1.C*x + linmodel1.Du*d
-    nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing)
+    f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d
+    h(x,d,model)   = model.C*x + model.Dd*d
+    nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing, p=linmodel1)
 
     mhe1 = MovingHorizonEstimator(linmodel1, He=5)
     @test mhe1.nym == 2
@@ -857,6 +859,12 @@ end
     mhe13 = MovingHorizonEstimator(linmodel2, He=5)
     @test isa(mhe13, MovingHorizonEstimator{Float32})
 
+    mhe14 = MovingHorizonEstimator(
+        nonlinmodel, He=5, gradient=AutoFiniteDiff(), jacobian=AutoFiniteDiff()
+    )
+    @test mhe14.gradient == AutoFiniteDiff()
+    @test mhe14.jacobian == AutoFiniteDiff()
+
     @test_throws ArgumentError MovingHorizonEstimator(linmodel1)
     @test_throws ArgumentError MovingHorizonEstimator(linmodel1, He=0)
     @test_throws ArgumentError MovingHorizonEstimator(linmodel1, Cwt=-1)

test/3_test_predictive_control.jl

@@ -614,10 +614,12 @@ end
     @test nmpc17.transcription == MultipleShooting()
     @test length(nmpc17.Z̃) == linmodel1.nu*nmpc17.Hc + nmpc17.estim.nx̂*nmpc17.Hp + nmpc17.nϵ
     @test size(nmpc17.con.Aeq, 1) == nmpc17.estim.nx̂*nmpc17.Hp
-    @test_nowarn nmpc18 = NonLinMPC(nonlinmodel, Hp=10, 
+    nmpc18 = NonLinMPC(nonlinmodel, Hp=10, 
         gradient=AutoFiniteDiff(), 
         jacobian=AutoFiniteDiff()
     )
+    @test nmpc18.gradient == AutoFiniteDiff()
+    @test nmpc18.jacobian == AutoFiniteDiff()
 
     nonlinmodel2 = NonLinModel{Float32}(f, h, Ts, 2, 4, 2, 1, solver=nothing)
     nmpc15  = NonLinMPC(nonlinmodel2, Hp=15)
@@ -724,6 +726,19 @@ end
     info = getinfo(nmpc9)
     @test info[:u] ≈ u
     @test info[:Ŷ][end] ≈ 20 atol=5e-2
+    nmpc10 = setconstraint!(NonLinMPC(
+        nonlinmodel, Nwt=[0], Hp=100, Hc=1, 
+        gradient=AutoFiniteDiff(),
+        jacobian=AutoFiniteDiff()), 
+        ymax=[100], ymin=[-100]
+    )
+    preparestate!(nmpc10, [0], [0])
+    u = moveinput!(nmpc10, [10], [0])
+    @test u ≈ [2] atol=5e-2
+    info = getinfo(nmpc10)
+    @test info[:u] ≈ u
+    @test info[:Ŷ][end] ≈ 10 atol=5e-2
+
     @test_nowarn ModelPredictiveControl.info2debugstr(info)
 end