@@ -24,7 +24,13 @@ struct NonLinMPC{SE<:StateEstimator, JEfunc<:Function} <: PredictiveController
2424 G:: Matrix{Float64}
2525 J:: Matrix{Float64}
2626 K:: Matrix{Float64}
27- Q:: Matrix{Float64}
27+ V:: Matrix{Float64}
28+ ẽx̂:: Matrix{Float64}
29+ fx̂:: Vector{Float64}
30+ gx̂:: Matrix{Float64}
31+ jx̂:: Matrix{Float64}
32+ kx̂:: Matrix{Float64}
33+ vx̂:: Matrix{Float64}
2834 P̃:: Hermitian{Float64, Matrix{Float64}}
2935 q̃:: Vector{Float64}
3036 p:: Vector{Float64}
@@ -48,8 +54,8 @@ struct NonLinMPC{SE<:StateEstimator, JEfunc<:Function} <: PredictiveController
4854 R̂y, R̂u = zeros (ny* Hp), zeros (nu* Hp) # dummy vals (updated just before optimization)
4955 noR̂u = iszero (L_Hp)
5056 S, T = init_ΔUtoU (nu, Hp, Hc)
51- E, F, G, J, K, Q = init_predmat (estim, model, Hp, Hc)
52- con, S̃, Ñ_Hc, Ẽ = init_defaultcon (model , Hp, Hc, C, S, N_Hc, E)
57+ E, F, G, J, K, V, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂ = init_predmat (estim, model, Hp, Hc)
58+ con, S̃, Ñ_Hc, Ẽ, ẽx̂ = init_defaultcon (estim , Hp, Hc, C, S, N_Hc, E, ex̂ )
5359 P̃, q̃, p = init_quadprog (model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
5460 Ks, Ps = init_stochpred (estim, Hp)
5561 d0, D̂0 = zeros (nd), zeros (nd* Hp)
@@ -62,7 +68,7 @@ struct NonLinMPC{SE<:StateEstimator, JEfunc<:Function} <: PredictiveController
6268 Hp, Hc,
6369 M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt, JE, R̂u, R̂y, noR̂u,
6470 S̃, T,
65- Ẽ, F, G, J, K, Q , P̃, q̃, p,
71+ Ẽ, F, G, J, K, V, ẽx̂, fx̂, gx̂, jx̂, kx̂, vx̂ , P̃, q̃, p,
6672 Ks, Ps,
6773 d0, D̂0,
6874 Ŷop, Dop,
@@ -113,7 +119,7 @@ This method uses the default state estimator :
113119
114120# Arguments
115121- `model::SimModel` : model used for controller predictions and state estimations.
116- - `Hp=10 `: prediction horizon ``H_p``.
122+ - `Hp=nothing `: prediction horizon ``H_p``, must be specified for [`NonLinModel`](@ref) .
117123- `Hc=2` : control horizon ``H_c``.
118124- `Mwt=fill(1.0,model.ny)` : main diagonal of ``\m athbf{M}`` weight matrix (vector).
119125- `Nwt=fill(0.1,model.nu)` : main diagonal of ``\m athbf{N}`` weight matrix (vector).
@@ -155,7 +161,7 @@ for common mistakes when writing these functions.
155161"""
156162function NonLinMPC (
157163 model:: SimModel ;
158- Hp:: Int = DEFAULT_HP ,
164+ Hp:: Union{ Int, Nothing} = nothing ,
159165 Hc:: Int = DEFAULT_HC,
160166 Mwt = fill (DEFAULT_MWT, model. ny),
161167 Nwt = fill (DEFAULT_NWT, model. nu),
@@ -169,9 +175,10 @@ function NonLinMPC(
169175 estim = UnscentedKalmanFilter (model; kwargs... )
170176 NonLinMPC (estim; Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, optim)
171177end
178+
172179function NonLinMPC (
173180 model:: LinModel ;
174- Hp:: Int = DEFAULT_HP ,
181+ Hp:: Union{ Int, Nothing} = nothing ,
175182 Hc:: Int = DEFAULT_HC,
176183 Mwt = fill (DEFAULT_MWT, model. ny),
177184 Nwt = fill (DEFAULT_NWT, model. nu),
@@ -211,7 +218,7 @@ NonLinMPC controller with a sample time Ts = 10.0 s, Ipopt optimizer, UnscentedK
211218"""
212219function NonLinMPC (
213220 estim:: SE ;
214- Hp:: Int = DEFAULT_HP ,
221+ Hp:: Union{ Int, Nothing} = nothing ,
215222 Hc:: Int = DEFAULT_HC,
216223 Mwt = fill (DEFAULT_MWT, estim. model. ny),
217224 Nwt = fill (DEFAULT_NWT, estim. model. nu),
@@ -221,6 +228,7 @@ function NonLinMPC(
221228 JE:: JEFunc = (_,_,_) -> 0.0 ,
222229 optim:: JuMP.Model = JuMP. Model (optimizer_with_attributes (Ipopt. Optimizer," sb" => " yes"
223230) where {SE<: StateEstimator , JEFunc<: Function }
231+ Hp = default_Hp (estim. model, Hp)
224232 return NonLinMPC {SE, JEFunc} (estim, Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, optim)
225233end
226234
@@ -256,20 +264,22 @@ function init_optimization!(mpc::NonLinMPC)
256264 @constraint (optim, linconstraint, A* ΔŨvar .≤ b)
257265 # --- nonlinear optimization init ---
258266 model = mpc. estim. model
259- ny, nu, Hp, Hc = model. ny, model. nu, mpc. Hp , mpc. Hc
260- nC = (2 * Hp* nu + 2 * nvar + 2 * Hp* ny) - length (mpc. con. b)
267+ ny, nu, nx̂, Hp = model. ny, model. nu, mpc. estim . nx̂ , mpc. Hp
268+ nC = (2 * Hp* nu + 2 * nvar + 2 * Hp* ny + 2 * nx̂ ) - length (mpc. con. b)
261269 # inspired from https://jump.dev/JuMP.jl/stable/tutorials/nonlinear/tips_and_tricks/#User-defined-operators-with-vector-outputs
262- Jfunc, Cfunc = let mpc= mpc, model= model, nC= nC, nvar= nvar , nŶ= Hp* ny
270+ Jfunc, Cfunc = let mpc= mpc, model= model, nC= nC, nvar= nvar , nŶ= Hp* ny, nx̂ = nx̂
263271 last_ΔŨtup_float, last_ΔŨtup_dual = nothing , nothing
264272 Ŷ_cache:: DiffCacheType = DiffCache (zeros (nŶ), nvar + 3 )
265273 C_cache:: DiffCacheType = DiffCache (zeros (nC), nvar + 3 )
274+ x̂_cache:: DiffCacheType = DiffCache (zeros (nx̂), nvar + 3 )
266275 function Jfunc (ΔŨtup:: Float64... )
267276 Ŷ = get_tmp (Ŷ_cache, ΔŨtup[1 ])
268277 ΔŨ = collect (ΔŨtup)
269278 if ΔŨtup != last_ΔŨtup_float
279+ x̂ = get_tmp (x̂_cache, ΔŨtup[1 ])
270280 C = get_tmp (C_cache, ΔŨtup[1 ])
271- predict! (Ŷ, mpc, model, ΔŨ)
272- con_nonlinprog! (C, mpc, model, Ŷ, ΔŨ)
281+ Ŷ, x̂ end = predict! (Ŷ, x̂ , mpc, model, ΔŨ)
282+ con_nonlinprog! (C, mpc, model, x̂ end , Ŷ, ΔŨ)
273283 last_ΔŨtup_float = ΔŨtup
274284 end
275285 return obj_nonlinprog (mpc, model, Ŷ, ΔŨ)
@@ -278,31 +288,34 @@ function init_optimization!(mpc::NonLinMPC)
278288 Ŷ = get_tmp (Ŷ_cache, ΔŨtup[1 ])
279289 ΔŨ = collect (ΔŨtup)
280290 if ΔŨtup != last_ΔŨtup_dual
291+ x̂ = get_tmp (x̂_cache, ΔŨtup[1 ])
281292 C = get_tmp (C_cache, ΔŨtup[1 ])
282- predict! (Ŷ, mpc, model, ΔŨ)
283- con_nonlinprog! (C, mpc, model, Ŷ, ΔŨ)
293+ Ŷ, x̂ end = predict! (Ŷ, x̂ , mpc, model, ΔŨ)
294+ con_nonlinprog! (C, mpc, model, x̂ end , Ŷ, ΔŨ)
284295 last_ΔŨtup_dual = ΔŨtup
285296 end
286297 return obj_nonlinprog (mpc, model, Ŷ, ΔŨ)
287298 end
288299 function con_nonlinprog_i (i, ΔŨtup:: NTuple{N, Float64} ) where {N}
289300 C = get_tmp (C_cache, ΔŨtup[1 ])
290301 if ΔŨtup != last_ΔŨtup_float
302+ x̂ = get_tmp (x̂_cache, ΔŨtup[1 ])
291303 Ŷ = get_tmp (Ŷ_cache, ΔŨtup[1 ])
292304 ΔŨ = collect (ΔŨtup)
293- predict! (Ŷ, mpc, model, ΔŨ)
294- con_nonlinprog! (C, mpc, model, Ŷ, ΔŨ)
305+ Ŷ, x̂ end = predict! (Ŷ, x̂ , mpc, model, ΔŨ)
306+ C = con_nonlinprog! (C, mpc, model, x̂ end , Ŷ, ΔŨ)
295307 last_ΔŨtup_float = ΔŨtup
296308 end
297309 return C[i]
298310 end
299311 function con_nonlinprog_i (i, ΔŨtup:: NTuple{N, Real} ) where {N}
300312 C = get_tmp (C_cache, ΔŨtup[1 ])
301313 if ΔŨtup != last_ΔŨtup_dual
314+ x̂ = get_tmp (x̂_cache, ΔŨtup[1 ])
302315 Ŷ = get_tmp (Ŷ_cache, ΔŨtup[1 ])
303316 ΔŨ = collect (ΔŨtup)
304- predict! (Ŷ, mpc, model, ΔŨ)
305- con_nonlinprog! (C, mpc, model, Ŷ, ΔŨ)
317+ Ŷ, x̂ end = predict! (Ŷ, x̂ , mpc, model, ΔŨ)
318+ C = con_nonlinprog! (C, mpc, model, x̂ end , Ŷ, ΔŨ)
306319 last_ΔŨtup_dual = ΔŨtup
307320 end
308321 return C[i]
@@ -313,15 +326,22 @@ function init_optimization!(mpc::NonLinMPC)
313326 register (optim, :Jfunc , nvar, Jfunc, autodiff= true )
314327 @NLobjective (optim, Min, Jfunc (ΔŨvar... ))
315328 if nC ≠ 0
316- n = 0
329+ i_end_Ymin, i_end_Ymax, i_end_x̂min = 1 Hp * ny, 2 Hp * ny, 2 Hp * ny + nx̂
317330 for i in eachindex (con. Ymin)
318331 sym = Symbol (" C_Ymin_$i "
319- register (optim, sym, nvar, Cfunc[n + i], autodiff= true )
332+ register (optim, sym, nvar, Cfunc[i], autodiff= true )
320333 end
321- n = lastindex (con. Ymin)
322334 for i in eachindex (con. Ymax)
323335 sym = Symbol (" C_Ymax_$i "
324- register (optim, sym, nvar, Cfunc[n + i], autodiff= true )
336+ register (optim, sym, nvar, Cfunc[i_end_Ymin+ i], autodiff= true )
337+ end
338+ for i in eachindex (con. x̂min)
339+ sym = Symbol (" C_x̂min_$i "
340+ register (optim, sym, nvar, Cfunc[i_end_Ymax+ i], autodiff= true )
341+ end
342+ for i in eachindex (con. x̂max)
343+ sym = Symbol (" C_x̂max_$i "
344+ register (optim, sym, nvar, Cfunc[i_end_x̂min+ i], autodiff= true )
325345 end
326346 end
327347 return nothing
@@ -345,27 +365,41 @@ function setnonlincon!(mpc::NonLinMPC, ::NonLinModel)
345365 f_sym = Symbol (" C_Ymax_$(i) "
346366 add_nonlinear_constraint (optim, :($ (f_sym)($ (ΔŨvar... )) <= 0 ))
347367 end
368+ for i in findall (.! isinf .(con. x̂min))
369+ f_sym = Symbol (" C_x̂min_$(i) "
370+ add_nonlinear_constraint (optim, :($ (f_sym)($ (ΔŨvar... )) <= 0 ))
371+ end
372+ for i in findall (.! isinf .(con. x̂max))
373+ f_sym = Symbol (" C_x̂max_$(i) "
374+ add_nonlinear_constraint (optim, :($ (f_sym)($ (ΔŨvar... )) <= 0 ))
375+ end
348376 return nothing
349377end
350378
351379"""
352- con_nonlinprog!(C, mpc::NonLinMPC, model::SimModel, ΔŨ)
380+ con_nonlinprog!(C, mpc::NonLinMPC, model::SimModel, x̂end, Ŷ, ΔŨ)
353381
354382Nonlinear constrains for [`NonLinMPC`](@ref) when `model` is not a [`LinModel`](@ref).
355383"""
356- function con_nonlinprog! (C, mpc:: NonLinMPC , model:: SimModel , Ŷ, ΔŨ)
357- ny, Hp = model. ny, mpc. Hp
384+ function con_nonlinprog! (C, mpc:: NonLinMPC , model:: SimModel , x̂end , Ŷ, ΔŨ)
385+ ny, nx̂, Hp = model. ny, mpc. estim. nx̂, mpc. Hp
386+ i_end_Ymin, i_end_Ymax = 1 Hp* ny , 2 Hp* ny
387+ i_end_x̂min, i_end_x̂max = 2 Hp* ny + 1 nx̂, 2 Hp* ny + 2 nx̂
358388 if ! isinf (mpc. C) # constraint softening activated :
359389 ϵ = ΔŨ[end ]
360- C[begin : (Hp* ny)] = (mpc. con. Ymin - Ŷ) - ϵ* mpc. con. c_Ymin
361- C[(Hp* ny+ 1 ): end ] = (Ŷ - mpc. con. Ymax) - ϵ* mpc. con. c_Ymax
390+ C[ 1 : i_end_Ymin] = (mpc. con. Ymin - Ŷ) - ϵ* mpc. con. c_Ymin
391+ C[i_end_Ymin+ 1 : i_end_Ymax] = (Ŷ - mpc. con. Ymax) - ϵ* mpc. con. c_Ymax
392+ C[i_end_Ymax+ 1 : i_end_x̂min] = (mpc. con. x̂min - x̂end ) - ϵ* mpc. con. c_x̂min
393+ C[i_end_x̂min+ 1 : i_end_x̂max] = (x̂end - mpc. con. x̂max) - ϵ* mpc. con. c_x̂max
362394 else # no constraint softening :
363- C[begin : (Hp* ny)] = (mpc. con. Ymin - Ŷ)
364- C[(Hp* ny+ 1 ): end ] = (Ŷ - mpc. con. Ymax)
395+ C[ 1 : i_end_Ymin] = mpc. con. Ymin - Ŷ
396+ C[i_end_Ymin+ 1 : i_end_Ymax] = Ŷ - mpc. con. Ymax
397+ C[i_end_Ymax+ 1 : i_end_x̂min] = mpc. con. x̂min - x̂end
398+ C[i_end_x̂min+ 1 : i_end_x̂max] = x̂end - mpc. con. x̂max
365399 end
366400 C[isinf .(C)] .= 0 # replace ±Inf with 0 to avoid INVALID_MODEL error
367401 return C
368402end
369403
370404" No nonlinear constraints if `model` is a [`LinModel`](@ref), return `C` unchanged."
371- con_nonlinprog! (C, :: NonLinMPC , :: LinModel , _ , _ ) = C
405+ con_nonlinprog! (C, :: NonLinMPC , :: LinModel , _ , _ , _ ) = C
0 commit comments