Skip to content

Commit fb89cbe

Browse files
franckgagafranckgaga
authored
Merge pull request #86 from JuliaControl/simpler_setmodel!
Added: `Mwt`, `Nwt` and `Lwt` kwargs in `setmodel!`
2 parents 3a266dc + 2065d2f commit fb89cbe

File tree

7 files changed

+278
-157
lines changed

7 files changed

+278
-157
lines changed

‎Project.toml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
name = "ModelPredictiveControl"
22
uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
33
authors = ["Francis Gagnon"]
4-
version = "0.22.0"
4+
version = "0.22.1"
55

66
[deps]
77
ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"

‎src/controller/construct.jl‎

Lines changed: 97 additions & 67 deletions
Original file line numberDiff line numberDiff line change
@@ -146,97 +146,127 @@ function setconstraint!(
146146
model, con, optim = mpc.estim.model, mpc.con, mpc.optim
147147
nu, ny, nx̂, Hp, Hc, nϵ = model.nu, model.ny, mpc.estim.nx̂, mpc.Hp, mpc.Hc, mpc.nϵ
148148
notSolvedYet = (JuMP.termination_status(optim) == JuMP.OPTIMIZE_NOT_CALLED)
149-
isnothing(Umin) && !isnothing(umin) && (Umin = repeat(umin, Hp))
150-
isnothing(Umax) && !isnothing(umax) && (Umax = repeat(umax, Hp))
151-
isnothing(ΔUmin) && !isnothing(Δumin) && (ΔUmin = repeat(Δumin, Hc))
152-
isnothing(ΔUmax) && !isnothing(Δumax) && (ΔUmax = repeat(Δumax, Hc))
153-
isnothing(Ymin) && !isnothing(ymin) && (Ymin = repeat(ymin, Hp))
154-
isnothing(Ymax) && !isnothing(ymax) && (Ymax = repeat(ymax, Hp))
155-
isnothing(C_umin) && !isnothing(c_umin) && (C_umin = repeat(c_umin, Hp))
156-
isnothing(C_umax) && !isnothing(c_umax) && (C_umax = repeat(c_umax, Hp))
157-
isnothing(C_Δumin) && !isnothing(c_Δumin) && (C_Δumin = repeat(c_Δumin, Hc))
158-
isnothing(C_Δumax) && !isnothing(c_Δumax) && (C_Δumax = repeat(c_Δumax, Hc))
159-
isnothing(C_ymin) && !isnothing(c_ymin) && (C_ymin = repeat(c_ymin, Hp))
160-
isnothing(C_ymax) && !isnothing(c_ymax) && (C_ymax = repeat(c_ymax, Hp))
161-
if !all(isnothing.((C_umin, C_umax, C_Δumin, C_Δumax, C_ymin, C_ymax, c_x̂min, c_x̂max)))
162-
nϵ == 1 || throw(ArgumentError("Slack variable weight Cwt must be finite to set softness parameters"))
163-
notSolvedYet || error("Cannot set softness parameters after calling moveinput!")
164-
end
165-
if !isnothing(Umin)
166-
size(Umin) == (nu*Hp,) || throw(ArgumentError("Umin size must be $((nu*Hp,))"))
149+
if isnothing(Umin) && !isnothing(umin)
150+
size(umin) == (nu,) || throw(ArgumentError("umin size must be $((nu,))"))
151+
for i = 1:nu*Hp
152+
con.U0min[i] = umin[(i-1) % nu + 1] - mpc.Uop[i]
153+
end
154+
elseif !isnothing(Umin)
155+
size(Umin) == (nu*Hp,) || throw(ArgumentError("Umin size must be $((nu*Hp,))"))
167156
con.U0min .= Umin .- mpc.Uop
168157
end
169-
if !isnothing(Umax)
158+
if isnothing(Umax) && !isnothing(umax)
159+
size(umax) == (nu,) || throw(ArgumentError("umax size must be $((nu,))"))
160+
for i = 1:nu*Hp
161+
con.U0max[i] = umax[(i-1) % nu + 1] - mpc.Uop[i]
162+
end
163+
elseif !isnothing(Umax)
170164
size(Umax) == (nu*Hp,) || throw(ArgumentError("Umax size must be $((nu*Hp,))"))
171165
con.U0max .= Umax .- mpc.Uop
172166
end
173-
if !isnothing(ΔUmin)
167+
if isnothing(ΔUmin) && !isnothing(Δumin)
168+
size(Δumin) == (nu,) || throw(ArgumentError("Δumin size must be $((nu,))"))
169+
for i = 1:nu*Hc
170+
con.ΔŨmin[i] = Δumin[(i-1) % nu + 1]
171+
end
172+
elseif !isnothing(ΔUmin)
174173
size(ΔUmin) == (nu*Hc,) || throw(ArgumentError("ΔUmin size must be $((nu*Hc,))"))
175174
con.ΔŨmin[1:nu*Hc] .= ΔUmin
176175
end
177-
if !isnothing(ΔUmax)
176+
if isnothing(ΔUmax) && !isnothing(Δumax)
177+
size(Δumax) == (nu,) || throw(ArgumentError("Δumax size must be $((nu,))"))
178+
for i = 1:nu*Hc
179+
con.ΔŨmax[i] = Δumax[(i-1) % nu + 1]
180+
end
181+
elseif !isnothing(ΔUmax)
178182
size(ΔUmax) == (nu*Hc,) || throw(ArgumentError("ΔUmax size must be $((nu*Hc,))"))
179183
con.ΔŨmax[1:nu*Hc] .= ΔUmax
180184
end
181-
if !isnothing(Ymin)
185+
if isnothing(Ymin) && !isnothing(ymin)
186+
size(ymin) == (ny,) || throw(ArgumentError("ymin size must be $((ny,))"))
187+
for i = 1:ny*Hp
188+
con.Y0min[i] = ymin[(i-1) % ny + 1] - mpc.Yop[i]
189+
end
190+
elseif !isnothing(Ymin)
182191
size(Ymin) == (ny*Hp,) || throw(ArgumentError("Ymin size must be $((ny*Hp,))"))
183192
con.Y0min .= Ymin .- mpc.Yop
184193
end
185-
if !isnothing(Ymax)
194+
if isnothing(Ymax) && !isnothing(ymax)
195+
size(ymax) == (ny,) || throw(ArgumentError("ymax size must be $((ny,))"))
196+
for i = 1:ny*Hp
197+
con.Y0max[i] = ymax[(i-1) % ny + 1] - mpc.Yop[i]
198+
end
199+
elseif !isnothing(Ymax)
186200
size(Ymax) == (ny*Hp,) || throw(ArgumentError("Ymax size must be $((ny*Hp,))"))
187201
con.Y0max .= Ymax .- mpc.Yop
188202
end
189203
if !isnothing(x̂min)
190-
size(x̂min) == (nx̂,) || throw(ArgumentError("x̂min size must be $((nx̂,))"))
204+
size(x̂min) == (nx̂,) || throw(ArgumentError("x̂min size must be $((nx̂,))"))
191205
con.x̂0min .= x̂min .- mpc.estim.x̂op
192206
end
193207
if !isnothing(x̂max)
194-
size(x̂max) == (nx̂,) || throw(ArgumentError("x̂max size must be $((nx̂,))"))
208+
size(x̂max) == (nx̂,) || throw(ArgumentError("x̂max size must be $((nx̂,))"))
195209
con.x̂0max .= x̂max .- mpc.estim.x̂op
196210
end
197-
if !isnothing(C_umin)
198-
size(C_umin) == (nu*Hp,) || throw(ArgumentError("C_umin size must be $((nu*Hp,))"))
199-
any(C_umin .< 0) && error("C_umin weights should be non-negative")
200-
con.A_Umin[:, end] .= -C_umin
201-
end
202-
if !isnothing(C_umax)
203-
size(C_umax) == (nu*Hp,) || throw(ArgumentError("C_umax size must be $((nu*Hp,))"))
204-
any(C_umax .< 0) && error("C_umax weights should be non-negative")
205-
con.A_Umax[:, end] .= -C_umax
206-
end
207-
if !isnothing(C_Δumin)
208-
size(C_Δumin) == (nu*Hc,) || throw(ArgumentError("C_Δumin size must be $((nu*Hc,))"))
209-
any(C_Δumin .< 0) && error("C_Δumin weights should be non-negative")
210-
con.A_ΔŨmin[1:end-1, end] .= -C_Δumin
211-
end
212-
if !isnothing(C_Δumax)
213-
size(C_Δumax) == (nu*Hc,) || throw(ArgumentError("C_Δumax size must be $((nu*Hc,))"))
214-
any(C_Δumax .< 0) && error("C_Δumax weights should be non-negative")
215-
con.A_ΔŨmax[1:end-1, end] .= -C_Δumax
216-
end
217-
if !isnothing(C_ymin)
218-
size(C_ymin) == (ny*Hp,) || throw(ArgumentError("C_ymin size must be $((ny*Hp,))"))
219-
any(C_ymin .< 0) && error("C_ymin weights should be non-negative")
220-
con.C_ymin .= C_ymin
221-
size(con.A_Ymin, 1) ≠ 0 && (con.A_Ymin[:, end] .= -con.C_ymin) # for LinModel
222-
end
223-
if !isnothing(C_ymax)
224-
size(C_ymax) == (ny*Hp,) || throw(ArgumentError("C_ymax size must be $((ny*Hp,))"))
225-
any(C_ymax .< 0) && error("C_ymax weights should be non-negative")
226-
con.C_ymax .= C_ymax
227-
size(con.A_Ymax, 1) ≠ 0 && (con.A_Ymax[:, end] .= -con.C_ymax) # for LinModel
228-
end
229-
if !isnothing(c_x̂min)
230-
size(c_x̂min) == (nx̂,) || throw(ArgumentError("c_x̂min size must be $((nx̂,))"))
231-
any(c_x̂min .< 0) && error("c_x̂min weights should be non-negative")
232-
con.c_x̂min .= c_x̂min
233-
size(con.A_x̂min, 1) ≠ 0 && (con.A_x̂min[:, end] .= -con.c_x̂min) # for LinModel
211+
allECRs = (
212+
c_umin, c_umax, c_Δumin, c_Δumax, c_ymin, c_ymax,
213+
C_umin, C_umax, C_Δumin, C_Δumax, C_ymin, C_ymax, c_x̂min, c_x̂max,
214+
)
215+
if any(ECR -> !isnothing(ECR), allECRs)
216+
nϵ == 1 || throw(ArgumentError("Slack variable weight Cwt must be finite to set softness parameters"))
217+
notSolvedYet || error("Cannot set softness parameters after calling moveinput!")
234218
end
235-
if !isnothing(c_x̂max)
236-
size(c_x̂max) == (nx̂,) || throw(ArgumentError("c_x̂max size must be $((nx̂,))"))
237-
any(c_x̂max .< 0) && error("c_x̂max weights should be non-negative")
238-
con.c_x̂max .= c_x̂max
239-
size(con.A_x̂max, 1) ≠ 0 && (con.A_x̂max[:, end] .= -con.c_x̂max) # for LinModel
219+
if notSolvedYet
220+
isnothing(C_umin) && !isnothing(c_umin) && (C_umin = repeat(c_umin, Hp))
221+
isnothing(C_umax) && !isnothing(c_umax) && (C_umax = repeat(c_umax, Hp))
222+
isnothing(C_Δumin) && !isnothing(c_Δumin) && (C_Δumin = repeat(c_Δumin, Hc))
223+
isnothing(C_Δumax) && !isnothing(c_Δumax) && (C_Δumax = repeat(c_Δumax, Hc))
224+
isnothing(C_ymin) && !isnothing(c_ymin) && (C_ymin = repeat(c_ymin, Hp))
225+
isnothing(C_ymax) && !isnothing(c_ymax) && (C_ymax = repeat(c_ymax, Hp))
226+
if !isnothing(C_umin)
227+
size(C_umin) == (nu*Hp,) || throw(ArgumentError("C_umin size must be $((nu*Hp,))"))
228+
any(C_umin .< 0) && error("C_umin weights should be non-negative")
229+
con.A_Umin[:, end] .= -C_umin
230+
end
231+
if !isnothing(C_umax)
232+
size(C_umax) == (nu*Hp,) || throw(ArgumentError("C_umax size must be $((nu*Hp,))"))
233+
any(C_umax .< 0) && error("C_umax weights should be non-negative")
234+
con.A_Umax[:, end] .= -C_umax
235+
end
236+
if !isnothing(C_Δumin)
237+
size(C_Δumin) == (nu*Hc,) || throw(ArgumentError("C_Δumin size must be $((nu*Hc,))"))
238+
any(C_Δumin .< 0) && error("C_Δumin weights should be non-negative")
239+
con.A_ΔŨmin[1:end-1, end] .= -C_Δumin
240+
end
241+
if !isnothing(C_Δumax)
242+
size(C_Δumax) == (nu*Hc,) || throw(ArgumentError("C_Δumax size must be $((nu*Hc,))"))
243+
any(C_Δumax .< 0) && error("C_Δumax weights should be non-negative")
244+
con.A_ΔŨmax[1:end-1, end] .= -C_Δumax
245+
end
246+
if !isnothing(C_ymin)
247+
size(C_ymin) == (ny*Hp,) || throw(ArgumentError("C_ymin size must be $((ny*Hp,))"))
248+
any(C_ymin .< 0) && error("C_ymin weights should be non-negative")
249+
con.C_ymin .= C_ymin
250+
size(con.A_Ymin, 1) ≠ 0 && (con.A_Ymin[:, end] .= -con.C_ymin) # for LinModel
251+
end
252+
if !isnothing(C_ymax)
253+
size(C_ymax) == (ny*Hp,) || throw(ArgumentError("C_ymax size must be $((ny*Hp,))"))
254+
any(C_ymax .< 0) && error("C_ymax weights should be non-negative")
255+
con.C_ymax .= C_ymax
256+
size(con.A_Ymax, 1) ≠ 0 && (con.A_Ymax[:, end] .= -con.C_ymax) # for LinModel
257+
end
258+
if !isnothing(c_x̂min)
259+
size(c_x̂min) == (nx̂,) || throw(ArgumentError("c_x̂min size must be $((nx̂,))"))
260+
any(c_x̂min .< 0) && error("c_x̂min weights should be non-negative")
261+
con.c_x̂min .= c_x̂min
262+
size(con.A_x̂min, 1) ≠ 0 && (con.A_x̂min[:, end] .= -con.c_x̂min) # for LinModel
263+
end
264+
if !isnothing(c_x̂max)
265+
size(c_x̂max) == (nx̂,) || throw(ArgumentError("c_x̂max size must be $((nx̂,))"))
266+
any(c_x̂max .< 0) && error("c_x̂max weights should be non-negative")
267+
con.c_x̂max .= c_x̂max
268+
size(con.A_x̂max, 1) ≠ 0 && (con.A_x̂max[:, end] .= -con.c_x̂max) # for LinModel
269+
end
240270
end
241271
i_Umin, i_Umax = .!isinf.(con.U0min), .!isinf.(con.U0max)
242272
i_ΔŨmin, i_ΔŨmax = .!isinf.(con.ΔŨmin), .!isinf.(con.ΔŨmax)

‎src/controller/execute.jl‎

Lines changed: 54 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -543,41 +543,79 @@ prediction horizon ``H_p``.
543543
Keyword arguments with *`emphasis`* are non-Unicode alternatives.
544544
545545
- `mpc::PredictiveController` : controller to set model and weights.
546-
- `model=mpc.estim.model` : new plant model ([`NonLinModel`](@ref) not supported).
547-
- `M_Hp=mpc.M_Hp` : new ``\mathbf{M}_{H_p}`` weight matrix.
548-
- `Ñ_Hc=mpc.Ñ_Hc` or *`Ntilde_Hc`* : new ``\mathbf{Ñ}_{H_c}`` weight matrix (see definition
549-
above).
550-
- `L_Hp=mpc.L_Hp` : new ``\mathbf{L}_{H_p}`` weight matrix.
546+
- `model=mpc.estim.model` : new plant model (not supported by [`NonLinModel`](@ref)).
547+
- `Mwt=nothing` : new main diagonal in ``\mathbf{M}`` weight matrix (vector).
548+
- `Nwt=nothing` : new main diagonal in ``\mathbf{N}`` weight matrix (vector).
549+
- `Lwt=nothing` : new main diagonal in ``\mathbf{L}`` weight matrix (vector).
550+
- `M_Hp=nothing` : new ``\mathbf{M}_{H_p}`` weight matrix.
551+
- `Ñ_Hc=nothing` or *`Ntilde_Hc`* : new ``\mathbf{Ñ}_{H_c}`` weight matrix (see def. above).
552+
- `L_Hp=nothing` : new ``\mathbf{L}_{H_p}`` weight matrix.
551553
- additional keyword arguments are passed to `setmodel!(mpc.estim)`.
552554
553555
# Examples
554556
```jldoctest
555557
julia> mpc = LinMPC(KalmanFilter(LinModel(ss(0.1, 0.5, 1, 0, 4.0)), σR=[√25]), Hp=1, Hc=1);
556558
557-
julia> mpc.estim.model.A[], mpc.estim.R̂[], mpc.M_Hp[]
558-
(0.1, 25.0, 1.0)
559+
julia> mpc.estim.model.A[1], mpc.estim.R̂[1], mpc.M_Hp[1], mpc.Ñ_Hc[1]
560+
(0.1, 25.0, 1.0, 0.1)
559561
560-
julia> setmodel!(mpc, LinModel(ss(0.42, 0.5, 1, 0, 4.0)); R̂=[9], M_Hp=[0]);
562+
julia> setmodel!(mpc, LinModel(ss(0.42, 0.5, 1, 0, 4.0)); R̂=[9], M_Hp=[10], Nwt=[0.666]);
561563
562-
julia> mpc.estim.model.A[], mpc.estim.R̂[], mpc.M_Hp[]
563-
(0.42, 9.0, 0.0)
564+
julia> mpc.estim.model.A[1], mpc.estim.R̂[1], mpc.M_Hp[1], mpc.Ñ_Hc[1]
565+
(0.42, 9.0, 10.0, 0.666)
564566
```
565567
"""
566568
function setmodel!(
567569
mpc::PredictiveController,
568570
model = mpc.estim.model;
569-
M_Hp = mpc.M_Hp,
570-
Ntilde_Hc = mpc.Ñ_Hc,
571-
L_Hp = mpc.L_Hp,
571+
Mwt = nothing,
572+
Nwt = nothing,
573+
Lwt = nothing,
574+
M_Hp = nothing,
575+
Ntilde_Hc = nothing,
576+
L_Hp = nothing,
572577
Ñ_Hc = Ntilde_Hc,
573578
kwargs...
574579
)
575580
x̂op_old = copy(mpc.estim.x̂op)
576581
nu, ny, Hp, Hc, nϵ = model.nu, model.ny, mpc.Hp, mpc.Hc, mpc.nϵ
577582
setmodel!(mpc.estim, model; kwargs...)
578-
mpc.M_Hp .= to_hermitian(M_Hp)
579-
mpc.Ñ_Hc .= to_hermitian(Ñ_Hc)
580-
mpc.L_Hp .= to_hermitian(L_Hp)
583+
if isnothing(M_Hp) && !isnothing(Mwt)
584+
size(Mwt) == (ny,) || throw(ArgumentError("Mwt should be a vector of length $ny"))
585+
any(x -> x < 0, Mwt) && throw(ArgumentError("Mwt values should be nonnegative"))
586+
for i=1:ny*Hp
587+
mpc.M_Hp[i, i] = Mwt[(i-1) % ny + 1]
588+
end
589+
elseif !isnothing(M_Hp)
590+
M_Hp = to_hermitian(M_Hp)
591+
nŶ = ny*Hp
592+
size(M_Hp) == (nŶ, nŶ) || throw(ArgumentError("M_Hp size should be ($nŶ, $nŶ)"))
593+
mpc.M_Hp .= M_Hp
594+
end
595+
if isnothing(Ñ_Hc) && !isnothing(Nwt)
596+
size(Nwt) == (nu,) || throw(ArgumentError("Nwt should be a vector of length $nu"))
597+
any(x -> x < 0, Nwt) && throw(ArgumentError("Nwt values should be nonnegative"))
598+
for i=1:nu*Hc
599+
mpc.Ñ_Hc[i, i] = Nwt[(i-1) % nu + 1]
600+
end
601+
elseif !isnothing(Ñ_Hc)
602+
Ñ_Hc = to_hermitian(Ñ_Hc)
603+
nΔŨ = nu*Hc+nϵ
604+
size(Ñ_Hc) == (nΔŨ, nΔŨ) || throw(ArgumentError("Ñ_Hc size should be ($nΔŨ, $nΔŨ)"))
605+
mpc.Ñ_Hc .= Ñ_Hc
606+
end
607+
if isnothing(L_Hp) && !isnothing(Lwt)
608+
size(Lwt) == (nu,) || throw(ArgumentError("Lwt should be a vector of length $nu"))
609+
any(x -> x < 0, Lwt) && throw(ArgumentError("Lwt values should be nonnegative"))
610+
for i=1:nu*Hp
611+
mpc.L_Hp[i, i] = Lwt[(i-1) % nu + 1]
612+
end
613+
elseif !isnothing(L_Hp)
614+
L_Hp = to_hermitian(L_Hp)
615+
nU = nu*Hp
616+
size(L_Hp) == (nU, nU) || throw(ArgumentError("L_Hp size should be ($nU, $nU)"))
617+
mpc.L_Hp .= L_Hp
618+
end
581619
setmodel_controller!(mpc, x̂op_old, M_Hp, Ñ_Hc, L_Hp)
582620
return mpc
583621
end

‎src/estimator/execute.jl‎

Lines changed: 9 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -254,11 +254,13 @@ time must stay the same. Note that the observability and controllability of the
254254
augmented model is not verified (see Extended Help for more info).
255255
256256
# Arguments
257+
!!! info
258+
Keyword arguments with *`emphasis`* are non-Unicode alternatives.
257259
258260
- `estim::StateEstimator` : estimator to set model and covariances.
259-
- `model=estim.model` : new plant model ([`NonLinModel`](@ref) not supported).
260-
- `Q̂=nothing` : new augmented model ``\mathbf{Q̂}`` covariance matrix.
261-
- `R̂=nothing` : new augmented model ``\mathbf{R̂}`` covariance matrix.
261+
- `model=estim.model` : new plant model (not supported by [`NonLinModel`](@ref)).
262+
- `Q̂=nothing` or *`Qhat`* : new augmented model ``\mathbf{Q̂}`` covariance matrix.
263+
- `R̂=nothing` or *`Rhat`* : new augmented model ``\mathbf{R̂}`` covariance matrix.
262264
263265
# Examples
264266
```jldoctest
@@ -283,8 +285,10 @@ julia> kf.model.A[], kf.Q̂[1, 1], kf.Q̂[2, 2]
283285
function setmodel!(
284286
estim::StateEstimator,
285287
model = estim.model;
286-
Q̂ = nothing,
287-
R̂ = nothing
288+
Qhat = nothing,
289+
Rhat = nothing,
290+
Q̂ = Qhat,
291+
R̂ = Rhat
288292
)
289293
uop_old = copy(estim.model.uop)
290294
yop_old = copy(estim.model.yop)

0 commit comments

Comments
 (0)