diff --git a/Project.toml b/Project.toml index 742ac0fad..5b3fb9dd3 100644 --- a/Project.toml +++ b/Project.toml @@ -1,6 +1,6 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" -version = "2.4.0" +version = "2.4.1" authors = ["Francis Gagnon"] [deps] diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index a9627c41a..9d5989b2a 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -834,7 +834,7 @@ function get_nonlinobj_op(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where J end end else - update_objective! = function (J, ∇J, Z̃_∇J, Z̃_arg) + function (J, ∇J, Z̃_∇J, Z̃_arg) if isdifferent(Z̃_arg, Z̃_∇J) Z̃_∇J .= Z̃_arg J[], _ = value_and_gradient!( diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 6ddd9a0e7..b8ac0e89e 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1250,10 +1250,10 @@ in which ``\mathbf{U_0}`` and ``\mathbf{Y_0^m}`` respectively include the deviat 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: +``j=N_k`` to ``0``, thus one additional data point. 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̄} ``` diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index b67cc026d..c5f8a515d 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -93,7 +93,7 @@ following fields: - `:Ŵ` or *`:What`* : optimal estimated process noise over ``N_k``, ``\mathbf{Ŵ}`` - `:ε` or *`:epsilon`* : optimal slack variable, ``ε`` -- `:X̂` or *`:Xhat`* : optimal estimated states over ``N_k+1``, ``\mathbf{X̂}`` +- `:X̂` or *`:Xhat`* : optimal estimated states over ``N_k``, ``\mathbf{X̂}`` - `:x̂` or *`:xhat`* : optimal estimated state, ``\mathbf{x̂}_k(k+p)`` - `:V̂` or *`:Vhat`* : optimal estimated sensor noise over ``N_k``, ``\mathbf{V̂}`` - `:P̄` or *`:Pbar`* : estimation error covariance at arrival, ``\mathbf{P̄}`` @@ -104,10 +104,11 @@ following fields: - `:J` : objective value optimum, ``J`` - `:Ym` : measured outputs over ``N_k``, ``\mathbf{Y^m}`` - `:U` : manipulated inputs over ``N_k``, ``\mathbf{U}`` -- `:D` : measured disturbances over ``N_k``, ``\mathbf{D}`` +- `:D` : measured disturbances over ``N_k+1``, ``\mathbf{D}`` - `:sol` : solution summary of the optimizer for printing -For [`NonLinModel`](@ref), it also includes the following fields: +For [`NonLinModel`](@ref) or under custom nonlinear inequality constraints (`nc>0`), it also +includes the following fields: - `:∇J` or *`:nablaJ`* : optimal gradient of the objective function, ``\mathbf{\nabla} J`` - `:∇²J` or *`:nabla2J`* : optimal Hessian of the objective function, ``\mathbf{\nabla^2}J`` @@ -136,42 +137,39 @@ julia> round.(getinfo(estim)[:Ŷ], digits=3) function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real model, buffer, Nk = estim.model, estim.buffer, estim.Nk[] nu, ny, nd = model.nu, model.ny, model.nd - nx̂, nym, nŵ, nε = estim.nx̂, estim.nym, estim.nx̂, estim.nε - nx̃ = nε + nx̂ + nx̂, nym, nŵ = estim.nx̂, estim.nym, estim.nx̂ + Z̃, Ŵ = estim.Z̃, estim.Ŵ info = Dict{Symbol, Any}() + Ŷ0 = Vector{NT}(undef, ny*Nk) V̂, X̂0 = buffer.V̂, buffer.X̂ x̂0arr, û0, k, ŷ0 = buffer.x̂, buffer.û, buffer.k, buffer.ŷ - x̂0arr = getarrival!(x̂0arr, estim, estim.Z̃) + x̂0arr = getarrival!(x̂0arr, estim, Z̃) x̄ = estim.x̂0arr_old - x̂0arr - V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, x̂0arr, estim.Ŵ, estim.Z̃) - X̂0 = [x̂0arr; X̂0] - Ym0, U0, D0 = estim.Y0m[1:nym*Nk], estim.U0[1:nu*Nk], estim.D0[1:nd*Nk] - Ŷ0m, Ŷ0 = Vector{NT}(undef, nym*Nk), Vector{NT}(undef, ny*Nk) - for i=1:Nk - d0 = @views D0[(1 + nd*(i-1)):(nd*i)] - x̂0 = @views X̂0[(1 + nx̂*(i-1)):(nx̂*i)] - @views ĥ!(Ŷ0[(1 + ny*(i-1)):(ny*i)], estim, model, x̂0, d0) - Ŷ0m[(1 + nym*(i-1)):(nym*i)] .= @views Ŷ0[(1 + ny*(i-1)):(ny*i)][estim.i_ym] - end - Ym, U, D, Ŷm, Ŷ = Ym0, U0, D0, Ŷ0m, Ŷ0 + V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, x̂0arr, Ŵ, Z̃) + Ŷ0 = predict_outputs_mhe!(Ŷ0, estim, X̂0, x̂0arr) + J = obj_nonlinprog(estim, estim.model, x̄, V̂, Ŵ, Z̃) + Ym0, U0, D0 = estim.Y0m[1:nym*Nk], estim.U0[1:nu*Nk], estim.D0[1:nd*(Nk+1)] + Ym, U, D, Ŷ, X̂, x̂arr = Ym0, U0, D0, Ŷ0, X̂0, x̂0arr for i=1:Nk - Ŷ[(1 + ny*(i-1)):(ny*i)] .+= model.yop - Ŷm[(1 + nym*(i-1)):(nym*i)] .+= @views model.yop[estim.i_ym] + X̂[( 1 + nx̂*(i-1)):(nx̂*i)] .+= estim.x̂op + Ŷ[( 1 + ny*(i-1)):(ny*i)] .+= model.yop Ym[(1 + nym*(i-1)):(nym*i)] .+= @views model.yop[estim.i_ym] - U[(1 + nu*(i-1)):(nu*i)] .+= model.uop - D[(1 + nd*(i-1)):(nd*i)] .+= model.dop + U[( 1 + nu*(i-1)):(nu*i)] .+= model.uop + D[( 1 + nd*(i-1)):(nd*i)] .+= model.dop end - info[:Ŵ] = estim.Ŵ[1:Nk*nŵ] - info[:x̂arr] = x̂0arr + estim.x̂op - info[:ε] = nε ≠ 0 ? estim.Z̃[begin] : zero(NT) - info[:J] = obj_nonlinprog(estim, estim.model, x̄, V̂, estim.Ŵ, estim.Z̃) - info[:X̂] = (X̂0 .+ @views [estim.x̂op; estim.X̂op])[1:nx̂*(Nk+1)] + D[end-nd+1:end] .+= model.dop + x̂arr .+= estim.x̂op + info[:Ŵ] = Ŵ[1:nŵ*Nk] + info[:ε] = getε(estim, Z̃) + info[:X̂] = X̂ info[:x̂] = estim.x̂0 .+ estim.x̂op info[:V̂] = V̂ info[:P̄] = estim.P̂arr_old info[:x̄] = x̄ info[:Ŷ] = Ŷ - info[:Ŷm] = Ŷm + info[:Ŷm] = Ŷ[vec(estim.i_ym .+ ny.*(0:Nk-1)')] + info[:x̂arr] = x̂arr + info[:J] = J info[:Ym] = Ym info[:U] = U info[:D] = D @@ -195,13 +193,12 @@ end """ - addinfo!(info, estim::MovingHorizonEstimator, model::NonLinModel) + addinfo!(info, estim::MovingHorizonEstimator, model::SimModel) -> info -For [`NonLinModel`](@ref), add the various derivatives. +Add the various derivatives if model is *not* a [`LinModel`](@ref) or if `nc > 0`. """ -function addinfo!( - info, estim::MovingHorizonEstimator{NT}, model::NonLinModel -) where NT <:Real +function addinfo!(info, estim::MovingHorizonEstimator{NT}, model::SimModel) where NT <:Real + model isa LinModel && iszero(estim.con.nc) && return info # --- objective derivatives --- optim, con = estim.optim, estim.con hess = estim.hessian @@ -305,9 +302,6 @@ function addinfo!( return info end -"Nothing to add in the `info` dict for [`LinModel`](@ref)." -addinfo!(info, ::MovingHorizonEstimator, ::LinModel) = info - "Get the estimated state at arrival from the decision vector `Z̃`." function getarrival!(x̂0arr, estim::MovingHorizonEstimator, Z̃) nx̃ = estim.nε + estim.nx̂ @@ -856,9 +850,9 @@ function predict_mhe!( V̂, X̂0, û0, k, ŷ0, estim::MovingHorizonEstimator, model::SimModel, x̂0arr, Ŵ, _ ) nu, nd, nx̂, nŵ, nym, Nk = model.nu, model.nd, estim.nx̂, estim.nx̂, estim.nym, estim.Nk[] - x̂0 = x̂0arr + x̂0 = @views x̂0arr[1:nx̂] if Nk < estim.He - V̂ .= 0 # fill unused values a 0 for tracer sparsity detection + V̂ .= 0 # fill unused values with 0s for tracer sparsity detection X̂0 .= 0 end if estim.direct # p = 0 @@ -895,6 +889,32 @@ function predict_mhe!( return V̂, X̂0 end +@doc raw""" + predict_outputs_mhe!(Ŷ0, estim::MovingHorizonEstimator, X̂0, x̂0arr) -> Ŷ0 + +Predict in-place the outputs of `estim` [`MovingHorizonEstimator`](@ref). + +This function is not used for the optimization, but it can be useful to predict the +estimated outputs ``\mathbf{ŷ_0}(k-j+1)`` from ``j=N_k`` to ``1``, stored in-place in the +`Ŷ0` vector. The argument `X̂0` is computed from [`predict_mhe!`](@ref) and contains the +estimated states from ``k-N_k+1+p`` to ``k+p``. The argument `x̂0arr` is computed from +[`getarrival!`](@ref) and contains the arrival state estimate for the time step ``k-N_k+p``. +""" +function predict_outputs_mhe!(Ŷ0, estim::MovingHorizonEstimator, X̂0, x̂0arr) + model = estim.model + nd, ny, nx̂, Nk = model.nd, model.ny, estim.nx̂, estim.Nk[] + D0 = estim.D0 + p = estim.direct ? 0 : 1 + x̂0 = @views estim.direct ? X̂0[1:nx̂] : x̂0arr[1:nx̂] + for j=1:Nk + d0 = @views D0[(1 + nd*j):(nd*(j+1))] # 1st data in D0 is d0(k-Nk), not used here + ŷ0 = @views Ŷ0[(1 + ny*(j-1)):(ny*j)] + ĥ!(ŷ0, estim, estim.model, x̂0, d0) + j < Nk || break + x̂0 = @views X̂0[(1 + nx̂*(j-p)):(nx̂*(j-p+1))] + end + return Ŷ0 +end """ update_predictions!( diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 259ea33dc..e99b59168 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1287,7 +1287,7 @@ end @test_throws ArgumentError setconstraint!(mhe4, c_v̂max=[1,1]) end -@testitem "MHE constraint violation" setup=[SetupMPCtests] begin +@testitem "MHE constraint violation (LinModel)" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30]) mhe = MovingHorizonEstimator(linmodel, He=1, nint_ym=0) @@ -1336,6 +1336,37 @@ end info = getinfo(mhe) @test info[:V̂] ≈ [-1,-1] atol=5e-2 + linmodel2 = LinModel(sys, Ts, i_u=[1,2], i_d=[3]) + linmodel2 = setop!(linmodel2, uop=[10,50], yop=[50,30], dop=[5]) + function gclv!(LHS, X̂e, _, _, _, _, _, _, _, nx̂, _ ) + for i in 1:div(length(X̂e), nx̂) + LHS[(i-1)+1] = 0.5 - X̂e[(i-1)*nx̂ + 1] # First state >= 0.5 + end + return nothing + end + nx̂ = linmodel2.nx + nc = 6 + mhe = MovingHorizonEstimator(linmodel2, He=5, gc=gclv!, nc=nc, nint_ym=0, p=nx̂) + preparestate!(mhe, [50, 30], [5]) + x̂ = updatestate!(mhe, [10, 50], [50, 30], [5]) + @test x̂[1] ≈ 0.5 atol = 5e-2 + + function gcln!(LHS, _, _, Ŵe, _, _, _, _,_, nx̂, _) + LHS .= Ŵe[1:nx̂] + return nothing + end + nx̂ = linmodel2.nx + nc = linmodel2.nx + mhe = MovingHorizonEstimator(linmodel2, He=1, gc=gcln!, nc=nc, nint_ym=0, direct=false, p=nx̂) + preparestate!(mhe, [50, 30], [5]) + x̂ = updatestate!(mhe, [10, 50], [50, 30], [5]) + @test mhe.Ŵ ≈ zeros(nx̂) atol=5e-2 +end + +@testitem "MHE constraint violation (NonLinModel)" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra + linmodel = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30]) + f = (x,u,_,model) -> model.A*x + model.Bu*u h = (x,_,model) -> model.C*x nonlinmodel = NonLinModel(f, h, Ts, 2, 2, 2, p=linmodel, solver=nothing) @@ -1388,30 +1419,7 @@ end linmodel2 = LinModel(sys, Ts, i_u=[1,2], i_d=[3]) linmodel2 = setop!(linmodel2, uop=[10,50], yop=[50,30], dop=[5]) - function gclv!(LHS, X̂e, _, _, _, _, _, _, _, nx̂, _ ) - for i in 1:div(length(X̂e), nx̂) - LHS[(i-1)+1] = 0.5 - X̂e[(i-1)*nx̂ + 1] # First state >= 0.5 - end - return nothing - end - nx̂ = linmodel2.nx - nc = 6 - mhe = MovingHorizonEstimator(linmodel2, He=5, gc=gclv!, nc=nc, nint_ym=0, p=nx̂) - preparestate!(mhe, [50, 30], [5]) - x̂ = updatestate!(mhe, [10, 50], [50, 30], [5]) - @test x̂[1] ≈ 0.5 atol = 5e-2 - function gcln!(LHS, _, _, Ŵe, _, _, _, _,_, nx̂, _) - LHS .= Ŵe[1:nx̂] - return nothing - end - nx̂ = linmodel2.nx - nc = linmodel2.nx - mhe = MovingHorizonEstimator(linmodel2, He=1, gc=gcln!, nc=nc, nint_ym=0, direct=false, p=nx̂) - preparestate!(mhe, [50, 30], [5]) - x̂ = updatestate!(mhe, [10, 50], [50, 30], [5]) - @test mhe.Ŵ ≈ zeros(nx̂) atol=5e-2 - f = (x,u,d,model) -> model.A*x + model.Bu*u + model.Bd*d h = (x,d,model) -> model.C*x + model.Dd*d nonlinmodel2 = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing, p=linmodel2) @@ -1424,9 +1432,9 @@ end end nc = 2 nx̂ = nonlinmodel2.nx - mhe = MovingHorizonEstimator(nonlinmodel2, He=1, gc=gcnlv!, nc=nc, nint_ym=0, p=nx̂) - preparestate!(mhe, [50, 30], [5]) - x̂ = updatestate!(mhe, [10, 50], [50, 30], [5]) + mhe2 = MovingHorizonEstimator(nonlinmodel2, He=1, gc=gcnlv!, nc=nc, nint_ym=0, p=nx̂) + preparestate!(mhe2, [50, 30], [5]) + x̂ = updatestate!(mhe2, [10, 50], [50, 30], [5]) @test x̂[1] ≈ 0.5 atol = 5e-2 function gclnl!(LHS, _, _, Ŵe, _, _, _, _,_, nx̂, _) @@ -1435,11 +1443,10 @@ end end nx̂ = nonlinmodel2.nx nc = nonlinmodel2.nx - mhe = MovingHorizonEstimator(nonlinmodel2, He=1, gc=gclnl!, nc=nc, nint_ym=0, p=nx̂) - preparestate!(mhe, [50, 30], [5]) - x̂ = updatestate!(mhe, [10, 50], [50, 30], [5]) - @test mhe.Ŵ ≈ zeros(nx̂) atol=5e-2 - + mhe2 = MovingHorizonEstimator(nonlinmodel2, He=1, gc=gclnl!, nc=nc, nint_ym=0, p=nx̂) + preparestate!(mhe2, [50, 30], [5]) + x̂ = updatestate!(mhe2, [10, 50], [50, 30], [5]) + @test mhe2.Ŵ ≈ zeros(nx̂) atol=5e-2 end @testitem "MHE set model" setup=[SetupMPCtests] begin