Skip to content

Commit ceb9cdc

Browse files
baggepinnenbaggepinnen
committed
add utilities and tests for disturbance modeling
Apply suggestions from code review Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> rm plot
1 parent da48a1f commit ceb9cdc

File tree

4 files changed

+261
-1
lines changed

4 files changed

+261
-1
lines changed

‎src/systems/analysis_points.jl‎

Lines changed: 33 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -960,3 +960,36 @@ Compute the (linearized) loop-transfer function in analysis point `ap`, from `ap
960960
961961
See also [`get_sensitivity`](@ref), [`get_comp_sensitivity`](@ref), [`open_loop`](@ref).
962962
""" get_looptransfer
963+
#
964+
965+
"""
966+
generate_control_function(sys::ModelingToolkit.AbstractODESystem, input_ap_name::Union{Symbol, Vector{Symbol}, AnalysisPoint, Vector{AnalysisPoint}}, dist_ap_name::Union{Symbol, Vector{Symbol}, AnalysisPoint, Vector{AnalysisPoint}}; system_modifier = identity, kwargs)
967+
968+
When called with analysis points as input arguments, we assume that all analysis points corresponds to connections that should be opened (broken). The use case for this is to get rid of input signal blocks, such as `Step` or `Sine`, since these are useful for simulation but are not needed when using the plant model in a controller or state estimator.
969+
"""
970+
function generate_control_function(
971+
sys::ModelingToolkit.AbstractODESystem, input_ap_name::Union{
972+
Symbol, Vector{Symbol}, AnalysisPoint, Vector{AnalysisPoint}},
973+
dist_ap_name::Union{
974+
Nothing, Symbol, Vector{Symbol}, AnalysisPoint, Vector{AnalysisPoint}} = nothing;
975+
system_modifier = identity,
976+
kwargs...)
977+
input_ap_name = canonicalize_ap(sys, input_ap_name)
978+
u = []
979+
for input_ap in input_ap_name
980+
sys, (du, _) = open_loop(sys, input_ap)
981+
push!(u, du)
982+
end
983+
if dist_ap_name === nothing
984+
return ModelingToolkit.generate_control_function(system_modifier(sys), u; kwargs...)
985+
end
986+
987+
dist_ap_name = canonicalize_ap(sys, dist_ap_name)
988+
d = []
989+
for dist_ap in dist_ap_name
990+
sys, (du, _) = open_loop(sys, dist_ap)
991+
push!(d, du)
992+
end
993+
994+
ModelingToolkit.generate_control_function(system_modifier(sys), u, d; kwargs...)
995+
end

‎src/systems/diffeqs/odesystem.jl‎

Lines changed: 12 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -436,6 +436,8 @@ an array of inputs `inputs` is given, and `param_only` is false for a time-depen
436436
"""
437437
function build_explicit_observed_function(sys, ts;
438438
inputs = nothing,
439+
disturbance_inputs = nothing,
440+
disturbance_argument = false,
439441
expression = false,
440442
eval_expression = false,
441443
eval_module = @__MODULE__,
@@ -512,13 +514,22 @@ function build_explicit_observed_function(sys, ts;
512514
ps = setdiff(ps, inputs) # Inputs have been converted to parameters by io_preprocessing, remove those from the parameter list
513515
inputs = (inputs,)
514516
end
517+
if disturbance_inputs !== nothing
518+
# Disturbance inputs may or may not be included as inputs, depending on disturbance_argument
519+
ps = setdiff(ps, disturbance_inputs)
520+
end
521+
if disturbance_argument
522+
disturbance_inputs = (disturbance_inputs,)
523+
else
524+
disturbance_inputs = ()
525+
end
515526
ps = reorder_parameters(sys, ps)
516527
iv = if is_time_dependent(sys)
517528
(get_iv(sys),)
518529
else
519530
()
520531
end
521-
args = (dvs..., inputs..., ps..., iv...)
532+
args = (dvs..., inputs..., ps..., iv..., disturbance_inputs...)
522533
p_start = length(dvs) + length(inputs) + 1
523534
p_end = length(dvs) + length(inputs) + length(ps)
524535
fns = build_function_wrapper(

‎test/downstream/test_disturbance_model.jl‎

Lines changed: 215 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,215 @@
1+
#=
2+
This file implements and tests a typical workflow for state estimation with disturbance models
3+
The primary subject of the tests is the analysis-point features and the
4+
analysis-point specific method for `generate_control_function`.
5+
=#
6+
using ModelingToolkit, OrdinaryDiffEq, LinearAlgebra, Test
7+
using ModelingToolkitStandardLibrary.Mechanical.Rotational
8+
using ModelingToolkitStandardLibrary.Blocks
9+
using ModelingToolkit: connect
10+
# using Plots
11+
12+
using ModelingToolkit: t_nounits as t, D_nounits as D
13+
14+
indexof(sym, syms) = findfirst(isequal(sym), syms)
15+
16+
## Build the system model ======================================================
17+
@mtkmodel SystemModel begin
18+
@parameters begin
19+
m1 = 1
20+
m2 = 1
21+
k = 10 # Spring stiffness
22+
c = 3 # Damping coefficient
23+
end
24+
@components begin
25+
inertia1 = Inertia(; J = m1, phi = 0, w = 0)
26+
inertia2 = Inertia(; J = m2, phi = 0, w = 0)
27+
spring = Spring(; c = k)
28+
damper = Damper(; d = c)
29+
torque = Torque(use_support = false)
30+
end
31+
@equations begin
32+
connect(torque.flange, inertia1.flange_a)
33+
connect(inertia1.flange_b, spring.flange_a, damper.flange_a)
34+
connect(inertia2.flange_a, spring.flange_b, damper.flange_b)
35+
end
36+
end
37+
38+
@mtkmodel ModelWithInputs begin
39+
@components begin
40+
input_signal = Blocks.Sine(frequency = 1, amplitude = 1)
41+
disturbance_signal1 = Blocks.Constant(k = 0)
42+
disturbance_signal2 = Blocks.Constant(k = 0)
43+
disturbance_torque1 = Torque(use_support = false)
44+
disturbance_torque2 = Torque(use_support = false)
45+
system_model = SystemModel()
46+
end
47+
@equations begin
48+
connect(input_signal.output, :u, system_model.torque.tau)
49+
connect(disturbance_signal1.output, :d1, disturbance_torque1.tau)
50+
connect(disturbance_signal2.output, :d2, disturbance_torque2.tau)
51+
connect(disturbance_torque1.flange, system_model.inertia1.flange_b)
52+
connect(disturbance_torque2.flange, system_model.inertia2.flange_b)
53+
end
54+
end
55+
56+
@named model = ModelWithInputs() # Model with load disturbance
57+
ssys = structural_simplify(model)
58+
prob = ODEProblem(ssys, [], (0.0, 10.0))
59+
sol = solve(prob, Tsit5())
60+
# plot(sol)
61+
62+
##
63+
using ControlSystemsBase, ControlSystemsMTK
64+
cmodel = complete(model)
65+
P = cmodel.system_model
66+
lsys = named_ss(
67+
model, [:u, :d1], [P.inertia1.phi, P.inertia2.phi, P.inertia1.w, P.inertia2.w])
68+
69+
##
70+
# If we now want to add a disturbance model, we cannot do that since we have already connected a constant to the disturbance input. We have also already used the name `d` for an analysis point, but that might not be an issue since we create an outer model and get a new namespace.
71+
72+
s = tf("s")
73+
dist(; name) = ODESystem(1 / s; name)
74+
75+
@mtkmodel SystemModelWithDisturbanceModel begin
76+
@components begin
77+
input_signal = Blocks.Sine(frequency = 1, amplitude = 1)
78+
disturbance_signal1 = Blocks.Constant(k = 0)
79+
disturbance_signal2 = Blocks.Constant(k = 0)
80+
disturbance_torque1 = Torque(use_support = false)
81+
disturbance_torque2 = Torque(use_support = false)
82+
disturbance_model = dist()
83+
system_model = SystemModel()
84+
end
85+
@equations begin
86+
connect(input_signal.output, :u, system_model.torque.tau)
87+
connect(disturbance_signal1.output, :d1, disturbance_model.input)
88+
connect(disturbance_model.output, disturbance_torque1.tau)
89+
connect(disturbance_signal2.output, :d2, disturbance_torque2.tau)
90+
connect(disturbance_torque1.flange, system_model.inertia1.flange_b)
91+
connect(disturbance_torque2.flange, system_model.inertia2.flange_b)
92+
end
93+
end
94+
95+
@named model_with_disturbance = SystemModelWithDisturbanceModel()
96+
# ssys = structural_simplify(open_loop(model_with_disturbance, :d)) # Open loop worked, but it's a bit awkward that we have to use it here
97+
# lsys2 = named_ss(model_with_disturbance, [:u, :d1],
98+
# [P.inertia1.phi, P.inertia2.phi, P.inertia1.w, P.inertia2.w])
99+
ssys = structural_simplify(model_with_disturbance)
100+
prob = ODEProblem(ssys, [], (0.0, 10.0))
101+
sol = solve(prob, Tsit5())
102+
@test SciMLBase.successful_retcode(sol)
103+
# plot(sol)
104+
105+
##
106+
# Now we only have an integrating disturbance affecting inertia1, what if we want both integrating and direct Gaussian? We'd need a "PI controller" disturbancemodel. If we add the disturbance model (s+1)/s we get the integrating and non-integrating noises being correlated which is fine, it reduces the dimensions of the sigma point by 1.
107+
108+
dist3(; name) = ODESystem(ss(1 + 10 / s, balance = false); name)
109+
110+
@mtkmodel SystemModelWithDisturbanceModel begin
111+
@components begin
112+
input_signal = Blocks.Sine(frequency = 1, amplitude = 1)
113+
disturbance_signal1 = Blocks.Constant(k = 0)
114+
disturbance_signal2 = Blocks.Constant(k = 0)
115+
disturbance_torque1 = Torque(use_support = false)
116+
disturbance_torque2 = Torque(use_support = false)
117+
disturbance_model = dist3()
118+
system_model = SystemModel()
119+
120+
y = Blocks.Add()
121+
angle_sensor = AngleSensor()
122+
output_disturbance = Blocks.Constant(k = 0)
123+
end
124+
@equations begin
125+
connect(input_signal.output, :u, system_model.torque.tau)
126+
connect(disturbance_signal1.output, :d1, disturbance_model.input)
127+
connect(disturbance_model.output, disturbance_torque1.tau)
128+
connect(disturbance_signal2.output, :d2, disturbance_torque2.tau)
129+
connect(disturbance_torque1.flange, system_model.inertia1.flange_b)
130+
connect(disturbance_torque2.flange, system_model.inertia2.flange_b)
131+
132+
connect(system_model.inertia1.flange_b, angle_sensor.flange)
133+
connect(angle_sensor.phi, y.input1)
134+
connect(output_disturbance.output, :dy, y.input2)
135+
end
136+
end
137+
138+
@named model_with_disturbance = SystemModelWithDisturbanceModel()
139+
# ssys = structural_simplify(open_loop(model_with_disturbance, :d)) # Open loop worked, but it's a bit awkward that we have to use it here
140+
# lsys3 = named_ss(model_with_disturbance, [:u, :d1],
141+
# [P.inertia1.phi, P.inertia2.phi, P.inertia1.w, P.inertia2.w])
142+
ssys = structural_simplify(model_with_disturbance)
143+
prob = ODEProblem(ssys, [], (0.0, 10.0))
144+
sol = solve(prob, Tsit5())
145+
@test SciMLBase.successful_retcode(sol)
146+
# plot(sol)
147+
148+
## Generate function for an augmented Unscented Kalman Filter =====================
149+
# temp = open_loop(model_with_disturbance, :d)
150+
outputs = [P.inertia1.phi, P.inertia2.phi, P.inertia1.w, P.inertia2.w]
151+
(f_oop1, f_ip), x_sym, p_sym, io_sys = ModelingToolkit.generate_control_function(
152+
model_with_disturbance, [:u], [:d1, :d2, :dy], split = false)
153+
154+
(f_oop2, f_ip2), x_sym, p_sym, io_sys = ModelingToolkit.generate_control_function(
155+
model_with_disturbance, [:u], [:d1, :d2, :dy],
156+
disturbance_argument = true, split = false)
157+
158+
measurement = ModelingToolkit.build_explicit_observed_function(
159+
io_sys, outputs, inputs = ModelingToolkit.inputs(io_sys)[1:1])
160+
measurement2 = ModelingToolkit.build_explicit_observed_function(
161+
io_sys, [io_sys.y.output.u], inputs = ModelingToolkit.inputs(io_sys)[1:1],
162+
disturbance_inputs = ModelingToolkit.inputs(io_sys)[2:end],
163+
disturbance_argument = true)
164+
165+
op = ModelingToolkit.inputs(io_sys) .=> 0
166+
x0, p = ModelingToolkit.get_u0_p(io_sys, op, op)
167+
x = zeros(5)
168+
u = zeros(1)
169+
d = zeros(3)
170+
@test f_oop2(x, u, p, t, d) == zeros(5)
171+
@test measurement(x, u, p, 0.0) == [0, 0, 0, 0]
172+
@test measurement2(x, u, p, 0.0, d) == [0]
173+
174+
# Add to the integrating disturbance input
175+
d = [1, 0, 0]
176+
@test sort(f_oop2(x, u, p, 0.0, d)) == [0, 0, 0, 1, 1] # Affects disturbance state and one velocity
177+
@test measurement2(x, u, p, 0.0, d) == [0]
178+
179+
d = [0, 1, 0]
180+
@test sort(f_oop2(x, u, p, 0.0, d)) == [0, 0, 0, 0, 1] # Affects one velocity
181+
@test measurement(x, u, p, 0.0) == [0, 0, 0, 0]
182+
@test measurement2(x, u, p, 0.0, d) == [0]
183+
184+
d = [0, 0, 1]
185+
@test sort(f_oop2(x, u, p, 0.0, d)) == [0, 0, 0, 0, 0] # Affects nothing
186+
@test measurement(x, u, p, 0.0) == [0, 0, 0, 0]
187+
@test measurement2(x, u, p, 0.0, d) == [1] # We have now disturbed the output
188+
189+
## Further downstream tests that the functions generated above actually have the properties required to use for state estimation
190+
#
191+
# using LowLevelParticleFilters, SeeToDee
192+
# Ts = 0.001
193+
# discrete_dynamics = SeeToDee.Rk4(f_oop2, Ts)
194+
# nx = length(x_sym)
195+
# nu = 1
196+
# nw = 2
197+
# ny = length(outputs)
198+
# R1 = Diagonal([1e-5, 1e-5])
199+
# R2 = 0.1 * I(ny)
200+
# op = ModelingToolkit.inputs(io_sys) .=> 0
201+
# x0, p = ModelingToolkit.get_u0_p(io_sys, op, op)
202+
# d0 = LowLevelParticleFilters.SimpleMvNormal(x0, 10.0I(nx))
203+
# measurement_model = UKFMeasurementModel{Float64, false, false}(measurement, R2; nx, ny)
204+
# kf = UnscentedKalmanFilter{false, false, true, false}(
205+
# discrete_dynamics, measurement_model, R1, d0; nu, Ts, p)
206+
207+
# tvec = 0:Ts:sol.t[end]
208+
# u = vcat.(Array(sol(tvec, idxs = P.torque.tau.u)))
209+
# y = collect.(eachcol(Array(sol(tvec, idxs = outputs)) .+ 1e-2 .* randn.()))
210+
211+
# inds = 1:5805
212+
# res = forward_trajectory(kf, u, y)
213+
214+
# plot(res, size = (1000, 1000));
215+
# plot!(sol, idxs = x_sym, sp = (1:nx)', l = :dash);

‎test/runtests.jl‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -120,6 +120,7 @@ end
120120
@safetestset "Linearization Dummy Derivative Tests" include("downstream/linearization_dd.jl")
121121
@safetestset "Inverse Models Test" include("downstream/inversemodel.jl")
122122
@safetestset "Analysis Points Test" include("downstream/analysis_points.jl")
123+
@safetestset "Analysis Points Test" include("downstream/test_disturbance_model.jl")
123124
end
124125

125126
if GROUP == "All" || GROUP == "FMI"

0 commit comments

Comments
 (0)