Skip to content

Commit 42ebcbc

Browse files
franckgagafranckgaga
committed
added: custom NL con in MHE now works!
1 parent 1c3cf3d commit 42ebcbc

5 files changed

Lines changed: 151 additions & 83 deletions

File tree

‎src/controller/nonlinmpc.jl‎

Lines changed: 4 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -736,11 +736,11 @@ function init_optimization!(
736736
mpc::NonLinMPC, model::SimModel, optim::JuMP.GenericModel{JNT}
737737
) where JNT<:Real
738738
# --- variables and linear constraints ---
739-
con, transcription = mpc.con, mpc.transcription
739+
con = mpc.con
740740
nZ̃ = length(mpc.Z̃)
741741
JuMP.num_variables(optim) == 0 || JuMP.empty!(optim)
742742
JuMP.set_silent(optim)
743-
limit_solve_time(mpc.optim, mpc.estim.model.Ts)
743+
limit_solve_time(mpc.optim, model.Ts)
744744
@variable(optim, Z̃var[1:nZ̃])
745745
A = con.A[con.i_b, :]
746746
b = con.b[con.i_b]
@@ -749,15 +749,8 @@ function init_optimization!(
749749
beq = con.beq
750750
@constraint(optim, linconstrainteq, Aeq*Z̃var .== beq)
751751
# --- nonlinear optimization init ---
752-
if mpc.nϵ == 1 && JuMP.solver_name(optim) == "Ipopt"
753-
C = mpc.weights.Ñ_Hc[end]
754-
try
755-
JuMP.get_attribute(optim, "nlp_scaling_max_gradient")
756-
catch
757-
# default "nlp_scaling_max_gradient" to `10.0/C` if not already set:
758-
JuMP.set_attribute(optim, "nlp_scaling_max_gradient", 10.0/C)
759-
end
760-
end
752+
C = mpc.nϵ > 0 ? mpc.weights.Cwt : Inf
753+
set_scaling_gradient!(optim, C)
761754
J_op = get_nonlinobj_op(mpc, optim)
762755
g_oracle, geq_oracle = get_nonlincon_oracle(mpc, optim)
763756
@objective(optim, Min, J_op(Z̃var...))

‎src/controller/transcription.jl‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1330,7 +1330,7 @@ The method mutates the `g` vectors in argument and returns it. Only the custom c
13301330
`gc` are include in the `g` vector.
13311331
"""
13321332
function con_nonlinprog!(
1333-
g, ::PredictiveController, ::LinModel, ::TranscriptionMethod, _ , _ , gc, ϵ
1333+
g, ::PredictiveController, ::LinModel, ::TranscriptionMethod, _ , _ , gc, _
13341334
)
13351335
for i in eachindex(g)
13361336
g[i] = gc[i]

‎src/estimator/mhe/construct.jl‎

Lines changed: 64 additions & 52 deletions
Original file line numberDiff line numberDiff line change
@@ -66,7 +66,8 @@ struct MovingHorizonEstimator{
6666
JM<:JuMP.GenericModel,
6767
GB<:AbstractADType,
6868
JB<:AbstractADType,
69-
HB<:Union{AbstractADType, Nothing},
69+
HB<:Union{AbstractADType, Nothing},
70+
PT<:Any,
7071
GCfunc<:Function,
7172
CE<:KalmanEstimator,
7273
} <: StateEstimator{NT}
@@ -92,6 +93,7 @@ struct MovingHorizonEstimator{
9293
nym::Int
9394
nyu::Int
9495
nxs::Int
96+
p::PT
9597
As ::Matrix{NT}
9698
Cs_u::Matrix{NT}
9799
Cs_y::Matrix{NT}
@@ -133,7 +135,7 @@ struct MovingHorizonEstimator{
133135
function MovingHorizonEstimator{NT}(
134136
model::SM,
135137
He, i_ym, nint_u, nint_ym, cov::KC, Cwt,
136-
gc!::GCfunc, nc, p,
138+
gc!::GCfunc, nc, p::PT,
137139
optim::JM, gradient::GB, jacobian::JB, hessian::HB, covestim::CE;
138140
direct=true
139141
) where {
@@ -144,6 +146,7 @@ struct MovingHorizonEstimator{
144146
GB<:AbstractADType,
145147
JB<:AbstractADType,
146148
HB<:Union{AbstractADType, Nothing},
149+
PT<:Any,
147150
GCfunc<:Function,
148151
CE<:KalmanEstimator{NT}
149152
}
@@ -181,7 +184,7 @@ struct MovingHorizonEstimator{
181184
Nk = [0]
182185
corrected = [false]
183186
buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk, He, nε)
184-
estim = new{NT, SM, KC, JM, GB, JB, HB, GCfunc, CE}(
187+
estim = new{NT, SM, KC, JM, GB, JB, HB, PT, GCfunc, CE}(
185188
model,
186189
cov,
187190
optim, con,
@@ -190,6 +193,7 @@ struct MovingHorizonEstimator{
190193
Z̃, lastu0, x̂op, f̂op, x̂0,
191194
He, nε,
192195
i_ym, nx̂, nym, nyu, nxs,
196+
p,
193197
As, Cs_u, Cs_y, nint_u, nint_ym,
194198
Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm,
195199
Ẽ, F, G, J, B, ẽx̄, fx̄,
@@ -386,26 +390,26 @@ MovingHorizonEstimator estimator with a sample time Ts = 10.0 s:
386390
unavailable e.g.: ``\mathbf{u}(k)`` with ``p=0``. They are filled with `NaN` values.
387391
The exact time steps of the `NaN`s are detailed in the last column below.
388392
389-
| ARGUMENT | SIZE | FIRST SAMPLE | LAST SAMPLE | MISSING SAMPLES (NAN) |
390-
| :--------------- | :-------------- | :-------------- | :-------------- | :------------------------- |
391-
| ``\mathbf{X̂_e}`` | `((Nk+1)*nx̂,)` | ``k - N_k + p`` | ``k + p`` | — |
392-
| ``\mathbf{V̂_e}`` | `((Nk+1)*nym,)` | ``k - N_k + p`` | ``k + p`` | ``k - N_k, k + 1`` |
393-
| ``\mathbf{Ŵ_e}`` | `((Nk+1)*nx̂,)` | ``k - N_k + p`` | ``k + p`` | ``k - N_k + p - 1, k + p`` |
394-
| ``\mathbf{U_e}`` | `((Nk+1)*nu,)` | ``k - N_k + p`` | ``k + p`` | ``k + p`` |
395-
| ``\mathbf{Y_e^m}`` | `((Nk+1)*nym,)` | ``k - N_k + p`` | ``k + p`` | ``k + 1`` |
396-
| ``\mathbf{D_e}`` | `((Nk+1)*nd,)` | ``k - N_k + p`` | ``k + p`` | ``k + 1`` |
397-
| ``\mathbf{P̄}`` | `(nx̂, nx̂)` | ``k - N_k + p`` | ``k - N_k + p`` | — |
398-
| ``\mathbf{x̄}`` | `(nx̂,)` | ``k - N_k + p`` | ``k - N_k + p`` | — |
399-
| ``\mathbf{p}`` | var. | — | — | — |
400-
| ``ε`` | `()` | — | — | — |
393+
| ARGUMENT | SIZE | FIRST SAMPLE | LAST SAMPLE | MISSING SAMPLES |
394+
| :--------------- | :-------------- | :-------------- | :-------------- | :----------------- |
395+
| ``\mathbf{X̂_e}`` | `((Nk+1)*nx̂,)` | ``k - N_k + p`` | ``k + p`` | — |
396+
| ``\mathbf{V̂_e}`` | `((Nk+1)*nym,)` | ``k - N_k + p`` | ``k + p`` | ``k - N_k, k + 1`` |
397+
| ``\mathbf{Ŵ_e}`` | `((Nk+1)*nx̂,)` | ``k - N_k + p`` | ``k + p`` | ``k + p`` |
398+
| ``\mathbf{U_e}`` | `((Nk+1)*nu,)` | ``k - N_k + p`` | ``k + p`` | ``k + p`` |
399+
| ``\mathbf{Y_e^m}`` | `((Nk+1)*nym,)` | ``k - N_k + p`` | ``k + p`` | ``k + 1`` |
400+
| ``\mathbf{D_e}`` | `((Nk+1)*nd,)` | ``k - N_k + p`` | ``k + p`` | ``k + 1`` |
401+
| ``\mathbf{P̄}`` | `(nx̂, nx̂)` | ``k - N_k + p`` | ``k - N_k + p`` | — |
402+
| ``\mathbf{x̄}`` | `(nx̂,)` | ``k - N_k + p`` | ``k - N_k + p`` | — |
403+
| ``\mathbf{p}`` | var. | — | — | — |
404+
| ``ε`` | `()` | — | — | — |
401405
402406
If `LHS` represents the result of the left-hand side in the inequality
403407
``\mathbf{g_c}(\mathbf{X̂_e, V̂_e, Ŵ_e, U_e, Y_e^m, D_e, P̄, x̄, p}, ε) ≤ \mathbf{0}``,
404408
the function `gc` can be implemented in two possible ways:
405409
406410
1. **Non-mutating function** (out-of-place): define it as `gc(X̂e, V̂e, Ŵe, Ue, Yem, De,
407411
P̄, x̄, p, ε) -> LHS`. This syntax is simple and intuitive but it allocates more memory.
408-
2. **Mutating function** (in-place): define it as `gc!(LHS, X̂e, V̂e, Ŵe, Ue, Yme, De, P̄,
412+
2. **Mutating function** (in-place): define it as `gc!(LHS, X̂e, V̂e, Ŵe, Ue, Yem, De, P̄,
409413
x̄, p, ε) -> nothing`. This syntax reduces the allocations and potentially the
410414
computational burden as well.
411415
@@ -921,7 +925,7 @@ function setconstraint!(
921925
return estim
922926
end
923927

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

927931
"""
@@ -931,7 +935,7 @@ Re-construct nonlinear constraints and add them to `estim.optim`.
931935
"""
932936
function reset_nonlincon!(estim::MovingHorizonEstimator, model::NonLinModel)
933937
g_oracle = get_nonlincon_oracle(estim, estim.optim)
934-
set_nonlincon!(estim, model, estim.optim, g_oracle)
938+
set_nonlincon!(estim, estim.optim, g_oracle)
935939
end
936940

937941
@doc raw"""
@@ -1462,15 +1466,21 @@ Init the quadratic optimization of [`MovingHorizonEstimator`](@ref).
14621466
function init_optimization!(
14631467
estim::MovingHorizonEstimator, model::LinModel, optim::JuMP.GenericModel,
14641468
)
1469+
C, con = estim.C, estim.con
14651470
nZ̃ = length(estim.Z̃)
14661471
JuMP.num_variables(optim) == 0 || JuMP.empty!(optim)
14671472
JuMP.set_silent(optim)
14681473
limit_solve_time(optim, model.Ts)
14691474
@variable(optim, Z̃var[1:nZ̃])
1470-
A = estim.con.A[estim.con.i_b, :]
1471-
b = estim.con.b[estim.con.i_b]
1475+
A = con.A[con.i_b, :]
1476+
b = con.b[con.i_b]
14721477
@constraint(optim, linconstraint, A*Z̃var .≤ b)
14731478
@objective(optim, Min, obj_quadprog(Z̃var, estim.H̃, estim.q̃))
1479+
if con.nc > 0
1480+
set_scaling_gradient!(optim, C)
1481+
g_oracle = get_nonlincon_oracle(estim, optim)
1482+
set_nonlincon!(estim, optim, g_oracle)
1483+
end
14741484
return nothing
14751485
end
14761486

@@ -1491,23 +1501,16 @@ function init_optimization!(
14911501
JuMP.set_silent(optim)
14921502
limit_solve_time(optim, model.Ts)
14931503
@variable(optim, Z̃var[1:nZ̃])
1494-
A = estim.con.A[con.i_b, :]
1495-
b = estim.con.b[con.i_b]
1504+
A = con.A[con.i_b, :]
1505+
b = con.b[con.i_b]
14961506
@constraint(optim, linconstraint, A*Z̃var .≤ b)
14971507
# --- nonlinear optimization init ---
1498-
if !isinf(C) && JuMP.solver_name(optim) == "Ipopt"
1499-
try
1500-
JuMP.get_attribute(optim, "nlp_scaling_max_gradient")
1501-
catch
1502-
# default "nlp_scaling_max_gradient" to `10.0/C` if not already set:
1503-
JuMP.set_attribute(optim, "nlp_scaling_max_gradient", 10.0/C)
1504-
end
1505-
end
1508+
set_scaling_gradient!(optim, C)
15061509
# constraints with vector nonlinear oracle, objective function with splatting:
15071510
J_op = get_nonlinobj_op(estim, optim)
15081511
g_oracle = get_nonlincon_oracle(estim, optim)
15091512
@objective(optim, Min, J_op(Z̃var...))
1510-
set_nonlincon!(estim, model, optim, g_oracle)
1513+
set_nonlincon!(estim, optim, g_oracle)
15111514
return nothing
15121515
end
15131516

@@ -1534,23 +1537,28 @@ function get_nonlinobj_op(
15341537
He = estim.He
15351538
ng = length(con.i_g)
15361539
nŴ, nV̂, nX̂, ng, nZ̃ = He*nx̂, He*nym, He*nx̂, length(con.i_g), length(estim.Z̃)
1540+
nŴe, nX̂e, nV̂e = (He+1)*nx̂, (He+1)*nx̂, (He+1)*nym
15371541
strict = Val(true)
15381542
myNaN = convert(JNT, NaN)
15391543
J::Vector{JNT} = zeros(JNT, 1)
1540-
x̂0arr::Vector{JNT}, x̄::Vector{JNT} = zeros(JNT, nx̂), zeros(JNT, nx̂)
1544+
x̂0arr::Vector{JNT}, x̄::Vector{JNT} = zeros(JNT, nx̂), zeros(JNT, nx̂)
15411545
Ŵ::Vector{JNT} = zeros(JNT, nŴ)
1542-
V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂)
1546+
V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂)
1547+
Ŵe::Vector{JNT} = zeros(JNT, nŴe)
1548+
V̂e::Vector{JNT}, X̂e::Vector{JNT} = zeros(JNT, nV̂e), zeros(JNT, nX̂e)
15431549
k::Vector{JNT} = zeros(JNT, nk)
1544-
û0::Vector{JNT}, ŷ0::Vector{JNT} = zeros(JNT, nu), zeros(JNT, nŷ)
1545-
gc::Vector{JNT}, g::Vector{JNT} = zeros(JNT, nc), zeros(JNT, ng)
1546-
function J!(Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g)
1547-
update_prediction!(x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g, estim, Z̃)
1550+
û0::Vector{JNT}, ŷ0::Vector{JNT} = zeros(JNT, nu), zeros(JNT, nŷ)
1551+
gc::Vector{JNT}, g::Vector{JNT} = zeros(JNT, nc), zeros(JNT, ng)
1552+
function J!(Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, û0, k, ŷ0, gc, g)
1553+
update_prediction!(x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, û0, k, ŷ0, gc, g, estim, Z̃)
15481554
return obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃)
15491555
end
15501556
Z̃_J = fill(myNaN, nZ̃) # NaN to force update_predictions! at first call
15511557
J_cache = (
1552-
Cache(x̂0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0),
1553-
Cache(û0), Cache(k), Cache(ŷ0), Cache(gc), Cache(g),
1558+
Cache(x̂0arr), Cache(x̄),
1559+
Cache(Ŵ), Cache(V̂), Cache(X̂0),
1560+
Cache(Ŵe), Cache(V̂e), Cache(X̂e),
1561+
Cache(û0), Cache(k), Cache(ŷ0), Cache(gc), Cache(g),
15541562
)
15551563
# temporarily "fill" the estimation window for the preparation of the gradient:
15561564
estim.Nk[] = He
@@ -1641,30 +1649,35 @@ function get_nonlincon_oracle(
16411649
ng, ngi = length(con.i_g), sum(con.i_g)
16421650
nc = con.nc
16431651
nŴ, nV̂, nX̂, nZ̃ = He*nx̂, He*nym, He*nx̂, length(estim.Z̃)
1652+
nŴe, nX̂e, nV̂e = (He+1)*nx̂, (He+1)*nx̂, (He+1)*nym
16441653
strict = Val(true)
16451654
myNaN, myInf = convert(JNT, NaN), convert(JNT, Inf)
16461655
x̂0arr::Vector{JNT}, x̄::Vector{JNT} = zeros(JNT, nx̂), zeros(JNT, nx̂)
16471656
Ŵ::Vector{JNT} = zeros(JNT, nŴ)
1648-
V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂)
1657+
V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂)
1658+
Ŵe::Vector{JNT} = zeros(JNT, nŴe)
1659+
V̂e::Vector{JNT}, X̂e::Vector{JNT} = zeros(JNT, nV̂e), zeros(JNT, nX̂e)
16491660
k::Vector{JNT} = zeros(JNT, nk)
16501661
û0::Vector{JNT}, ŷ0::Vector{JNT} = zeros(JNT, nu), zeros(JNT, nŷ)
16511662
gc::Vector{JNT}, g::Vector{JNT} = zeros(JNT, nc), zeros(JNT, ng)
16521663
gi::Vector{JNT} = zeros(JNT, ngi)
16531664
λi::Vector{JNT} = rand(JNT, ngi)
16541665
# -------------- inequality constraint: nonlinear oracle -------------------------
1655-
function gi!(gi, Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g)
1656-
update_prediction!(x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g, estim, Z̃)
1666+
function gi!(gi, Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, û0, k, ŷ0, gc, g)
1667+
update_prediction!(x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, û0, k, ŷ0, gc, g, estim, Z̃)
16571668
gi .= @views g[i_g]
16581669
return nothing
16591670
end
1660-
function ℓ_gi(Z̃, λi, x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g, gi)
1661-
update_prediction!(x̂0arr, x̄, Ŵ, V̂, X̂0, û0, k, ŷ0, gc, g, estim, Z̃)
1671+
function ℓ_gi(Z̃, λi, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, û0, k, ŷ0, gc, g, gi)
1672+
update_prediction!(x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, û0, k, ŷ0, gc, g, estim, Z̃)
16621673
gi .= @views g[i_g]
16631674
return dot(λi, gi)
16641675
end
16651676
Z̃_∇gi = fill(myNaN, nZ̃) # NaN to force update_predictions! at first call
16661677
∇gi_cache = (
1667-
Cache(x̂0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0),
1678+
Cache(x̂0arr), Cache(x̄),
1679+
Cache(Ŵ), Cache(V̂), Cache(X̂0),
1680+
Cache(Ŵe), Cache(V̂e), Cache(X̂e),
16681681
Cache(û0), Cache(k), Cache(ŷ0), Cache(gc), Cache(g),
16691682
)
16701683
# temporarily "fill" the estimation window for the preparation of the gradient:
@@ -1675,7 +1688,9 @@ function get_nonlincon_oracle(
16751688
∇gi_structure = init_diffstructure(∇gi)
16761689
if !isnothing(hess)
16771690
∇²gi_cache = (
1678-
Cache(x̂0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0),
1691+
Cache(x̂0arr), Cache(x̄),
1692+
Cache(Ŵ), Cache(V̂), Cache(X̂0),
1693+
Cache(Ŵe), Cache(V̂e), Cache(X̂e),
16791694
Cache(û0), Cache(k), Cache(ŷ0), Cache(gc), Cache(g), Cache(gi)
16801695
)
16811696
estim.Nk[] = He # see comment above
@@ -1722,16 +1737,13 @@ function get_nonlincon_oracle(
17221737
return g_oracle
17231738
end
17241739

1725-
"By default, there is no nonlinear constraint, thus do nothing."
1726-
set_nonlincon!(::MovingHorizonEstimator, ::SimModel, _ , _ ) = nothing
1727-
17281740
"""
1729-
set_nonlincon!(estim::MovingHorizonEstimator, ::NonLinModel, optim, g_oracle)
1741+
set_nonlincon!(estim::MovingHorizonEstimator, optim, g_oracle)
17301742
1731-
Set the nonlinear inequality constraints for `NonLinModel`, if any.
1743+
Set the nonlinear inequality constraints of `estim`, if any.
17321744
"""
17331745
function set_nonlincon!(
1734-
estim::MovingHorizonEstimator, ::NonLinModel, optim::JuMP.GenericModel{JNT}, g_oracle
1746+
estim::MovingHorizonEstimator, optim::JuMP.GenericModel{JNT}, g_oracle
17351747
) where JNT<:Real
17361748
Z̃var = optim[:Z̃var]
17371749
nonlin_constraints = JuMP.all_constraints(

0 commit comments

Comments
 (0)