Skip to content

Commit 218b0f1

Browse files
franckgagafranckgaga
committed
added: NonLinModel support for p and t/k arguments.
1 parent 422958d commit 218b0f1

File tree

4 files changed

+89
-66
lines changed

4 files changed

+89
-66
lines changed

‎src/model/linmodel.jl‎

Lines changed: 7 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@ struct LinModel{NT<:Real} <: SimModel{NT}
55
Bd ::Matrix{NT}
66
Dd ::Matrix{NT}
77
x0::Vector{NT}
8+
k::Vector{Int}
89
Ts::NT
910
t::Vector{NT}
1011
nu::Int
@@ -36,6 +37,7 @@ struct LinModel{NT<:Real} <: SimModel{NT}
3637
size(Bd) == (nx,nd) || error("Bd size must be $((nx,nd))")
3738
size(Dd) == (ny,nd) || error("Dd size must be $((ny,nd))")
3839
Ts > 0 || error("Sampling time Ts must be positive")
40+
k = [0]
3941
uop = zeros(NT, nu)
4042
yop = zeros(NT, ny)
4143
dop = zeros(NT, nd)
@@ -51,7 +53,7 @@ struct LinModel{NT<:Real} <: SimModel{NT}
5153
return new{NT}(
5254
A, Bu, C, Bd, Dd,
5355
x0,
54-
Ts, t,
56+
k, Ts, t,
5557
nu, nx, ny, nd,
5658
uop, yop, dop, xop, fop,
5759
uname, yname, dname, xname,
@@ -254,11 +256,11 @@ function steadystate!(model::LinModel, u0, d0)
254256
end
255257

256258
"""
257-
f!(xnext0, model::LinModel, x0, u0, d0) -> nothing
259+
f!(xnext0, model::LinModel, x0, u0, d0, _ , _ ) -> nothing
258260
259261
Evaluate `xnext0 = A*x0 + Bu*u0 + Bd*d0` in-place when `model` is a [`LinModel`](@ref).
260262
"""
261-
function f!(xnext0, model::LinModel, x0, u0, d0)
263+
function f!(xnext0, model::LinModel, x0, u0, d0, _ , _ )
262264
mul!(xnext0, model.A, x0)
263265
mul!(xnext0, model.Bu, u0, 1, 1)
264266
mul!(xnext0, model.Bd, d0, 1, 1)
@@ -267,11 +269,11 @@ end
267269

268270

269271
"""
270-
h!(y0, model::LinModel, x0, d0) -> nothing
272+
h!(y0, model::LinModel, x0, d0, _ , _ ) -> nothing
271273
272274
Evaluate `y0 = C*x0 + Dd*d0` in-place when `model` is a [`LinModel`](@ref).
273275
"""
274-
function h!(y0, model::LinModel, x0, d0)
276+
function h!(y0, model::LinModel, x0, d0, _ , _ )
275277
mul!(y0, model.C, x0)
276278
mul!(y0, model.Dd, d0, 1, 1)
277279
return nothing

‎src/model/nonlinmodel.jl‎

Lines changed: 63 additions & 47 deletions
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,12 @@
1-
struct NonLinModel{NT<:Real, F<:Function, H<:Function, DS<:DiffSolver} <: SimModel{NT}
1+
struct NonLinModel{
2+
NT<:Real, F<:Function, H<:Function, P<:Any, DS<:DiffSolver
3+
} <: SimModel{NT}
24
x0::Vector{NT}
35
f!::F
46
h!::H
7+
p::P
58
solver::DS
9+
k::Vector{Int}
610
Ts::NT
711
t::Vector{NT}
812
nu::Int
@@ -19,10 +23,11 @@ struct NonLinModel{NT<:Real, F<:Function, H<:Function, DS<:DiffSolver} <: SimMod
1923
dname::Vector{String}
2024
xname::Vector{String}
2125
buffer::SimModelBuffer{NT}
22-
function NonLinModel{NT, F, H, DS}(
23-
f!::F, h!::H, solver::DS, Ts, nu, nx, ny, nd
24-
) where {NT<:Real, F<:Function, H<:Function, DS<:DiffSolver}
26+
function NonLinModel{NT, F, H, P, DS}(
27+
f!::F, h!::H, Ts, nu, nx, ny, nd, p::P, solver::DS
28+
) where {NT<:Real, F<:Function, H<:Function, P<:Any, DS<:DiffSolver}
2529
Ts > 0 || error("Sampling time Ts must be positive")
30+
k = [0]
2631
uop = zeros(NT, nu)
2732
yop = zeros(NT, ny)
2833
dop = zeros(NT, nd)
@@ -35,11 +40,12 @@ struct NonLinModel{NT<:Real, F<:Function, H<:Function, DS<:DiffSolver} <: SimMod
3540
x0 = zeros(NT, nx)
3641
t = zeros(NT, 1)
3742
buffer = SimModelBuffer{NT}(nu, nx, ny, nd)
38-
return new{NT, F, H, DS}(
43+
return new{NT, F, H, P, DS}(
3944
x0,
40-
f!, h!,
45+
f!, h!,
46+
p,
4147
solver,
42-
Ts, t,
48+
k, Ts, t,
4349
nu, nx, ny, nd,
4450
uop, yop, dop, xop, fop,
4551
uname, yname, dname, xname,
@@ -49,8 +55,8 @@ struct NonLinModel{NT<:Real, F<:Function, H<:Function, DS<:DiffSolver} <: SimMod
4955
end
5056

5157
@doc raw"""
52-
NonLinModel{NT}(f::Function, h::Function, Ts, nu, nx, ny, nd=0; solver=RungeKutta(4))
53-
NonLinModel{NT}(f!::Function, h!::Function, Ts, nu, nx, ny, nd=0; solver=RungeKutta(4))
58+
NonLinModel{NT}(f::Function, h::Function, Ts, nu, nx, ny, nd=0; p=[], solver=RungeKutta(4))
59+
NonLinModel{NT}(f!::Function, h!::Function, Ts, nu, nx, ny, nd=0; p=[], solver=RungeKutta(4))
5460
5561
Construct a nonlinear model from state-space functions `f`/`f!` and `h`/`h!`.
5662
@@ -59,23 +65,27 @@ continuous dynamics. Use `solver=nothing` for the discrete case (see Extended He
5965
functions are defined as:
6066
```math
6167
\begin{aligned}
62-
\mathbf{ẋ}(t) &= \mathbf{f}\Big( \mathbf{x}(t), \mathbf{u}(t), \mathbf{d}(t) \Big) \\
63-
\mathbf{y}(t) &= \mathbf{h}\Big( \mathbf{x}(t), \mathbf{d}(t) \Big)
68+
\mathbf{ẋ}(t) &= \mathbf{f}\Big( \mathbf{x}(t), \mathbf{u}(t), \mathbf{d}(t), \mathbf{p}(t), t \Big) \\
69+
\mathbf{y}(t) &= \mathbf{h}\Big( \mathbf{x}(t), \mathbf{d}(t), \mathbf{p}(t), t \Big)
6470
\end{aligned}
6571
```
66-
They can be implemented in two possible ways:
67-
68-
1. **Non-mutating functions** (out-of-place): define them as `f(x, u, d) -> ẋ` and
69-
`h(x, d) -> y`. This syntax is simple and intuitive but it allocates more memory.
70-
2. **Mutating functions** (in-place): define them as `f!(ẋ, x, u, d) -> nothing` and
71-
`h!(y, x, d) -> nothing`. This syntax reduces the allocations and potentially the
72+
where ``\mathbf{x}``, ``\mathbf{y}, ``\mathbf{u}``, ``\mathbf{d}`` and ``\mathbf{p}`` are
73+
respectively the state, output, manipulated input, measured disturbance and parameter vectors,
74+
and ``t`` the time in second. The functions can be implemented in two possible ways:
75+
76+
1. **Non-mutating functions** (out-of-place): define them as `f(x, u, d, p, t) -> ẋ` and
77+
`h(x, d, p, t) -> y`. This syntax is simple and intuitive but it allocates more memory.
78+
2. **Mutating functions** (in-place): define them as `f!(ẋ, x, u, d, p, t) -> nothing` and
79+
`h!(y, x, d, p, t) -> nothing`. This syntax reduces the allocations and potentially the
7280
computational burden as well.
7381
7482
`Ts` is the sampling time in second. `nu`, `nx`, `ny` and `nd` are the respective number of
75-
manipulated inputs, states, outputs and measured disturbances.
83+
manipulated inputs, states, outputs and measured disturbances. The keyword argument `p`
84+
is the parameters of the model passed to the two functions. It can be of any Julia object
85+
but use a mutable type if you want to change them later e.g.: a vector.
7686
7787
!!! tip
78-
Replace the `d` argument with `_` if `nd = 0` (see Examples below).
88+
Replace the `d`, `p` or `t` argument with `_` if not needed (see Examples below).
7989
8090
A 4th order [`RungeKutta`](@ref) solver discretizes the differential equations by default.
8191
The rest of the documentation assumes discrete dynamics since all models end up in this
@@ -118,52 +128,58 @@ NonLinModel with a sample time Ts = 2.0 s, empty solver and:
118128
State-space functions are similar for discrete dynamics:
119129
```math
120130
\begin{aligned}
121-
\mathbf{x}(k+1) &= \mathbf{f}\Big( \mathbf{x}(k), \mathbf{u}(k), \mathbf{d}(k) \Big) \\
122-
\mathbf{y}(k) &= \mathbf{h}\Big( \mathbf{x}(k), \mathbf{d}(k) \Big)
131+
\mathbf{x}(k+1) &= \mathbf{f}\Big( \mathbf{x}(k), \mathbf{u}(k), \mathbf{d}(k), \mathbf{p}(k), k \Big) \\
132+
\mathbf{y}(k) &= \mathbf{h}\Big( \mathbf{x}(k), \mathbf{d}(k), \mathbf{p}(k), k \Big)
123133
\end{aligned}
124134
```
125135
with two possible implementations as well:
126136
127-
1. **Non-mutating functions**: define them as `f(x, u, d) -> xnext` and `h(x, d) -> y`.
128-
2. **Mutating functions**: define them as `f!(xnext, x, u, d) -> nothing` and
129-
`h!(y, x, d) -> nothing`.
137+
1. **Non-mutating functions**: define them as `f(x, u, d, p, k) -> xnext` and
138+
`h(x, d, p, k) -> y`.
139+
2. **Mutating functions**: define them as `f!(xnext, x, u, d, p, k) -> nothing` and
140+
`h!(y, x, d, p, k) -> nothing`.
130141
"""
131142
function NonLinModel{NT}(
132-
f::Function, h::Function, Ts::Real, nu::Int, nx::Int, ny::Int, nd::Int=0;
133-
solver=RungeKutta(4)
143+
f::Function, h::Function, Ts::Real, nu::Int, nx::Int, ny::Int, nd::Int=0;
144+
p=NT[], solver=RungeKutta(4)
134145
) where {NT<:Real}
135146
isnothing(solver) && (solver=EmptySolver())
136147
ismutating_f = validate_f(NT, f)
137148
ismutating_h = validate_h(NT, h)
138-
f! = ismutating_f ? f : (xnext, x, u, d) -> xnext .= f(x, u, d)
139-
h! = ismutating_h ? h : (y, x, d) -> y .= h(x, d)
149+
f! = ismutating_f ? f : f!(xnext, x, u, d, p, k) = (xnext .= f(x, u, d, p, k); nothing)
150+
h! = ismutating_h ? h : h!(y, x, d, p, k) = (y .= h(x, d, p, k); nothing)
140151
f!, h! = get_solver_functions(NT, solver, f!, h!, Ts, nu, nx, ny, nd)
141-
F, H, DS = get_types(f!, h!, solver)
142-
return NonLinModel{NT, F, H, DS}(f!, h!, solver, Ts, nu, nx, ny, nd)
152+
F, H, P, DS = get_types(f!, h!, p, solver)
153+
return NonLinModel{NT, F, H, P, DS}(f!, h!, Ts, nu, nx, ny, nd, p, solver)
143154
end
144155

145156
function NonLinModel(
146-
f::Function, h::Function, Ts::Real, nu::Int, nx::Int, ny::Int, nd::Int=0;
147-
solver=RungeKutta(4)
157+
f::Function, h::Function, Ts::Real, nu::Int, nx::Int, ny::Int, nd::Int=0;
158+
p=Float64[], solver=RungeKutta(4)
148159
)
149-
return NonLinModel{Float64}(f, h, Ts, nu, nx, ny, nd; solver)
160+
return NonLinModel{Float64}(f, h, Ts, nu, nx, ny, nd; p, solver)
150161
end
151162

152163
"Get the types of `f!`, `h!` and `solver` to construct a `NonLinModel`."
153-
get_types(::F, ::H, ::DS) where {F<:Function, H<:Function, DS<:DiffSolver} = F, H, DS
164+
function get_types(
165+
::F, ::H, ::P, ::DS
166+
) where {F<:Function, H<:Function, P<:Any, DS<:DiffSolver}
167+
return F, H, P, DS
168+
end
154169

155170
"""
156171
validate_f(NT, f) -> ismutating
157172
158173
Validate `f` function argument signature and return `true` if it is mutating.
159174
"""
160175
function validate_f(NT, f)
161-
ismutating = hasmethod(f, Tuple{Vector{NT}, Vector{NT}, Vector{NT}, Vector{NT}})
162-
if !(ismutating || hasmethod(f, Tuple{Vector{NT}, Vector{NT}, Vector{NT}}))
176+
ismutating = hasmethod(f, Tuple{Vector{NT}, Vector{NT}, Vector{NT}, Vector{NT}, Any, NT})
177+
if !(ismutating || hasmethod(f, Tuple{Vector{NT}, Vector{NT}, Vector{NT}, Any, NT}))
163178
error(
164179
"the state function has no method with type signature "*
165-
"f(x::Vector{$(NT)}, u::Vector{$(NT)}, d::Vector{$(NT)}) or mutating form "*
166-
"f!(xnext::Vector{$(NT)}, x::Vector{$(NT)}, u::Vector{$(NT)}, d::Vector{$(NT)})"
180+
"f(x::Vector{$(NT)}, u::Vector{$(NT)}, d::Vector{$(NT)}, p::Any, t::$(NT)) or "*
181+
"mutating form f!(xnext::Vector{$(NT)}, x::Vector{$(NT)}, u::Vector{$(NT)}, "*
182+
"d::Vector{$(NT)}, p::Any, t::$(NT))"
167183
)
168184
end
169185
return ismutating
@@ -175,12 +191,12 @@ end
175191
Validate `h` function argument signature and return `true` if it is mutating.
176192
"""
177193
function validate_h(NT, h)
178-
ismutating = hasmethod(h, Tuple{Vector{NT}, Vector{NT}, Vector{NT}})
179-
if !(ismutating || hasmethod(h, Tuple{Vector{NT}, Vector{NT}}))
194+
ismutating = hasmethod(h, Tuple{Vector{NT}, Vector{NT}, Vector{NT}, Any, NT})
195+
if !(ismutating || hasmethod(h, Tuple{Vector{NT}, Vector{NT}, Any, NT}))
180196
error(
181197
"the output function has no method with type signature "*
182-
"h(x::Vector{$(NT)}, d::Vector{$(NT)}) or mutating form "*
183-
"h!(y::Vector{$(NT)}, x::Vector{$(NT)}, d::Vector{$(NT)})"
198+
"h(x::Vector{$(NT)}, d::Vector{$(NT)}, p::Any, t::$(NT)) or mutating form "*
199+
"h!(y::Vector{$(NT)}, x::Vector{$(NT)}, d::Vector{$(NT)}, p::Any, t::$(NT))"
184200
)
185201
end
186202
return ismutating
@@ -189,11 +205,11 @@ end
189205
"Do nothing if `model` is a [`NonLinModel`](@ref)."
190206
steadystate!(::SimModel, _ , _ ) = nothing
191207

192-
"Call `f!(xnext0, x0, u0, d0)` with `model.f!` method for [`NonLinModel`](@ref)."
193-
f!(xnext0, model::NonLinModel, x0, u0, d0) = model.f!(xnext0, x0, u0, d0)
208+
"Call `f!(xnext0, x0, u0, d0, p, k)` with `model.f!` method for [`NonLinModel`](@ref)."
209+
f!(xnext0, model::NonLinModel, x0, u0, d0, p, k) = model.f!(xnext0, x0, u0, d0, p, k)
194210

195-
"Call `h!(y0, x0, d0)` with `model.h` method for [`NonLinModel`](@ref)."
196-
h!(y0, model::NonLinModel, x0, d0) = model.h!(y0, x0, d0)
211+
"Call `h!(y0, x0, d0, p, k)` with `model.h` method for [`NonLinModel`](@ref)."
212+
h!(y0, model::NonLinModel, x0, d0, p, k) = model.h!(y0, x0, d0, p, k)
197213

198214
detailstr(model::NonLinModel) = ", $(typeof(model.solver).name.name) solver"
199-
detailstr(::NonLinModel{<:Real, <:Function, <:Function, <:EmptySolver}) = ", empty solver"
215+
detailstr(::NonLinModel{<:Real, <:Function, <:Function, <:Any, <:EmptySolver}) = ", empty solver"

‎src/model/solver.jl‎

Lines changed: 9 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -45,7 +45,7 @@ function get_solver_functions(NT::DataType, solver::RungeKutta, fc!, hc!, Ts, _
4545
k2_cache::DiffCache{Vector{NT}, Vector{NT}} = DiffCache(zeros(NT, nx), Nc)
4646
k3_cache::DiffCache{Vector{NT}, Vector{NT}} = DiffCache(zeros(NT, nx), Nc)
4747
k4_cache::DiffCache{Vector{NT}, Vector{NT}} = DiffCache(zeros(NT, nx), Nc)
48-
f! = function inner_solver(xnext, x, u, d)
48+
f! = function inner_solver_f!(xnext, x, u, d, p, k)
4949
CT = promote_type(eltype(x), eltype(u), eltype(d))
5050
# dummy variable for get_tmp, necessary for PreallocationTools + Julia 1.6 :
5151
var::CT = 0
@@ -55,22 +55,23 @@ function get_solver_functions(NT::DataType, solver::RungeKutta, fc!, hc!, Ts, _
5555
k3 = get_tmp(k3_cache, var)
5656
k4 = get_tmp(k4_cache, var)
5757
@. xcur = x
58-
for i=1:solver.supersample
59-
xterm = xnext
58+
for k_inner=0:solver.supersample-1
59+
xterm = xnext # TODO: move this out of the loop, just above (to test) ?
60+
t = k*Ts + k_inner*Ts_inner
6061
@. xterm = xcur
61-
fc!(k1, xterm, u, d)
62+
fc!(k1, xterm, u, d, p, t)
6263
@. xterm = xcur + k1 * Ts_inner/2
63-
fc!(k2, xterm, u, d)
64+
fc!(k2, xterm, u, d, p, t)
6465
@. xterm = xcur + k2 * Ts_inner/2
65-
fc!(k3, xterm, u, d)
66+
fc!(k3, xterm, u, d, p, t)
6667
@. xterm = xcur + k3 * Ts_inner
67-
fc!(k4, xterm, u, d)
68+
fc!(k4, xterm, u, d, p, t)
6869
@. xcur = xcur + (k1 + 2k2 + 2k3 + k4)*Ts_inner/6
6970
end
7071
@. xnext = xcur
7172
return nothing
7273
end
73-
h! = hc!
74+
h!(y, x, d, p, k) = (hc!(y, x, d, p, k*Ts); nothing)
7475
return f!, h!
7576
end
7677

‎src/sim_model.jl‎

Lines changed: 10 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -187,10 +187,11 @@ detailstr(model::SimModel) = ""
187187
@doc raw"""
188188
initstate!(model::SimModel, u, d=[]) -> x
189189
190-
Init `model.x0` with manipulated inputs `u` and measured disturbances `d` steady-state.
190+
Init `model.x0` with manipulated inputs `u` and meas. dist. `d` steady-state and reset time.
191191
192-
The method tries to initialize the model state ``\mathbf{x}`` at steady-state. It removes
193-
the operating points on `u` and `d` and calls [`steadystate!`](@ref):
192+
The method resets the discrete time step `model.k` at `0` and tries to initialize the model
193+
state ``\mathbf{x}`` at steady-state. It removes the operating points on `u` and `d` and
194+
calls [`steadystate!`](@ref):
194195
195196
- If `model` is a [`LinModel`](@ref), the method computes the steady-state of current
196197
inputs `u` and measured disturbances `d`.
@@ -210,6 +211,7 @@ true
210211
"""
211212
function initstate!(model::SimModel, u, d=model.buffer.empty)
212213
validate_args(model::SimModel, d, u)
214+
model.k[] = 0
213215
u0, d0 = model.buffer.u, model.buffer.d
214216
u0 .= u .- model.uop
215217
d0 .= d .- model.dop
@@ -233,9 +235,10 @@ end
233235
@doc raw"""
234236
updatestate!(model::SimModel, u, d=[]) -> xnext
235237
236-
Update `model.x0` states with current inputs `u` and measured disturbances `d`.
238+
Update `model.x0` states with current inputs `u` and meas. dist. `d` and increment `model.k`.
237239
238240
The method computes and returns the model state for the next time step ``\mathbf{x}(k+1)``.
241+
It also increment the discrete time step `model.k` by `1`.
239242
240243
# Examples
241244
```jldoctest
@@ -248,10 +251,11 @@ julia> x = updatestate!(model, [1])
248251
"""
249252
function updatestate!(model::SimModel{NT}, u, d=model.buffer.empty) where NT <: Real
250253
validate_args(model::SimModel, d, u)
254+
model.k[] += 1
251255
u0, d0, xnext0 = model.buffer.u, model.buffer.d, model.buffer.x
252256
u0 .= u .- model.uop
253257
d0 .= d .- model.dop
254-
f!(xnext0, model, model.x0, u0, d0)
258+
f!(xnext0, model, model.x0, u0, d0, model.p, model.k[])
255259
xnext0 .+= model.fop .- model.xop
256260
model.x0 .= xnext0
257261
xnext = xnext0
@@ -280,7 +284,7 @@ function evaloutput(model::SimModel{NT}, d=model.buffer.empty) where NT <: Real
280284
validate_args(model, d)
281285
d0, y0 = model.buffer.d, model.buffer.y
282286
d0 .= d .- model.dop
283-
h!(y0, model, model.x0, d0)
287+
h!(y0, model, model.x0, d0, model.p, model.k[])
284288
y = y0
285289
y .+= model.yop
286290
return y

0 commit comments

Comments
 (0)