diff --git a/docs/src/index.md b/docs/src/index.md index 8a9a8a851..513f7022c 100644 --- a/docs/src/index.md +++ b/docs/src/index.md @@ -30,10 +30,10 @@ The documentation is divided in two parts: ```@contents Depth = 2 Pages = [ - "manual/installation.md", - "manual/linmpc.md", - "manual/nonlinmpc.md", - "manual/mtk.md" + joinpath("manual", "installation.md"), + joinpath("manual", "linmpc.md"), + joinpath("manual", "nonlinmpc.md"), + joinpath("manual", "mtk.md") ] ``` @@ -42,11 +42,11 @@ Pages = [ ```@contents Depth = 2 Pages = [ - "public/sim_model.md", - "public/state_estim.md", - "public/predictive_control.md", - "public/generic_func.md", - "public/plot_sim.md", + joinpath("public", "sim_model.md"), + joinpath("public", "state_estim.md"), + joinpath("public", "predictive_control.md"), + joinpath("public", "generic_func.md"), + joinpath("public", "plot_sim.md") ] ``` @@ -55,8 +55,8 @@ Pages = [ ```@contents Depth = 1 Pages = [ - "internals/sim_model.md", - "internals/state_estim.md", - "internals/predictive_control.md", + joinpath("internals", "sim_model.md"), + joinpath("internals", "state_estim.md"), + joinpath("internals", "predictive_control.md") ] ``` diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 9440f93ba..b10927c5d 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -206,7 +206,7 @@ The argument `Z̃orΔŨ` can be the augmented decision vector ``\mathbf{Z̃}`` input increment vector ``\mathbf{ΔŨ}``, it works with both. """ function getϵ(mpc::PredictiveController, Z̃orΔŨ::AbstractVector{NT}) where NT<:Real - return mpc.nϵ ≠ 0 ? Z̃orΔŨ[end] : zero(NT) + return mpc.nϵ > 0 ? Z̃orΔŨ[end] : zero(NT) end """ diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index e3383e926..c2e07b646 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -121,7 +121,7 @@ struct NonLinMPC{ # dummy vals (updated just before optimization): d0, D̂0, D̂e = zeros(NT, nd), zeros(NT, nd*Hp), zeros(NT, nd + nd*Hp) Uop, Yop, Dop = repeat(model.uop, Hp), repeat(model.yop, Hp), repeat(model.dop, Hp) - test_custom_functions(NT, model, JE, gc!, nc, Uop, Yop, Dop, p) + test_custom_function_mpc(NT, model, JE, gc!, nc, Uop, Yop, Dop, p) Mo, Co, λo = init_orthocolloc(model, transcription) nZ̃ = get_nZ(estim, transcription, Hp, Hc) + nϵ Z̃ = zeros(NT, nZ̃) @@ -167,7 +167,7 @@ controller minimizes the following objective function at each discrete time ``k` ``` subject to [`setconstraint!`](@ref) bounds, and the custom inequality constraints: ```math -\mathbf{g_c}(\mathbf{U_e}, \mathbf{Ŷ_e}, \mathbf{D̂_e}, \mathbf{p}, ϵ) ≤ \mathbf{0} +\mathbf{g_c}(\mathbf{U_e, Ŷ_e, D̂_e, p}, ϵ) ≤ \mathbf{0} ``` with the decision variables ``\mathbf{Z}`` and slack ``ϵ``. By default, a [`SingleShooting`](@ref) transcription method is used, hence ``\mathbf{Z=ΔU}``. The economic function ``J_E`` can @@ -222,8 +222,8 @@ This controller allocates memory at each time step for the optimization. - `JE=(_,_,_,_,_)->0.0` : economic or custom cost function ``J_E(\mathbf{U_e}, \mathbf{Ŷ_e}, \mathbf{D̂_e}, \mathbf{p}, ϵ)``. - `gc=(_,_,_,_,_,_)->nothing` or `gc!` : custom nonlinear inequality constraint function - ``\mathbf{g_c}(\mathbf{U_e}, \mathbf{Ŷ_e}, \mathbf{D̂_e}, \mathbf{p}, ϵ)``, mutating or - not (details in Extended Help). + ``\mathbf{g_c}(\mathbf{U_e}, \mathbf{Ŷ_e, D̂_e, p}, ϵ)``, mutating or not (details in + Extended Help). - `nc=0` : number of custom nonlinear inequality constraints. - `p=model.p` : ``J_E`` and ``\mathbf{g_c}`` functions parameter ``\mathbf{p}`` (any type). - `transcription=SingleShooting()` : a [`TranscriptionMethod`](@ref) for the optimization. @@ -276,13 +276,15 @@ NonLinMPC controller with a sample time Ts = 10.0 s: extended vectors ``\mathbf{U_e}``, ``\mathbf{Ŷ_e}`` and ``\mathbf{D̂_e}`` as arguments. They also receives the slack ``ϵ`` (scalar), which is always zero if `Cwt=Inf`. The following table details the vector sizes and the time steps of the first and last data - point in them: + point in them. - | VECTOR | SIZE | FIRST TIME STEP | LAST TIME STEP | - | :--------------- | :------------- | :-------------- | :------------- | - | ``\mathbf{U_e}`` | `(nu*(Hp+1),)` | ``k + 0`` | ``k + H_p`` | - | ``\mathbf{Ŷ_e}`` | `(ny*(Hp+1),)` | ``k + 0`` | ``k + H_p`` | - | ``\mathbf{D̂_e}`` | `(nd*(Hp+1),)` | ``k + 0`` | ``k + H_p`` | + | ARGUMENT | SIZE | FIRST SAMPLE | LAST SAMPLE | + | :--------------- | :------------- | :----------- | :-----------| + | ``\mathbf{U_e}`` | `((Hp+1)*nu,)` | ``k`` | ``k + H_p`` | + | ``\mathbf{Ŷ_e}`` | `((Hp+1)*ny,)` | ``k`` | ``k + H_p`` | + | ``\mathbf{D̂_e}`` | `((Hp+1)*nd,)` | ``k`` | ``k + H_p`` | + | ``\mathbf{p}`` | var. | — | — | + | ``ϵ`` | `()` | — | — | More precisely, the last two time steps in ``\mathbf{U_e}`` are forced to be equal, i.e. ``\mathbf{u}(k+H_p) = \mathbf{u}(k+H_p-1)``, since ``H_c ≤ H_p`` implies that @@ -290,8 +292,8 @@ NonLinMPC controller with a sample time Ts = 10.0 s: are the current state estimator output and measured disturbance, respectively, and ``\mathbf{Ŷ}`` and ``\mathbf{D̂}``, their respective predictions from ``k+1`` to ``k+H_p``. If `LHS` represents the result of the left-hand side in the inequality - ``\mathbf{g_c}(\mathbf{U_e}, \mathbf{Ŷ_e}, \mathbf{D̂_e}, \mathbf{p}, ϵ) ≤ \mathbf{0}``, - the function `gc` can be implemented in two possible ways: + ``\mathbf{g_c}(\mathbf{U_e, Ŷ_e, D̂_e, p}, ϵ) ≤ \mathbf{0}``, the function `gc` can be + implemented in two possible ways: 1. **Non-mutating function** (out-of-place): define it as `gc(Ue, Ŷe, D̂e, p, ϵ) -> LHS`. This syntax is simple and intuitive but it allocates more memory. @@ -442,7 +444,7 @@ function NonLinMPC( nb = move_blocking(Hp, Hc) Hc = get_Hc(nb) validate_JE(NT, JE) - gc! = get_mutating_gc(NT, gc) + gc! = get_mutating_gc_mpc(NT, gc) weights = ControllerWeights(estim.model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt) hessian = validate_hessian(hessian, gradient, DEFAULT_NONLINMPC_HESSIAN) return NonLinMPC{NT}( @@ -471,18 +473,23 @@ function validate_JE(NT, JE) end """ - validate_gc(NT, gc) -> ismutating + validate_gc_mpc(NT, gc) -> ismutating -Validate `gc` function argument signature and return `true` if it is mutating. +Validate `gc` function argument signature for MPC and return `true` if it is mutating. """ -function validate_gc(NT, gc) +function validate_gc_mpc(NT, gc) ismutating = hasmethod( gc, # LHS, Ue, Ŷe, D̂e, p, ϵ Tuple{Vector{NT}, Vector{NT}, Vector{NT}, Vector{NT}, Any, NT} ) + isnonmutating = hasmethod( + gc, + # Ue, Ŷe, D̂e, p, ϵ + Tuple{Vector{NT}, Vector{NT}, Vector{NT}, Any, NT} + ) # Ue, Ŷe, D̂e, p, ϵ - if !(ismutating || hasmethod(gc, Tuple{Vector{NT}, Vector{NT}, Vector{NT}, Any, NT})) + if !(ismutating || isnonmutating) error( "the custom constraint function has no method with type signature "* "gc(Ue::Vector{$(NT)}, Ŷe::Vector{$(NT)}, D̂e::Vector{$(NT)}, p::Any, ϵ::$(NT)) "* @@ -494,8 +501,8 @@ function validate_gc(NT, gc) end "Get mutating custom constraint function `gc!` from the provided function in argument." -function get_mutating_gc(NT, gc) - ismutating_gc = validate_gc(NT, gc) +function get_mutating_gc_mpc(NT, gc) + ismutating_gc = validate_gc_mpc(NT, gc) gc! = if ismutating_gc gc else @@ -508,7 +515,7 @@ function get_mutating_gc(NT, gc) end """ - test_custom_functions(NT, model::SimModel, JE, gc!, nc, Uop, Yop, Dop, p) + test_custom_function_mpc(NT, model::SimModel, JE, gc!, nc, Uop, Yop, Dop, p) Test the custom functions `JE` and `gc!` at the operating point `Uop`, `Yop`, `Dop`. @@ -516,7 +523,7 @@ This function is called at the end of `NonLinMPC` construction. It warns the use custom cost `JE` and constraint `gc!` functions crash at `model` operating points. This should ease troubleshooting of simple bugs e.g.: the user forgets to set the `nc` argument. """ -function test_custom_functions(NT, model::SimModel, JE, gc!, nc, Uop, Yop, Dop, p) +function test_custom_function_mpc(NT, model::SimModel, JE, gc!, nc, Uop, Yop, Dop, p) uop, dop, yop = model.uop, model.dop, model.yop Ue, Ŷe, D̂e = [Uop; uop], [yop; Yop], [dop; Dop] ϵ = zero(NT) @@ -729,11 +736,11 @@ function init_optimization!( mpc::NonLinMPC, model::SimModel, optim::JuMP.GenericModel{JNT} ) where JNT<:Real # --- variables and linear constraints --- - con, transcription = mpc.con, mpc.transcription + con = mpc.con nZ̃ = length(mpc.Z̃) JuMP.num_variables(optim) == 0 || JuMP.empty!(optim) JuMP.set_silent(optim) - limit_solve_time(mpc.optim, mpc.estim.model.Ts) + limit_solve_time(mpc.optim, model.Ts) @variable(optim, Z̃var[1:nZ̃]) A = con.A[con.i_b, :] b = con.b[con.i_b] @@ -742,15 +749,8 @@ function init_optimization!( beq = con.beq @constraint(optim, linconstrainteq, Aeq*Z̃var .== beq) # --- nonlinear optimization init --- - if mpc.nϵ == 1 && JuMP.solver_name(optim) == "Ipopt" - C = mpc.weights.Ñ_Hc[end] - try - JuMP.get_attribute(optim, "nlp_scaling_max_gradient") - catch - # default "nlp_scaling_max_gradient" to `10.0/C` if not already set: - JuMP.set_attribute(optim, "nlp_scaling_max_gradient", 10.0/C) - end - end + C = mpc.nϵ > 0 ? mpc.weights.Ñ_Hc[end, end] : Inf + set_scaling_gradient!(optim, C) J_op = get_nonlinobj_op(mpc, optim) g_oracle, geq_oracle = get_nonlincon_oracle(mpc, optim) @objective(optim, Min, J_op(Z̃var...)) @@ -926,7 +926,7 @@ function get_nonlincon_oracle(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JN myNaN, myInf = convert(JNT, NaN), convert(JNT, Inf) ΔŨ::Vector{JNT} = zeros(JNT, nΔŨ) x̂0end::Vector{JNT} = zeros(JNT, nx̂) - K::Vector{JNT} = zeros(JNT, nK) + K::Vector{JNT} = zeros(JNT, nK) Ue::Vector{JNT}, Ŷe::Vector{JNT} = zeros(JNT, nUe), zeros(JNT, nŶe) U0::Vector{JNT}, Ŷ0::Vector{JNT} = zeros(JNT, nU), zeros(JNT, nŶ) Û0::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nU), zeros(JNT, nX̂) @@ -1081,7 +1081,7 @@ function update_predictions!( ΔŨ = getΔŨ!(ΔŨ, mpc, transcription, Z̃) Ŷ0, x̂0end = predict!(Ŷ0, x̂0end, X̂0, Û0, K, mpc, model, transcription, U0, Z̃) Ue, Ŷe = extended_vectors!(Ue, Ŷe, mpc, U0, Ŷ0) - ϵ = getϵ(mpc, Z̃) + ϵ = getϵ(mpc, Z̃) gc = con_custom!(gc, mpc, Ue, Ŷe, ϵ) g = con_nonlinprog!(g, mpc, model, transcription, x̂0end, Ŷ0, gc, ϵ) geq = con_nonlinprogeq!(geq, X̂0, Û0, K, mpc, model, transcription, U0, Z̃) @@ -1114,7 +1114,7 @@ end Evaluate the custom inequality constraint `gc` in-place and return it. """ function con_custom!(gc, mpc::NonLinMPC, Ue, Ŷe, ϵ) - mpc.con.nc ≠ 0 && mpc.con.gc!(gc, Ue, Ŷe, mpc.D̂e, mpc.p, ϵ) + mpc.con.nc > 0 && mpc.con.gc!(gc, Ue, Ŷe, mpc.D̂e, mpc.p, ϵ) return gc end diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index fb5b61f9b..66607ae30 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1330,7 +1330,7 @@ The method mutates the `g` vectors in argument and returns it. Only the custom c `gc` are include in the `g` vector. """ function con_nonlinprog!( - g, ::PredictiveController, ::LinModel, ::TranscriptionMethod, _ , _ , gc, ϵ + g, ::PredictiveController, ::LinModel, ::TranscriptionMethod, _ , _ , gc, _ ) for i in eachindex(g) g[i] = gc[i] diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index 8efbe17f6..255c6fe4d 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -95,6 +95,7 @@ struct KalmanCovariances{ rethrow() end end + invQ̂_He = Hermitian(repeatdiag(invQ̂, He), :L) try inv!(invR̂) catch err @@ -104,10 +105,7 @@ struct KalmanCovariances{ rethrow() end end - invQ̂_He = repeatdiag(invQ̂, He) - invR̂_He = repeatdiag(invR̂, He) - invQ̂_He = Hermitian(invQ̂_He, :L) - invR̂_He = Hermitian(invR̂_He, :L) + invR̂_He = Hermitian(repeatdiag(invR̂, He), :L) return new{NT, Q̂C, R̂C}(P̂_0, P̂, Q̂, R̂, invP̄, invQ̂_He, invR̂_He) end end @@ -125,7 +123,7 @@ end """ validate_kfcov(model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0=nothing) -Validate sizes and Hermitianity of process `Q̂`` and sensor `R̂` noises covariance matrices. +Validate sizes and Hermitianity of process `Q̂` and sensor `R̂` noises covariance matrices. Also validate initial estimate covariance `P̂_0`, if provided. """ diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index fe77ab8fc..d81228d9c 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -17,12 +17,14 @@ the former is always a linear inequality constraint (it's a decision variable). `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} +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} @@ -31,6 +33,7 @@ struct EstimatorConstraint{NT<:Real} Ŵ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} @@ -40,13 +43,20 @@ struct EstimatorConstraint{NT<:Real} 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{ @@ -56,7 +66,9 @@ struct MovingHorizonEstimator{ JM<:JuMP.GenericModel, GB<:AbstractADType, JB<:AbstractADType, - HB<:Union{AbstractADType, Nothing}, + HB<:Union{AbstractADType, Nothing}, + PT<:Any, + GCfunc<:Function, CE<:KalmanEstimator, } <: StateEstimator{NT} model::SM @@ -64,7 +76,7 @@ struct MovingHorizonEstimator{ # 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} + con::EstimatorConstraint{NT, GCfunc} gradient::GB jacobian::JB hessian::HB @@ -81,6 +93,7 @@ struct MovingHorizonEstimator{ nym::Int nyu::Int nxs::Int + p::PT As ::Matrix{NT} Cs_u::Matrix{NT} Cs_y::Matrix{NT} @@ -105,11 +118,14 @@ struct MovingHorizonEstimator{ r::Vector{NT} C::NT X̂op::Vector{NT} - X̂0 ::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} @@ -119,6 +135,7 @@ struct MovingHorizonEstimator{ 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 { @@ -129,6 +146,8 @@ struct MovingHorizonEstimator{ 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 @@ -142,29 +161,32 @@ struct MovingHorizonEstimator{ Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] lastu0 = zeros(NT, nu) x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] - r = direct ? 0 : 1 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, r + 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̂ + 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) - X̂0, Y0m = zeros(NT, nx̂*He), zeros(NT, nym*He) - U0, D0 = zeros(NT, nu*He), zeros(NT, nd*(He+1)) - Ŵ = zeros(NT, nx̂*He) - buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk, He, nε) + 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] - estim = new{NT, SM, KC, JM, GB, JB, HB, CE}( + test_custom_function_mhe(NT, model, i_ym, He, gc!, nc, x̂op, p) + 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, @@ -173,13 +195,15 @@ struct MovingHorizonEstimator{ 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, X̂0, Y0m, U0, D0, Ŵ, - x̂0arr_old, P̂arr_old, Nk, + X̂op, + Y0m, Yem, U0, Ue, D0, De, Ŵ, + X̂0_old, x̂0arr_old, P̂arr_old, Nk, direct, corrected, buffer ) @@ -193,25 +217,28 @@ end Construct a moving horizon estimator (MHE) based on `model` ([`LinModel`](@ref) or [`NonLinModel`](@ref)). -It can handle constraints on the estimates, see [`setconstraint!`](@ref). 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``: +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 ``` -in which the arrival costs are evaluated from the states estimated at time ``k-N_k``: +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} ``` -and the covariances are repeated ``N_k`` times: +The covariances are repeated ``N_k`` times: ```math \begin{aligned} \mathbf{Q̂}_{N_k} &= \text{diag}\mathbf{(Q̂,Q̂,...,Q̂)} \\ @@ -226,10 +253,14 @@ N_k = \begin{cases} ``` The vectors ``\mathbf{Ŵ}`` and ``\mathbf{V̂}`` respectively encompass the estimated process noises ``\mathbf{ŵ}(k-j+p)`` from ``j=N_k`` to ``1`` and sensor noises ``\mathbf{v̂}(k-j+1)`` -from ``j=N_k`` to ``1``. The Extended Help defines the two 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. +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 @@ -267,7 +298,12 @@ transcription for now. - `σ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. -- `optim=default_optim_mhe(model)` : a [`JuMP.Model`](@extref) object with a quadratic or +- `gc=(_,_,_,_,_,_,_,_,_,_,_)->nothing` or `gc!` : custom nonlinear inequality constraint function + ``\mathbf{g_c}(\mathbf{X̂, V̂, Ŵ, U, Y^m, D, 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 @@ -342,6 +378,46 @@ MovingHorizonEstimator estimator with a sample time Ts = 10.0 s: [`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 @@ -392,14 +468,10 @@ MovingHorizonEstimator estimator with a sample time Ts = 10.0 s: ) ``` that is, it will test many coloring orders at preparation and keep the best. - - The slack variable ``ε`` relaxes the constraints if enabled, see [`setconstraint!`](@ref). - It is disabled 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 the estimated - state ``\mathbf{x̂}`` and sensor noise ``\mathbf{v̂}``). 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. + + 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; @@ -415,7 +487,11 @@ function MovingHorizonEstimator( sigmaPint_ym_0 = fill(1, max(sum(nint_ym), 0)), sigmaQint_ym = fill(1, max(sum(nint_ym), 0)), Cwt::Real = Inf, - optim::JM = default_optim_mhe(model), + 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, @@ -434,18 +510,27 @@ function MovingHorizonEstimator( 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; - direct, optim, gradient, jacobian, hessian + model, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt; + gc, gc!, nc, p, direct, optim, gradient, jacobian, hessian ) end -default_optim_mhe(::LinModel) = JuMP.Model(DEFAULT_LINMHE_OPTIMIZER, add_bridges=false) -default_optim_mhe(::SimModel) = JuMP.Model(DEFAULT_NONLINMHE_OPTIMIZER, add_bridges=false) +"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; - optim=default_optim_mhe(model), + gc!=(_,_,_,_,_,_,_,_,_,_,_) -> nothing, + gc=gc!, + nc=0, + optim=default_optim_mhe(model, nc), gradient=AutoForwardDiff(), jacobian=AutoForwardDiff(), hessian=false, @@ -466,7 +551,11 @@ of [`setstate!`](@ref) at the desired value. """ function MovingHorizonEstimator( model::SM, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt=Inf; - optim::JM = default_optim_mhe(model), + 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, @@ -475,11 +564,13 @@ function MovingHorizonEstimator( ) 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, + He, i_ym, nint_u, nint_ym, cov, Cwt, + gc!, nc, p, optim, gradient, jacobian, hessian, covestim; direct ) @@ -504,6 +595,101 @@ function validate_covestim(::KalmanCovariances, ::StateEstimator) "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) -> 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. 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) + 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, 4, 4), :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)] + gc!(gc, X̂e, V̂e, Ŵe, Ue, Yem, De, P̄, x̄, p, ε) + 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 in your function? + """, + exception=(err, catch_backtrace()) + ) + end + return nothing +end + @doc raw""" setconstraint!(estim::MovingHorizonEstimator; ) -> estim @@ -731,7 +917,8 @@ function setconstraint!( 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.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 @@ -744,17 +931,18 @@ function setconstraint!( @constraint(optim, linconstraint, A*Z̃var .≤ b) reset_nonlincon!(estim, model) else - i_b, i_g = init_matconstraint_mhe(model, + 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 calling updatestate!") + error("Cannot modify ±Inf constraints after first solve of estimation problem") end end return estim end -"By default, no nonlinear constraints, return nothing." +"By default, no nonlinear constraints or only custom ones, do and return nothing." reset_nonlincon!(::MovingHorizonEstimator, ::SimModel) = nothing """ @@ -764,11 +952,12 @@ 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, model, estim.optim, g_oracle) + set_nonlincon!(estim, estim.optim, g_oracle) end @doc raw""" - init_matconstraint_mhe(model::LinModel, + 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 @@ -781,17 +970,19 @@ The linear and nonlinear inequality constraints are respectively defined as: \mathbf{g(Z̃)} &≤ \mathbf{0} \end{aligned} ``` -`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`. +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}, +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 = BitVector() + i_g = trues(nc) if isempty(args) A = zeros(NT, length(i_b), 0) else @@ -802,11 +993,12 @@ function init_matconstraint_mhe(::LinModel{NT}, end "Init `i_b, A` without state and sensor noise constraints if `model` is not a [`LinModel`](@ref)." -function init_matconstraint_mhe(::SimModel{NT}, +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] + 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 @@ -826,8 +1018,9 @@ end 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̂ -) where {NT<:Real} + 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 @@ -847,18 +1040,20 @@ function init_defaultcon_mhe( 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, + 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}( + 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 + i_b, i_g, + gc!, nc ) return con, nε, Ẽ, ẽx̄ end @@ -1020,7 +1215,7 @@ end @doc raw""" init_predmat_mhe( - model::LinModel, He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, p + 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`. @@ -1149,11 +1344,12 @@ see [`initpred!(::MovingHorizonEstimator, ::LinModel)`](@ref) and [`linconstrain 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, p + 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) @@ -1260,10 +1456,11 @@ end "Return empty matrices if `model` is not a [`LinModel`](@ref), except for `ex̄`." function init_predmat_mhe( - model::SimModel{NT}, He, i_ym, Â, _ , _ , _ , _ , _ , _ , p + 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) @@ -1286,15 +1483,21 @@ 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̃) 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 = estim.con.A[estim.con.i_b, :] - b = estim.con.b[estim.con.i_b] + 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 + set_scaling_gradient!(optim, C) + g_oracle = get_nonlincon_oracle(estim, optim) + set_nonlincon!(estim, optim, g_oracle) + end return nothing end @@ -1315,23 +1518,16 @@ function init_optimization!( JuMP.set_silent(optim) limit_solve_time(optim, model.Ts) @variable(optim, Z̃var[1:nZ̃]) - A = estim.con.A[con.i_b, :] - b = estim.con.b[con.i_b] + A = con.A[con.i_b, :] + b = con.b[con.i_b] @constraint(optim, linconstraint, A*Z̃var .≤ b) # --- nonlinear optimization init --- - if !isinf(C) && JuMP.solver_name(optim) == "Ipopt" - try - JuMP.get_attribute(optim, "nlp_scaling_max_gradient") - catch - # default "nlp_scaling_max_gradient" to `10.0/C` if not already set: - JuMP.set_attribute(optim, "nlp_scaling_max_gradient", 10.0/C) - end - end + 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, model, optim, g_oracle) + set_nonlincon!(estim, optim, g_oracle) return nothing end @@ -1354,25 +1550,32 @@ function get_nonlinobj_op( ) where JNT<:Real model, con = estim.model, estim.con grad, hess = estim.gradient, estim.hessian - nx̂, nym, nŷ, nu, nk = estim.nx̂, estim.nym, model.ny, model.nu, model.nk + 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) - nV̂, nX̂, ng, nZ̃ = He*nym, He*nx̂, length(con.i_g), length(estim.Z̃) + 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) - V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) - k::Vector{JNT} = zeros(JNT, nk) - û0::Vector{JNT}, ŷ0::Vector{JNT} = zeros(JNT, nu), zeros(JNT, nŷ) - g::Vector{JNT} = zeros(JNT, ng) - x̄::Vector{JNT} = zeros(JNT, nx̂) - function J!(Z̃, V̂, X̂0, û0, k, ŷ0, g, x̄) - update_prediction!(V̂, X̂0, û0, k, ŷ0, g, estim, Z̃) - return obj_nonlinprog!(x̄, estim, model, V̂, Z̃) + 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(V̂), Cache(X̂0), Cache(û0), Cache(k), Cache(ŷ0), Cache(g), Cache(x̄), + 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 @@ -1461,27 +1664,39 @@ function get_nonlincon_oracle( 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) - nV̂, nX̂, nZ̃ = He*nym, He*nx̂, length(estim.Z̃) + 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) - V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) - k::Vector{JNT} = zeros(JNT, nk) - û0::Vector{JNT}, ŷ0::Vector{JNT} = zeros(JNT, nu), zeros(JNT, nŷ) - g::Vector{JNT}, gi::Vector{JNT} = zeros(JNT, ng), zeros(JNT, ngi) - λi::Vector{JNT} = rand(JNT, ngi) + 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̃, V̂, X̂0, û0, k, ŷ0, g) - update_prediction!(V̂, X̂0, û0, k, ŷ0, g, estim, Z̃) + 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, V̂, X̂0, û0, k, ŷ0, g, gi) - update_prediction!(V̂, X̂0, û0, k, ŷ0, g, estim, Z̃) + 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(V̂), Cache(X̂0), Cache(û0), Cache(k), Cache(ŷ0), Cache(g)) + ∇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) @@ -1490,7 +1705,10 @@ function get_nonlincon_oracle( ∇gi_structure = init_diffstructure(∇gi) if !isnothing(hess) ∇²gi_cache = ( - Cache(V̂), Cache(X̂0), Cache(û0), Cache(k), Cache(ŷ0), Cache(g), Cache(gi) + 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( @@ -1536,16 +1754,13 @@ function get_nonlincon_oracle( return g_oracle end -"By default, there is no nonlinear constraint, thus do nothing." -set_nonlincon!(::MovingHorizonEstimator, ::SimModel, _ , _ ) = nothing - """ - set_nonlincon!(estim::MovingHorizonEstimator, ::NonLinModel, optim, g_oracle) + set_nonlincon!(estim::MovingHorizonEstimator, optim, g_oracle) -Set the nonlinear inequality constraints for `NonLinModel`, if any. +Set the nonlinear inequality constraints of `estim`, if any. """ function set_nonlincon!( - estim::MovingHorizonEstimator, ::NonLinModel, optim::JuMP.GenericModel{JNT}, g_oracle + estim::MovingHorizonEstimator, optim::JuMP.GenericModel{JNT}, g_oracle ) where JNT<:Real Z̃var = optim[:Z̃var] nonlin_constraints = JuMP.all_constraints( diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 3a761c878..c504b6fa8 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -1,17 +1,33 @@ "Reset the data windows and time-varying variables for the moving horizon estimator." -function init_estimate_cov!(estim::MovingHorizonEstimator, _ , d0, u0) +function init_estimate_cov!(estim::MovingHorizonEstimator, y0m, d0, u0) + model = estim.model + nu, ny, nd = model.nu, model.ny, model.nd + uop, yop, dop = model.uop, model.yop, model.dop estim.Z̃ .= 0 - estim.X̂0 .= 0 - estim.Y0m .= 0 - estim.U0 .= 0 - estim.D0 .= 0 - estim.Ŵ .= 0 + estim.Y0m .= NaN + estim.Yem .= NaN + estim.U0 .= NaN + estim.Ue .= NaN + estim.D0 .= NaN + estim.De .= NaN + estim.Ŵ .= NaN + estim.X̂0_old .= NaN estim.Nk .= 0 + estim.F .= 0 estim.H̃ .= 0 estim.q̃ .= 0 estim.r .= 0 - estim.direct && (estim.U0[1:estim.model.nu] .= u0) # add u0(-1) to the data windows - estim.D0[1:estim.model.nd] .= d0 # add d0(-1) to the data windows + estim.con.Fx̂ .= 0 + if estim.direct + # add y0m(-1) to the extended data window (custom NL constraints): + estim.Yem[1:ny] .= y0m .+ @views yop[estim.i_ym] + # add u0(-1) to the two data windows: + estim.U0[1:nu] .= u0 + estim.Ue[1:nu] .= u0 .+ uop + # add d0(-1) to the extended data window (custom NL constraints): + nd > 0 && (estim.De[1:nd] .= d0 .+ dop) + end + nd > 0 && (estim.D0[1:nd] .= d0) # add d0(-1) to the data window estim.lastu0 .= u0 # estim.cov.P̂_0 is P̂(-1|-1) if estim.direct==false, else P̂(-1|0) invert_cov!(estim, estim.cov.P̂_0) @@ -123,11 +139,11 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real nx̂, nym, nŵ, nε = estim.nx̂, estim.nym, estim.nx̂, estim.nε nx̃ = nε + nx̂ info = Dict{Symbol, Any}() - V̂, X̂0 = similar(estim.Y0m[1:nym*Nk]), similar(estim.X̂0[1:nx̂*Nk]) - û0, k, ŷ0 = buffer.û, buffer.k, buffer.ŷ - V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, estim.Z̃) - x̂0arr = @views estim.Z̃[nx̃-nx̂+1:nx̃] - x̄ = estim.x̂0arr_old - x̂0arr + 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̄ = 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) @@ -148,8 +164,8 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real 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!(x̄, estim, estim.model, V̂, estim.Z̃) - info[:X̂] = X̂0 .+ @views [estim.x̂op; estim.X̂op[1:nx̂*Nk]] + 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)] info[:x̂] = estim.x̂0 .+ estim.x̂op info[:V̂] = V̂ info[:P̄] = estim.P̂arr_old @@ -189,22 +205,30 @@ function addinfo!( # --- objective derivatives --- optim, con = estim.optim, estim.con hess = estim.hessian - nx̂, nym, nŷ, nu, nk = estim.nx̂, estim.nym, model.ny, model.nu, model.nk + nx̂, nym, nŷ, nu, nk, nc = estim.nx̂, estim.nym, model.ny, model.nu, model.nk, con.nc 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) - nV̂, nX̂ = He*nym, He*nx̂ - V̂, X̂0 = zeros(NT, nV̂), zeros(NT, nX̂) - k = zeros(NT, nk) - û0, ŷ0 = zeros(NT, nu), zeros(NT, nŷ) - g, gi = zeros(NT, ng), zeros(NT, ngi) - x̄ = zeros(NT, nx̂) + nV̂, nX̂, nŴ = He*nym, He*nx̂, He*nx̂ + nŴe, nX̂e, nV̂e = (He+1)*nx̂, (He+1)*nx̂, (He+1)*nym + x̂0arr, x̄ = zeros(NT, nx̂), zeros(NT, nx̂) + Ŵ = zeros(NT, nŴ) + V̂, X̂0 = zeros(NT, nV̂), zeros(NT, nX̂) + Ŵe = zeros(NT, nŴe) + V̂e, X̂e = zeros(NT, nV̂e), zeros(NT, nX̂e) + k = zeros(NT, nk) + û0, ŷ0 = zeros(NT, nu), zeros(NT, nŷ) + gc, g = zeros(NT, nc), zeros(NT, ng) + gi = zeros(NT, ngi) J_cache = ( - Cache(V̂), Cache(X̂0), Cache(û0), Cache(k), Cache(ŷ0), Cache(g), Cache(x̄), + 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), ) - function J!(Z̃, V̂, X̂0, û0, k, ŷ0, g, x̄) - update_prediction!(V̂, X̂0, û0, k, ŷ0, g, estim, Z̃) - return obj_nonlinprog!(x̄, estim, model, V̂, Z̃) + 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 if !isnothing(hess) prep_∇²J = prepare_hessian(J!, hess, estim.Z̃, J_cache...) @@ -216,9 +240,14 @@ function addinfo!( ∇²J_opt, ∇²J_ncolors = nothing, nothing end # --- inequality constraint derivatives --- - ∇g_cache = (Cache(V̂), Cache(X̂0), Cache(û0), Cache(k), Cache(ŷ0), Cache(g)) - function gi!(gi, Z̃, V̂, X̂0, û0, k, ŷ0, g) - update_prediction!(V̂, X̂0, û0, k, ŷ0, g, estim, Z̃) + ∇g_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), + ) + 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 @@ -241,10 +270,13 @@ function addinfo!( end end ∇²g_cache = ( - Cache(V̂), Cache(X̂0), Cache(û0), Cache(k), Cache(ŷ0), Cache(g), Cache(gi) + 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) ) - function ℓ_gi(Z̃, λi, V̂, X̂0, û0, k, ŷ0, g, gi) - update_prediction!(V̂, X̂0, û0, k, ŷ0, g, estim, Z̃) + 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 @@ -276,13 +308,25 @@ 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̂ + return x̂0arr .= @views Z̃[nx̃-estim.nx̂+1:nx̃] +end + +"Get the estimated process noise over the horizon from the decision vector `Z̃`." +function getŴ!(Ŵ, estim::MovingHorizonEstimator, Z̃) + nx̃ = estim.nε + estim.nx̂ + return Ŵ .= @views Z̃[(nx̃ + 1):(nx̃ + estim.nx̂*estim.He)] +end + """ getε(estim::MovingHorizonEstimator, Z̃) -> ε Get the slack `ε` from the decision vector `Z̃` if present, otherwise return 0. """ function getε(estim::MovingHorizonEstimator, Z̃::AbstractVector{NT}) where NT<:Real - return estim.nε ≠ 0 ? Z̃[begin] : zero(NT) + return estim.nε > 0 ? Z̃[begin] : zero(NT) end """ @@ -293,39 +337,54 @@ Add data to the observation windows of the moving horizon estimator and clamp `e If ``k ≥ H_e``, the observation windows are moving in time and `estim.Nk` is clamped to `estim.He`. It returns `true` if the observation windows are moving, `false` otherwise. If no `u0` argument is provided, the manipulated input of the last time step is added to its -window (the correct value if `estim.direct == true`). +window (the correct value if `estim.direct`). """ function add_data_windows!(estim::MovingHorizonEstimator, y0m, d0, u0=estim.lastu0) model = estim.model nx̂, nym, nd, nu, nŵ = estim.nx̂, estim.nym, model.nd, model.nu, estim.nx̂ + yopm = @views model.yop[estim.i_ym] Nk = estim.Nk[] - p = estim.direct ? 0 : 1 - x̂0, ŵ = estim.x̂0, 0 # ŵ(k-1+p) = 0 for warm-start + p = estim.direct ? 0 : 1 # u0 argument is u0(k-1) if estim.direct, else u0(k) + x̂0_old = estim.x̂0 # x̂0_old is x̂0(k-1|k-1) if estim.direct, else x̂0(k|k-1) + ŵ = 0 # ŵ(k-1+p) = 0 for warm-start estim.Nk .+= 1 Nk = estim.Nk[] ismoving = (Nk > estim.He) + # --- data windows for the predictions --- + # see MovingHorzionEstimator extended help for the exact time steps in each data window if ismoving - estim.Y0m[1:end-nym] .= @views estim.Y0m[nym+1:end] - estim.Y0m[end-nym+1:end] .= y0m + estim.Y0m[1:end-nym] .= @views estim.Y0m[nym+1:end] + estim.Yem[1:end-nym] .= @views estim.Yem[nym+1:end] + estim.Y0m[end-nym+1:end] .= y0m + estim.Yem[(end-nym+1 - p*nym):(end - p*nym)] .= y0m .+ yopm if nd > 0 estim.D0[1:end-nd] .= @views estim.D0[nd+1:end] - estim.D0[end-nd+1:end] .= d0 + estim.De[1:end-nd] .= @views estim.De[nd+1:end] + estim.D0[end-nd+1:end] .= d0 + estim.De[(end-nd+1 - p*nd):(end - p*nd)] .= d0 .+ model.dop end - estim.U0[1:end-nu] .= @views estim.U0[nu+1:end] - estim.U0[end-nu+1:end] .= u0 - estim.X̂0[1:end-nx̂] .= @views estim.X̂0[nx̂+1:end] - estim.X̂0[end-nx̂+1:end] .= x̂0 - estim.Ŵ[1:end-nŵ] .= @views estim.Ŵ[nŵ+1:end] - estim.Ŵ[end-nŵ+1:end] .= ŵ + estim.U0[1:end-nu] .= @views estim.U0[nu+1:end] + estim.Ue[1:end-nu] .= @views estim.Ue[nu+1:end] + estim.U0[end-nu+1:end] .= u0 + estim.Ue[(end-nu+1 - nu):(end - nu)] .= u0 .+ model.uop + estim.Ŵ[1:end-nŵ] .= @views estim.Ŵ[nŵ+1:end] + estim.Ŵ[end-nŵ+1:end] .= ŵ + estim.X̂0_old[1:end-nx̂] .= @views estim.X̂0_old[nx̂+1:end] + estim.X̂0_old[end-nx̂+1:end] .= x̂0_old estim.Nk .= estim.He else - estim.Y0m[(1 + nym*(Nk-1)):(nym*Nk)] .= y0m - nd > 0 && (estim.D0[(1 + nd*Nk):(nd*(Nk+1))] .= d0) - estim.U0[(1 + nu*(Nk-1)):(nu*Nk)] .= u0 - estim.X̂0[(1 + nx̂*(Nk-1)):(nx̂*Nk)] .= x̂0 - estim.Ŵ[(1 + nŵ*(Nk-1)):(nŵ*Nk)] .= ŵ + estim.Y0m[(1 + nym*(Nk-1)):(nym*Nk)] .= y0m + estim.Yem[(1 + nym*(Nk-p)):(nym*(Nk-p+1))] .= y0m .+ yopm + if nd > 0 + estim.D0[(1 + nd*Nk):(nd*(Nk+1))] .= d0 + estim.De[(1 + nd*(Nk-p)):(nd*(Nk-p+1))] .= d0 .+ model.dop + end + estim.U0[(1 + nu*(Nk-1)):(nu*Nk)] .= u0 + estim.Ue[(1 + nu*(Nk-1)):(nu*Nk)] .= u0 .+ model.uop + estim.Ŵ[(1 + nŵ*(Nk-1)):(nŵ*Nk)] .= ŵ + estim.X̂0_old[(1 + nx̂*(Nk-1)):(nx̂*Nk)] .= x̂0_old end - estim.x̂0arr_old .= @views estim.X̂0[1:nx̂] + estim.x̂0arr_old .= @views estim.X̂0_old[1:nx̂] return ismoving end @@ -361,33 +420,51 @@ function initpred!(estim::MovingHorizonEstimator, model::LinModel) invP̄, invQ̂_He, invR̂_He = estim.cov.invP̄, estim.cov.invQ̂_He, estim.cov.invR̂_He F, C, optim = estim.F, estim.C, estim.optim nx̂, nŵ, nym, nε, Nk = estim.nx̂, estim.nx̂, estim.nym, estim.nε, estim.Nk[] - nYm, nŴ = nym*Nk, nŵ*Nk + nU, nYm, nŴ, nD = model.nu*Nk, estim.nym*Nk, nŵ*Nk, model.nd*(Nk+1) nZ̃ = nε + nx̂ + nŴ - # --- update F and fx̄ vectors for MHE predictions --- - F .= estim.Y0m .+ estim.B - mul!(F, estim.G, estim.U0, 1, 1) - if model.nd > 0 - mul!(F, estim.J, estim.D0, 1, 1) + # --- truncate vector and matrices if necessary --- + if Nk < estim.He + # avoid views since allocations only when Nk < He and we want fast mul!: + Y0m, B = estim.Y0m[1:nYm], estim.B[1:nYm] + G, U0 = estim.G[1:nYm, 1:nU], estim.U0[1:nU] + J, D0 = estim.J[1:nYm, 1:nD], estim.D0[1:nD] + Ẽ, ẽx̄ = estim.Ẽ[1:nYm, 1:nZ̃], estim.ẽx̄[:, 1:nZ̃] + F, q̃ = @views estim.F[1:nYm], estim.q̃[1:nZ̃] + H̃_data = @views estim.H̃.data[1:nZ̃, 1:nZ̃] + H̃ = @views estim.H̃[1:nZ̃, 1:nZ̃] + Z̃var = @views optim[:Z̃var][1:nZ̃] + else + Y0m, B = estim.Y0m, estim.B + G, U0 = estim.G, estim.U0 + J, D0 = estim.J, estim.D0 + Ẽ, ẽx̄ = estim.Ẽ, estim.ẽx̄ + F, q̃ = estim.F, estim.q̃ + H̃_data = estim.H̃.data + H̃ = estim.H̃ + Z̃var = optim[:Z̃var] end - estim.fx̄ .= estim.x̂0arr_old + invQ̂_Nk = trunc_cov(invQ̂_He, nx̂, Nk, estim.He) + invR̂_Nk = trunc_cov(invR̂_He, nym, Nk, estim.He) + fx̄ = estim.fx̄ + r = estim.r + # --- update F and fx̄ vectors for MHE predictions --- + F .= Y0m .+ B + mul!(F, G, U0, 1, 1) + (model.nd > 0) && mul!(F, J, D0, 1, 1) + fx̄ .= estim.x̂0arr_old # --- update H̃, q̃ and p vectors for quadratic optimization --- - ẼZ̃ = @views [estim.ẽx̄[:, 1:nZ̃]; estim.Ẽ[1:nYm, 1:nZ̃]] - FZ̃ = @views [estim.fx̄; estim.F[1:nYm]] - invQ̂_Nk = trunc_cov(estim.cov.invQ̂_He, estim.nx̂, Nk, estim.He) - invR̂_Nk = trunc_cov(estim.cov.invR̂_He, estim.nym, Nk, estim.He) + ẼZ̃ = [ẽx̄; Ẽ] + FZ̃ = [fx̄; F] M_Nk = [invP̄ zeros(nx̂, nYm); zeros(nYm, nx̂) invR̂_Nk] Ñ_Nk = [fill(C, nε, nε) zeros(nε, nx̂+nŴ); zeros(nx̂, nε+nx̂+nŴ); zeros(nŴ, nε+nx̂) invQ̂_Nk] M_Nk_ẼZ̃ = M_Nk*ẼZ̃ - @views mul!(estim.q̃[1:nZ̃], M_Nk_ẼZ̃', FZ̃) - @views lmul!(2, estim.q̃[1:nZ̃]) - estim.r .= dot(FZ̃, M_Nk, FZ̃) - estim.H̃.data[1:nZ̃, 1:nZ̃] .= Ñ_Nk - @views mul!(estim.H̃.data[1:nZ̃, 1:nZ̃], ẼZ̃', M_Nk_ẼZ̃, 1, 1) - @views lmul!(2, estim.H̃.data[1:nZ̃, 1:nZ̃]) - Z̃var_Nk::Vector{JuMP.VariableRef} = @views optim[:Z̃var][1:nZ̃] - H̃_Nk = @views estim.H̃[1:nZ̃,1:nZ̃] - q̃_Nk = @views estim.q̃[1:nZ̃] - JuMP.set_objective_function(optim, obj_quadprog(Z̃var_Nk, H̃_Nk, q̃_Nk)) + mul!(q̃, M_Nk_ẼZ̃', FZ̃) + lmul!(2, q̃) + r .= dot(FZ̃, M_Nk, FZ̃) + H̃_data .= Ñ_Nk + mul!(H̃_data, ẼZ̃', M_Nk_ẼZ̃, 1, 1) + lmul!(2, H̃_data) + JuMP.set_objective_function(optim, obj_quadprog(Z̃var, H̃, q̃)) return nothing end "Does nothing if `model` is not a [`LinModel`](@ref)." @@ -402,33 +479,47 @@ Also init ``\mathbf{F_x̂ = G_x̂ U_0 + J_x̂ D_0 + B_x̂}`` vector for the stat [`init_predmat_mhe`](@ref). """ function linconstraint!(estim::MovingHorizonEstimator, model::LinModel) - Fx̂ = estim.con.Fx̂ - Fx̂ .= estim.con.Bx̂ - mul!(Fx̂, estim.con.Gx̂, estim.U0, 1, 1) - if model.nd > 0 - mul!(Fx̂, estim.con.Jx̂, estim.D0, 1, 1) - end - X̂0min, X̂0max = trunc_bounds(estim, estim.con.X̂0min, estim.con.X̂0max, estim.nx̂) - Ŵmin, Ŵmax = trunc_bounds(estim, estim.con.Ŵmin, estim.con.Ŵmax, estim.nx̂) - V̂min, V̂max = trunc_bounds(estim, estim.con.V̂min, estim.con.V̂max, estim.nym) - nX̂, nŴ, nV̂ = length(X̂0min), length(Ŵmin), length(V̂min) + nx̂, nŵ, nym, Nk = estim.nx̂, estim.nx̂, estim.nym, estim.Nk[] + nU, nX̂, nD = model.nu*Nk, estim.nx̂*Nk, model.nd*(Nk+1) + # --- truncate vector and matrices if necessary --- + if Nk < estim.He + # avoid views since allocations only when Nk < He and we want fast mul!: + Bx̂ = estim.con.Bx̂[1:nX̂] + Gx̂, U0 = estim.con.Gx̂[1:nX̂, 1:nU], estim.U0[1:nU] + Jx̂, D0 = estim.con.Jx̂[1:nX̂, 1:nD], estim.D0[1:nD] + Fx̂ = @views estim.con.Fx̂[1:nX̂] + else + Bx̂ = estim.con.Bx̂ + Gx̂, U0 = estim.con.Gx̂, estim.U0 + Jx̂, D0 = estim.con.Jx̂, estim.D0 + Fx̂ = estim.con.Fx̂ + end + X̂0min, X̂0max = trunc_bounds(estim, estim.con.X̂0min, estim.con.X̂0max, nx̂) + Ŵmin, Ŵmax = trunc_bounds(estim, estim.con.Ŵmin, estim.con.Ŵmax, nŵ) + V̂min, V̂max = trunc_bounds(estim, estim.con.V̂min, estim.con.V̂max, nym) + # --- update Fx̂ vectors for MHE state constraints --- + Fx̂ .= Bx̂ + mul!(Fx̂, Gx̂, U0, 1, 1) + model.nd > 0 && mul!(Fx̂, Jx̂, D0, 1, 1) + # --- update b vector for linear inequality constraints --- + nX̂_He, nŴ_He, nV̂_He = length(X̂0min), length(Ŵmin), length(V̂min) nx̃ = length(estim.con.x̃0min) n = 0 estim.con.b[(n+1):(n+nx̃)] .= @. -estim.con.x̃0min n += nx̃ estim.con.b[(n+1):(n+nx̃)] .= @. +estim.con.x̃0max n += nx̃ - estim.con.b[(n+1):(n+nX̂)] .= @. -X̂0min + Fx̂ - n += nX̂ - estim.con.b[(n+1):(n+nX̂)] .= @. +X̂0max - Fx̂ - n += nX̂ - estim.con.b[(n+1):(n+nŴ)] .= @. -Ŵmin - n += nŴ - estim.con.b[(n+1):(n+nŴ)] .= @. +Ŵmax - n += nŴ - estim.con.b[(n+1):(n+nV̂)] .= @. -V̂min + estim.F - n += nV̂ - estim.con.b[(n+1):(n+nV̂)] .= @. +V̂max - estim.F + estim.con.b[(n+1):(n+nX̂_He)] .= @. -X̂0min + estim.con.Fx̂ + n += nX̂_He + estim.con.b[(n+1):(n+nX̂_He)] .= @. +X̂0max - estim.con.Fx̂ + n += nX̂_He + estim.con.b[(n+1):(n+nŴ_He)] .= @. -Ŵmin + n += nŴ_He + estim.con.b[(n+1):(n+nŴ_He)] .= @. +Ŵmax + n += nŴ_He + estim.con.b[(n+1):(n+nV̂_He)] .= @. -V̂min + estim.F + n += nV̂_He + estim.con.b[(n+1):(n+nV̂_He)] .= @. +V̂max - estim.F if any(estim.con.i_b) lincon = estim.optim[:linconstraint] JuMP.set_normalized_rhs(lincon, estim.con.b[estim.con.i_b]) @@ -438,16 +529,18 @@ end "Set `b` excluding state and sensor noise bounds if `model` is not a [`LinModel`](@ref)." function linconstraint!(estim::MovingHorizonEstimator, ::SimModel) + # --- truncate vector and matrices if necessary --- Ŵmin, Ŵmax = trunc_bounds(estim, estim.con.Ŵmin, estim.con.Ŵmax, estim.nx̂) - nx̃, nŴ = length(estim.con.x̃0min), length(Ŵmin) + # --- update b vector for linear inequality constraints --- + nx̃, nŴ_He = length(estim.con.x̃0min), length(Ŵmin) n = 0 estim.con.b[(n+1):(n+nx̃)] .= @. -estim.con.x̃0min n += nx̃ estim.con.b[(n+1):(n+nx̃)] .= @. +estim.con.x̃0max n += nx̃ - estim.con.b[(n+1):(n+nŴ)] .= @. -Ŵmin - n += nŴ - estim.con.b[(n+1):(n+nŴ)] .= @. +Ŵmax + estim.con.b[(n+1):(n+nŴ_He)] .= @. -Ŵmin + n += nŴ_He + estim.con.b[(n+1):(n+nŴ_He)] .= @. +Ŵmax if any(estim.con.i_b) lincon = estim.optim[:linconstraint] JuMP.set_normalized_rhs(lincon, estim.con.b[estim.con.i_b]) @@ -524,12 +617,13 @@ function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real estim.Z̃ .= JuMP.value.(Z̃var) end # --------- update estimate ----------------------- - û0, ŷ0, k = buffer.û, buffer.ŷ, buffer.k + x̂0arr, û0, ŷ0, k = buffer.x̂, buffer.û, buffer.ŷ, buffer.k + V̂, X̂0 = buffer.V̂, buffer.X̂ estim.Ŵ[1:nŵ*Nk] .= @views estim.Z̃[nx̃+1:nx̃+nŵ*Nk] # update Ŵ with optimum for warm-start - V̂, X̂0 = estim.buffer.V̂, estim.buffer.X̂ - predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, estim.Z̃) - x̂0next = @views X̂0[Nk*nx̂-nx̂+1:Nk*nx̂] - estim.x̂0 .= x̂0next + getarrival!(x̂0arr, estim, estim.Z̃) + predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, x̂0arr, estim.Ŵ, estim.Z̃) + x̂0corrORnext = @views X̂0[((Nk-1)*nx̂+1):(Nk*nx̂)] + estim.x̂0 .= x̂0corrORnext return estim.Z̃ end @@ -571,10 +665,11 @@ function set_warmstart_mhe!(estim::MovingHorizonEstimator{NT}, Z̃var) where NT< Z̃s[nx̃+1:end] = estim.Ŵ # verify definiteness of objective function: V̂, X̂0 = estim.buffer.V̂, estim.buffer.X̂ - predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, Z̃s) - Js = obj_nonlinprog!(x̄, estim, model, V̂, Z̃s) + x̄ .= 0 # x̂0arr == x̂arr_old implies the error at arrival x̄ is zero + predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, estim.x̂0arr_old, estim.Ŵ, Z̃s) + Js = obj_nonlinprog(estim, model, x̄, V̂, estim.Ŵ, Z̃s) if !isfinite(Js) - Z̃s[nx̃+1:end] = 0 + Z̃s[nx̃+1:end] .= 0 end # --- unused variable in Z̃ (applied only when Nk ≠ He) --- # We force the update of the NLP gradient and jacobian by warm-starting the unused @@ -676,45 +771,41 @@ function invert_cov!(estim::MovingHorizonEstimator, P̄) end """ - obj_nonlinprog!( _ , estim::MovingHorizonEstimator, ::LinModel, _ , Z̃) + obj_nonlinprog(estim::MovingHorizonEstimator, ::LinModel, _ , _ , _ , Z̃) Objective function of [`MovingHorizonEstimator`](@ref) when `model` is a [`LinModel`](@ref). It can be called on a [`MovingHorizonEstimator`](@ref) object to evaluate the objective -function at specific `Z̃` and `V̂` values. +function at specific `Z̃`. """ -function obj_nonlinprog!( - _ , estim::MovingHorizonEstimator, ::LinModel, _ , Z̃::AbstractVector{NT} -) where NT<:Real +function obj_nonlinprog(estim::MovingHorizonEstimator, ::LinModel, _ , _ , _ , Z̃) return obj_quadprog(Z̃, estim.H̃, estim.q̃) + estim.r[] end """ - obj_nonlinprog!(x̄, estim::MovingHorizonEstimator, model::SimModel, V̂, Z̃) + obj_nonlinprog(estim::MovingHorizonEstimator, model::SimModel, x̄, V̂, Ŵ, Z̃) 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. +The function `dot(x, A, x)` is a performant way of calculating `x'*A*x`. """ -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.cov.invP̄ - nx̃ = nε + nx̂ +function obj_nonlinprog(estim::MovingHorizonEstimator, ::SimModel, x̄, V̂, Ŵ, Z̃) + Nk = estim.Nk[] + invP̄ = estim.cov.invP̄ invQ̂_Nk = trunc_cov(estim.cov.invQ̂_He, estim.nx̂, Nk, estim.He) invR̂_Nk = trunc_cov(estim.cov.invR̂_He, estim.nym, Nk, estim.He) - 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 : zero(NT) + if Nk < estim.He + nŴ, nYm = Nk*estim.nx̂, Nk*estim.nym + Ŵ, V̂ = Ŵ[1:nŴ], V̂[1:nYm] + end + Jε = estim.nε > 0 ? estim.C*Z̃[begin]^2 : 0 return dot(x̄, invP̄, x̄) + dot(Ŵ, invQ̂_Nk, Ŵ) + dot(V̂, invR̂_Nk, V̂) + Jε end - - @doc raw""" - predict_mhe!(V̂, X̂0, _, _, _, estim::MovingHorizonEstimator, model::LinModel, Z̃) -> V̂, X̂0 + predict_mhe!( + V̂, X̂0, _, _, _, estim::MovingHorizonEstimator, model::LinModel, _ , _ , Z̃ + ) -> V̂, X̂0 Compute the `V̂` vector and `X̂0` vectors for the `MovingHorizonEstimator` and `LinModel`. @@ -728,17 +819,32 @@ noises from ``k-N_k+1`` to ``k``. The `X̂0` vector is estimated states from ``k \end{aligned} ``` """ -function predict_mhe!(V̂, X̂0, _ , _ , _ , estim::MovingHorizonEstimator, ::LinModel, Z̃) +function predict_mhe!( + V̂, X̂0, _ , _ , _ , estim::MovingHorizonEstimator, ::LinModel, _ , _ , Z̃ +) nε, Nk = estim.nε, estim.Nk[] - nX̂, nŴ, nYm = estim.nx̂*Nk, estim.nx̂*Nk, estim.nym*Nk - nZ̃ = nε + estim.nx̂ + nŴ - V̂[1:nYm] .= @views estim.Ẽ[1:nYm, 1:nZ̃]*Z̃[1:nZ̃] + estim.F[1:nYm] - X̂0[1:nX̂] .= @views estim.con.Ẽx̂[1:nX̂, 1:nZ̃]*Z̃[1:nZ̃] + estim.con.Fx̂[1:nX̂] + if Nk < estim.He + # avoid views since allocations only when Nk < He and we want fast mul!: + nX̂, nŴ, nYm = estim.nx̂*Nk, estim.nx̂*Nk, estim.nym*Nk + nZ̃ = nε + estim.nx̂ + nŴ + Ẽ, F = estim.Ẽ[1:nYm, 1:nZ̃], estim.F[1:nYm] + Ẽx̂, Fx̂ = estim.con.Ẽx̂[1:nX̂, 1:nZ̃], estim.con.Fx̂[1:nX̂] + Z̃ = Z̃[1:nZ̃] + V̂_res, X̂0_res = @views V̂[1:nYm], X̂0[1:nX̂] + else + Ẽ, F = estim.Ẽ, estim.F + Ẽx̂, Fx̂ = estim.con.Ẽx̂, estim.con.Fx̂ + V̂_res, X̂0_res = V̂, X̂0 + end + V̂_res .= mul!(V̂_res, Ẽ, Z̃) .+ F + X̂0_res .= mul!(X̂0_res, Ẽx̂, Z̃) .+ Fx̂ return V̂, X̂0 end @doc raw""" - predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim::MovingHorizonEstimator, model::SimModel, Z̃) -> V̂, X̂0 + predict_mhe!( + V̂, X̂0, û0, k, ŷ0, estim::MovingHorizonEstimator, model::SimModel, x̂0arr, Ŵ, _ + ) -> V̂, X̂0 Compute the vectors when `model` is *not* a [`LinModel`](@ref). @@ -746,17 +852,17 @@ The function mutates `V̂`, `X̂0`, `û0` and `ŷ0` vector arguments. The augm [`f̂!`](@ref) and [`ĥ!`](@ref) is called recursively in a `for` loop from ``j=1`` to ``N_k``, and by adding the estimated process noise ``\mathbf{ŵ}``. """ -function predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim::MovingHorizonEstimator, model::SimModel, Z̃) - nε, Nk = estim.nε, estim.Nk[] - nu, nd, nx̂, nŵ, nym = model.nu, model.nd, estim.nx̂, estim.nx̂, estim.nym - nx̃ = nε + nx̂ - x̂0 = @views Z̃[nx̃-nx̂+1:nx̃] - if estim.direct # p = 0 +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 + if estim.direct # p = 0 ŷ0next = ŷ0 d0 = @views estim.D0[1:nd] for j=1:Nk u0 = @views estim.U0[ (1 + nu * (j-1)):(nu*j)] - ŵ = @views Z̃[(1 + nx̃ + nŵ*(j-1)):(nx̃ + nŵ*j)] + ŵ = @views Ŵ[(1 + nŵ*(j-1)):(nŵ*j)] x̂0next = @views X̂0[(1 + nx̂ *(j-1)):(nx̂ *j)] f̂!(x̂0next, û0, k, estim, model, x̂0, u0, d0) x̂0next .+= ŵ @@ -767,12 +873,12 @@ function predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim::MovingHorizonEstimator, m V̂[(1 + nym*(j-1)):(nym*j)] .= y0nextm .- ŷ0nextm x̂0, d0 = x̂0next, d0next end - else # p = 1 + else # p = 1 for j=1:Nk y0m = @views estim.Y0m[(1 + nym * (j-1)):(nym*j)] u0 = @views estim.U0[ (1 + nu * (j-1)):(nu*j)] d0 = @views estim.D0[ (1 + nd*j):(nd*(j+1))] # 1st one is d(k-Nk), not used - ŵ = @views Z̃[(1 + nx̃ + nŵ*(j-1)):(nx̃ + nŵ*j)] + ŵ = @views Ŵ[(1 + nŵ*(j-1)):(nŵ*j)] ĥ!(ŷ0, estim, model, x̂0, d0) ŷ0m = @views ŷ0[estim.i_ym] V̂[(1 + nym*(j-1)):(nym*j)] .= y0m .- ŷ0m @@ -787,26 +893,89 @@ end """ - update_predictions!(V̂, X̂0, û0, k, ŷ0, g, estim::MovingHorizonEstimator, Z̃) + update_predictions!( + x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, û0, k, ŷ0, gc, g, + estim::MovingHorizonEstimator, Z̃ + ) -> nothing Update in-place the vectors for the predictions of `estim` estimator at decision vector `Z̃`. The method mutates all the arguments before `estim` argument. """ -function update_prediction!(V̂, X̂0, û0, k, ŷ0, g, estim::MovingHorizonEstimator, Z̃) - model = estim.model - V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, Z̃) - ε = getε(estim, Z̃) - g = con_nonlinprog_mhe!(g, estim, model, X̂0, V̂, ε) +function update_prediction!( + x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, û0, k, ŷ0, gc, g, estim::MovingHorizonEstimator, Z̃ +) + x̂0arr = getarrival!(x̂0arr, estim, Z̃) + x̄ .= estim.x̂0arr_old .- x̂0arr + Ŵ = getŴ!(Ŵ, estim, Z̃) + V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, estim.model, x̂0arr, Ŵ, Z̃) + Ŵe, V̂e, X̂e = extended_vectors!(Ŵe, V̂e, X̂e, estim, Ŵ, V̂, X̂0, x̂0arr) + ε = getε(estim, Z̃) + gc = con_custom_mhe!(gc, estim, X̂e, V̂e, Ŵe, x̄, ε) + g = con_nonlinprog_mhe!(g, estim, estim.model, X̂0, V̂, gc, ε) return nothing end """ - con_nonlinprog_mhe!(g, estim::MovingHorizonEstimator, model::SimModel, X̂0, V̂, ε) -> g + extended_vectors!( + Ŵe, V̂e, X̂e, estim::MovingHorizonEstimator, Ŵ, V̂, X̂0, x̂0arr + ) -> Ŵe, V̂e, X̂e + +Compute the extended `Ŵe, V̂e` and `X̂e` vectors for NLP using the `Ŵ, V̂` and `X̂0` vectors. + +See [`MovingHorizonEstimator`](@ref) for the definition of the vectors, the exact time +steps of the samples in them and the missing values with `NaN`s. The method mutates all +the arguments before `estim` argument. +""" +function extended_vectors!(Ŵe, V̂e, X̂e, estim::MovingHorizonEstimator, Ŵ, V̂, X̂0, x̂0arr) + nym, nŵ, nx̂ = estim.nym, estim.nx̂, estim.nx̂ + Ŵe[1:end-nŵ] .= Ŵ + Ŵe[end-nŵ+1:end] .= NaN + X̂e[1:nx̂] .= x̂0arr .+ estim.x̂op + X̂e[nx̂+1:end] .= X̂0 .+ estim.X̂op + if estim.direct + V̂e[1:nym] .= NaN + V̂e[1+nym:end] .= V̂ + else + V̂e[1:end-nym] .= V̂ + V̂e[end-nym+1:end] .= NaN + end + return Ŵe, V̂e, X̂e +end + + +""" + con_custom_mhe!(gc, estim::MovingHorizonEstimator, X̂e, V̂e, Ŵe, x̄, ε) -> gc + +Evaluate the custom inequality constraint `gc` in-place for [`MovingHorizonEstimator`](@ref). +""" +function con_custom_mhe!(gc, estim::MovingHorizonEstimator, X̂e, V̂e, Ŵe, x̄, ε) + if estim.con.nc > 0 + P̄ = estim.P̂arr_old + Nk = estim.Nk[] + Ue, Yem, De = estim.Ue, estim.Yem, estim.De + if Nk < estim.He + # avoid views since allocations only when Nk < He and we want fast mul!: + nX̂e, nŴe, nYem = (Nk+1)*estim.nx̂, (Nk+1)*estim.nx̂, (Nk+1)*estim.nym + nUe, nDe = (Nk+1)*estim.model.nu, (Nk+1)*estim.model.nd + Ue, Yem, De = estim.Ue[1:nUe], estim.Yem[1:nYem], estim.De[1:nDe] + X̂e, V̂e, Ŵe = X̂e[1:nX̂e], V̂e[1:nYem], Ŵe[1:nŴe] + else + Ue, Yem, De = estim.Ue, estim.Yem, estim.De + end + estim.con.gc!(gc, X̂e, V̂e, Ŵe, Ue, Yem, De, P̄, x̄, estim.p, ε) + end + return gc +end + +""" + con_nonlinprog_mhe!( + g, estim::MovingHorizonEstimator, model::SimModel, X̂0, V̂, gc, ε + ) -> g Compute nonlinear constrains `g` in-place for [`MovingHorizonEstimator`](@ref). """ -function con_nonlinprog_mhe!(g, estim::MovingHorizonEstimator, ::SimModel, X̂0, V̂, ε) +function con_nonlinprog_mhe!(g, estim::MovingHorizonEstimator, ::SimModel, X̂0, V̂, gc, ε) nX̂con, nX̂ = length(estim.con.X̂0min), estim.nx̂ *estim.Nk[] nV̂con, nV̂ = length(estim.con.V̂min), estim.nym*estim.Nk[] for i in eachindex(g) @@ -823,17 +992,32 @@ function con_nonlinprog_mhe!(g, estim::MovingHorizonEstimator, ::SimModel, X̂0, j = i - 2nX̂con jcon = nV̂con-nV̂+j g[i] = j > nV̂ ? 0 : estim.con.V̂min[jcon] - V̂[j] - ε*estim.con.C_v̂min[jcon] - else + elseif i ≤ 2nX̂con + 2nV̂con j = i - 2nX̂con - nV̂con jcon = nV̂con-nV̂+j g[i] = j > nV̂ ? 0 : V̂[j] - estim.con.V̂max[jcon] - ε*estim.con.C_v̂max[jcon] + else + j = i - 2nX̂con - 2nV̂con + g[i] = gc[j] end end return g end -"No nonlinear constraints if `model` is a [`LinModel`](@ref), return `g` unchanged." -con_nonlinprog_mhe!(g, ::MovingHorizonEstimator, ::LinModel, _ , _ , _ ) = g +""" + con_nonlinprog_mhe!(g, ::MovingHorizonEstimator, ::LinModel, _ , _ , gc, _ ) + +Compute the same but for [`LinModel`](@ref). + +The nonlinear custom inequality constraints in `gc` are the only nonlinear constraints +for this case. +""" +function con_nonlinprog_mhe!(g, ::MovingHorizonEstimator, ::LinModel, _ , _ , gc , _ ) + for i in eachindex(g) + g[i] = gc[i] + end + return g +end "Throw an error if P̂ != nothing." function setstate_cov!(::MovingHorizonEstimator, P̂) @@ -864,11 +1048,10 @@ function setmodel_estimator!( estim.f̂op .= f̂op estim.x̂0 .-= estim.x̂op # convert x̂ to x̂0 with the new operating point # --- predictions matrices --- - p = estim.direct ? 0 : 1 E, G, J, B, _ , Ex̂, Gx̂, Jx̂, Bx̂ = init_predmat_mhe( model, He, estim.i_ym, estim.Â, estim.B̂u, estim.Ĉm, estim.B̂d, estim.D̂dm, - estim.x̂op, estim.f̂op, p + estim.x̂op, estim.f̂op, estim.direct ) A_X̂min, A_X̂max, Ẽx̂ = relaxX̂(model, nε, con.C_x̂min, con.C_x̂max, Ex̂) A_V̂min, A_V̂max, Ẽ = relaxV̂(model, nε, con.C_v̂min, con.C_v̂max, E) @@ -917,23 +1100,23 @@ function setmodel_estimator!( JuMP.unregister(estim.optim, :linconstraint) @constraint(estim.optim, linconstraint, A*Z̃var .≤ b) # --- data windows --- - for i in 1:He - # convert x̂0 to x̂ with the old operating point: - estim.X̂0[(1+nx̂*(i-1)):(nx̂*i)] .+= x̂op_old + for i in 1:He # convert y0m to ym with the old operating point: - estim.Y0m[(1+nym*(i-1)):(nym*i)] .+= @views yop_old[estim.i_ym] + estim.Y0m[(1+nym*(i-1)):(nym*i)] .+= @views yop_old[estim.i_ym] # convert u0 to u with the old operating point: - estim.U0[(1+nu*(i-1)):(nu*i)] .+= uop_old + estim.U0[(1+nu*(i-1)):(nu*i)] .+= uop_old # convert d0 to d with the old operating point: - estim.D0[(1+nd*(i-1)):(nd*i)] .+= dop_old - # convert x̂ to x̂0 with the new operating point: - estim.X̂0[(1+nx̂*(i-1)):(nx̂*i)] .-= x̂op + estim.D0[(1+nd*(i-1)):(nd*i)] .+= dop_old + # convert x̂0 to x̂ with the old operating point: + estim.X̂0_old[(1+nx̂*(i-1)):(nx̂*i)] .+= x̂op_old # convert ym to y0m with the new operating point: - estim.Y0m[(1+nym*(i-1)):(nym*i)] .-= @views model.yop[estim.i_ym] + estim.Y0m[(1+nym*(i-1)):(nym*i)] .-= @views model.yop[estim.i_ym] # convert u to u0 with the new operating point: - estim.U0[(1+nu*(i-1)):(nu*i)] .-= model.uop + estim.U0[(1+nu*(i-1)):(nu*i)] .-= model.uop # convert d to d0 with the new operating point: - estim.D0[(1+nd*(i-1)):(nd*i)] .-= model.dop + estim.D0[(1+nd*(i-1)):(nd*i)] .-= model.dop + # convert x̂ to x̂0 with the new operating point: + estim.X̂0_old[(1+nx̂*(i-1)):(nx̂*i)] .-= x̂op end estim.lastu0 .+= uop_old estim.Z̃[nε+1:nε+nx̂] .+= x̂op_old @@ -946,14 +1129,30 @@ function setmodel_estimator!( estim.cov.Q̂ .= to_hermitian(Q̂) invQ̂ = Hermitian(estim.buffer.Q̂, :L) invQ̂ .= estim.cov.Q̂ - inv!(invQ̂) + try + inv!(invQ̂) + catch err + if err isa PosDefException + error("Q̂ is not positive definite") + else + rethrow() + end + end estim.cov.invQ̂_He .= Hermitian(repeatdiag(invQ̂, He), :L) end if !isnothing(R̂) estim.cov.R̂ .= to_hermitian(R̂) invR̂ = Hermitian(estim.buffer.R̂, :L) invR̂ .= estim.cov.R̂ - inv!(invR̂) + try + inv!(invR̂) + catch err + if err isa PosDefException + error("R̂ is not positive definite") + else + rethrow() + end + end estim.cov.invR̂_He .= Hermitian(repeatdiag(invR̂, He), :L) end return nothing diff --git a/src/general.jl b/src/general.jl index 0ef4f69b1..451c8fb40 100644 --- a/src/general.jl +++ b/src/general.jl @@ -86,6 +86,19 @@ function limit_solve_time(optim::GenericModel, Ts) end end +"Set the maximum gradient scaling in `optim` to `10.0/C` if optimizer is `Ipopt`." +function set_scaling_gradient!(optim::JuMP.GenericModel, C) + if !isinf(C) && JuMP.solver_name(optim) == "Ipopt" + try + JuMP.get_attribute(optim, "nlp_scaling_max_gradient") + catch + # default "nlp_scaling_max_gradient" to `10.0/C` if not already set: + JuMP.set_attribute(optim, "nlp_scaling_max_gradient", 10.0/C) + end + end + return nothing +end + "Init a differentiation result matrix as dense or sparse matrix, as required by `backend`." init_diffmat(T, ::AbstractADType, _ , nx, ny) = zeros(T, ny, nx) function init_diffmat(T, ::AutoSparse, prep , _ , _ ) @@ -211,7 +224,7 @@ to_hermitian(A) = A """ Compute the inverse of a the Hermitian positive definite matrix `A` in-place and return it. -There is 3 methods for this function: +There are 3 methods for this function: - If `A` is a `Hermitian{ for the source. diff --git a/src/model/nonlinmodel.jl b/src/model/nonlinmodel.jl index d1c68d0f4..282166199 100644 --- a/src/model/nonlinmodel.jl +++ b/src/model/nonlinmodel.jl @@ -249,11 +249,15 @@ Validate `f` function argument signature and return `true` if it is mutating. function validate_f(NT, f) ismutating = hasmethod( f, - # ẋ or xnext, x, u, d, p + # ẋ or xnext, x , u , d , p Tuple{ Vector{NT}, Vector{NT}, Vector{NT}, Vector{NT}, Any} ) - # x, u, d, p - if !(ismutating || hasmethod(f, Tuple{Vector{NT}, Vector{NT}, Vector{NT}, Any})) + isnonmutating = hasmethod( + f, + # x, u , d , p + Tuple{Vector{NT}, Vector{NT}, Vector{NT}, Any} + ) + if !(ismutating || isnonmutating) error( "the state function has no method with type signature "* "f(x::Vector{$(NT)}, u::Vector{$(NT)}, d::Vector{$(NT)}, p::Any) or "* @@ -272,11 +276,15 @@ Validate `h` function argument signature and return `true` if it is mutating. function validate_h(NT, h) ismutating = hasmethod( h, - # y, x, d, p + # y , x , d , p Tuple{Vector{NT}, Vector{NT}, Vector{NT}, Any} ) - # x, d, p - if !(ismutating || hasmethod(h, Tuple{Vector{NT}, Vector{NT}, Any})) + isnonmutating = hasmethod( + h, + # x , d , p + Tuple{Vector{NT}, Vector{NT}, Any} + ) + if !(ismutating || isnonmutating) error( "the output function has no method with type signature "* "h(x::Vector{$(NT)}, d::Vector{$(NT)}, p::Any) or mutating form "* diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index f90cc9aa5..8cf92a61b 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -848,7 +848,7 @@ end mhe7 = MovingHorizonEstimator(linmodel, He=10) @test mhe7.He == 10 - @test length(mhe7.X̂0) == mhe7.He*6 + @test length(mhe7.X̂0_old) == mhe7.He*6 @test length(mhe7.Y0m) == mhe7.He*2 @test length(mhe7.U0) == mhe7.He*2 @test length(mhe7.D0) == (mhe7.He+mhe7.direct)*1 @@ -1394,15 +1394,15 @@ end @test mhe. ≈ [0.2] @test evaloutput(mhe) ≈ [55.0] @test mhe.lastu0 ≈ [2.0 - 3.0] - @test mhe.U0 ≈ repeat([2.0 - 3.0], He) - @test mhe.Y0m ≈ repeat([50.0 - 55.0], He) + @test mhe.U0[1] ≈ 2.0 - 3.0 + @test mhe.Y0m[1] ≈ 50.0 - 55.0 x̂ = preparestate!(mhe, [55.0]) @test x̂ ≈ [3.0] newlinmodel = setop!(newlinmodel, uop=[3.0], yop=[55.0], xop=[8.0], fop=[8.0]) setmodel!(mhe, newlinmodel) @test mhe.x̂0 ≈ [3.0 - 8.0] @test mhe.Z̃[1] ≈ 3.0 - 8.0 - @test mhe.X̂0 ≈ repeat([3.0 - 8.0], He) + @test mhe.X̂0_old[1] ≈ 3.0 - 8.0 @test mhe.x̂0arr_old ≈ [3.0 - 8.0] @test mhe.con.X̂0min ≈ repeat([-1000 - 8.0], He) @test mhe.con.X̂0max ≈ repeat([+1000 - 8.0], He) @@ -1410,15 +1410,21 @@ end @test mhe.con.x̃0max ≈ [+1000 - 8.0] setmodel!(mhe, Q̂=[1e-3], R̂=[1e-6]) @test mhe.cov.Q̂ ≈ [1e-3] + @test mhe.cov.invQ̂_He ≈ diagm(repeat([1e3], He)) @test mhe.cov.R̂ ≈ [1e-6] + @test mhe.cov.invR̂_He ≈ diagm(repeat([1e6], He)) f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d h(x,d,model) = model.C*x + model.Du*d nonlinmodel = NonLinModel(f, h, 10.0, 1, 1, 1, p=linmodel, solver=nothing) mhe2 = MovingHorizonEstimator(nonlinmodel; He, nint_ym=0) setmodel!(mhe2, Q̂=[1e-3], R̂=[1e-6]) @test mhe2.cov.Q̂ ≈ [1e-3] + @test mhe2.cov.invQ̂_He ≈ diagm(repeat([1e3], He)) @test mhe2.cov.R̂ ≈ [1e-6] + @test mhe2.cov.invR̂_He ≈ diagm(repeat([1e6], He)) @test_throws ErrorException setmodel!(mhe2, deepcopy(nonlinmodel)) + @test_throws ErrorException setmodel!(mhe, Q̂=diagm([-0.1])) + @test_throws ErrorException setmodel!(mhe, R̂=diagm([-0.1])) end @testitem "MovingHorizonEstimator v.s. Kalman filters" setup=[SetupMPCtests] begin @@ -1560,6 +1566,142 @@ end @test X̂_lin ≈ X̂_nonlin atol=1e-3 rtol=1e-3 end + +@testitem "MovingHorizonEstimator construction with custom constraint (LinModel)" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra + linmodel = LinModel(sys, Ts, i_u=[1,2], i_d=[3]) + linmodel = setop!(linmodel, uop=[10,50], yop=[50,30], dop=[5]) + + # Custom constraint: simple bound on states using only X̂e and ε + function gc_lin(X̂e, _ , _ , _ , _ , _ , _ , _ ,nX̂e , ε) + gc = X̂e .- 100 .- ε # all states must be <= 100 + return [gc; zeros(nX̂e .- length(X̂e))] + end + He = 3 + nx̂ = length(linmodel.A) + 2 # number of states (with augmentation) + nc = nx̂ * (He+1) # Approximate constraint count for He=3 + nX̂e = nx̂ * (He+1) + mhe = MovingHorizonEstimator(linmodel, Cwt=1e5, He=He, gc=gc_lin, nc=nc, p=nX̂e) + + @test mhe.con.nc == nc + @test mhe.nε > 0 + @test mhe.He == 3 +end + +@testitem "MovingHorizonEstimator custom constraint violation (LinModel)" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra + linmodel = LinModel(sys, Ts, i_u=[1,2], i_d=[3]) + linmodel = setop!(linmodel, uop=[10,50], yop=[50,30], dop=[5]) + + # Custom constraint function using only X̂e and ε + # This constraint enforces that the first state must be >= 0.5 + function gc_constraint!(LHS, X̂e, V̂e, Ŵe, Ue, Yem, De, P̄, x̄, p, ε) + nx̂ = 4 # number of states (with augmentation) + # Extract and constrain first state at each time step + 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 + + nc = 3 # One constraint per time step (He=1 initially, but grows) + mhe = MovingHorizonEstimator(linmodel, He=1, gc=gc_constraint!, nc=nc, Cwt=1e3, nint_ym=0) + + preparestate!(mhe, [50, 30], [5]) + x̂ = updatestate!(mhe, [10, 50], [50, 30], [5]) + # The constraint should be satisfied (first state should be >= 0.5) + @test x̂[1] >= 0.5 - 0.1 # Allow some tolerance +end + +@testitem "MovingHorizonEstimator custom constraint violation (NonLinModel)" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra + using JuMP, Ipopt + linmodel = LinModel(sys, Ts, i_u=[1,2], i_d=[3]) + linmodel = setop!(linmodel, uop=[10,50], yop=[50,30], dop=[5]) + + 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=linmodel) + nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30], dop=[5]) + + # Custom constraint function using only X̂e and ε + # This constraint enforces that the first state must be >= 0.5 + function gc_constraint_nl!(LHS, X̂e, V̂e, Ŵe, Ue, Yem, De, P̄, x̄, p, ε) + nx̂ = 6 # number of states (with augmentation) + # Extract and constrain first state at each time step + 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 + + nc = 3 # One constraint per time step + optim = Model(Ipopt.Optimizer) + mhe = MovingHorizonEstimator(nonlinmodel, He=1, gc=gc_constraint_nl!, nc=nc, Cwt=1e3, nint_ym=0; optim) + + preparestate!(mhe, [50, 30], [5]) + x̂ = updatestate!(mhe, [10, 50], [50, 30], [5]) + # The constraint should be satisfied (first state should be >= 0.5) + @test x̂[1] >= 0.5 - 0.1 # Allow some tolerance +end + +@testitem "MovingHorizonEstimator custom constraint on noise (LinModel)" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra + linmodel = LinModel(sys, Ts, i_u=[1,2], i_d=[3]) + linmodel = setop!(linmodel, uop=[10,50], yop=[50,30], dop=[5]) + + # Custom constraint function using Ŵe and ε + # This constraint enforces that process noise must be <= 0 + function gc_noise_constraint!(LHS, X̂e, V̂e, Ŵe, Ue, Yem, De, P̄, x̄, p, ε) + nx̂ = 4 # number of states (with augmentation) + # Constraint: all process noise components must be <= 0 + for i in 1:length(Ŵe) + LHS[i] = Ŵe[i] # Ŵe <= 0 + end + return nothing + end + + nc = 4 * 2 # Two time steps with 4 states each + mhe = MovingHorizonEstimator(linmodel, He=1, gc=gc_noise_constraint!, nc=nc, Cwt=1e3, nint_ym=0) + + preparestate!(mhe, [50, 30], [5]) + x̂ = updatestate!(mhe, [10, 50], [50, 30], [5]) + # The constraint should be satisfied (process noise should be <= 0) + @test all(mhe.Ŵ .<= 0.1) # Allow some tolerance +end + +@testitem "MovingHorizonEstimator custom constraint on noise (NonLinModel)" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra + using JuMP, Ipopt + linmodel = LinModel(sys, Ts, i_u=[1,2], i_d=[3]) + linmodel = setop!(linmodel, uop=[10,50], yop=[50,30], dop=[5]) + + 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=linmodel) + nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30], dop=[5]) + + # Custom constraint function using Ŵe and ε + # This constraint enforces that process noise must be <= 0 + function gc_noise_constraint_nl!(LHS, X̂e, V̂e, Ŵe, Ue, Yem, De, P̄, x̄, p, ε) + nx̂ = 6 # number of states (with augmentation) + # Constraint: all process noise components must be <= 0 + for i in 1:length(Ŵe) + LHS[i] = Ŵe[i] # Ŵe <= 0 + end + return nothing + end + + nc = 6 * 2 # Two time steps with 6 states each + optim = Model(Ipopt.Optimizer) + mhe = MovingHorizonEstimator(nonlinmodel, He=1, gc=gc_noise_constraint_nl!, nc=nc, Cwt=1e3, nint_ym=0; optim) + + preparestate!(mhe, [50, 30], [5]) + x̂ = updatestate!(mhe, [10, 50], [50, 30], [5]) + # The constraint should be satisfied (process noise should be <= 0) + @test all(mhe.Ŵ .<= 0.1) # Allow some tolerance +end + @testitem "ManualEstimator construction" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = LinModel(sys,Ts,i_u=[1,2])