Skip to content

Commit 1d2c5db

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 b107633 commit 1d2c5db

File tree

4 files changed

+261
-1
lines changed

4 files changed

+261
-1
lines changed

‎src/systems/analysis_points.jl

+33
Original file line numberDiff line numberDiff line change
@@ -961,3 +961,36 @@ Compute the (linearized) loop-transfer function in analysis point `ap`, from `ap
961961
962962
See also [`get_sensitivity`](@ref), [`get_comp_sensitivity`](@ref), [`open_loop`](@ref).
963963
""" get_looptransfer
964+
#
965+
966+
"""
967+
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)
968+
969+
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.
970+
"""
971+
function generate_control_function(
972+
sys::ModelingToolkit.AbstractODESystem, input_ap_name::Union{
973+
Symbol, Vector{Symbol}, AnalysisPoint, Vector{AnalysisPoint}},
974+
dist_ap_name::Union{
975+
Nothing, Symbol, Vector{Symbol}, AnalysisPoint, Vector{AnalysisPoint}} = nothing;
976+
system_modifier = identity,
977+
kwargs...)
978+
input_ap_name = canonicalize_ap(sys, input_ap_name)
979+
u = []
980+
for input_ap in input_ap_name
981+
sys, (du, _) = open_loop(sys, input_ap)
982+
push!(u, du)
983+
end
984+
if dist_ap_name === nothing
985+
return ModelingToolkit.generate_control_function(system_modifier(sys), u; kwargs...)
986+
end
987+
988+
dist_ap_name = canonicalize_ap(sys, dist_ap_name)
989+
d = []
990+
for dist_ap in dist_ap_name
991+
sys, (du, _) = open_loop(sys, dist_ap)
992+
push!(d, du)
993+
end
994+
995+
ModelingToolkit.generate_control_function(system_modifier(sys), u, d; kwargs...)
996+
end

‎src/systems/diffeqs/odesystem.jl

+12-1
Original file line numberDiff line numberDiff line change
@@ -419,6 +419,8 @@ an array of inputs `inputs` is given, and `param_only` is false for a time-depen
419419
"""
420420
function build_explicit_observed_function(sys, ts;
421421
inputs = nothing,
422+
disturbance_inputs = nothing,
423+
disturbance_argument = false,
422424
expression = false,
423425
eval_expression = false,
424426
eval_module = @__MODULE__,
@@ -500,13 +502,22 @@ function build_explicit_observed_function(sys, ts;
500502
ps = setdiff(ps, inputs) # Inputs have been converted to parameters by io_preprocessing, remove those from the parameter list
501503
inputs = (inputs,)
502504
end
505+
if disturbance_inputs !== nothing
506+
# Disturbance inputs may or may not be included as inputs, depending on disturbance_argument
507+
ps = setdiff(ps, disturbance_inputs)
508+
end
509+
if disturbance_argument
510+
disturbance_inputs = (disturbance_inputs,)
511+
else
512+
disturbance_inputs = ()
513+
end
503514
ps = reorder_parameters(sys, ps)
504515
iv = if is_time_dependent(sys)
505516
(get_iv(sys),)
506517
else
507518
()
508519
end
509-
args = (dvs..., inputs..., ps..., iv...)
520+
args = (dvs..., inputs..., ps..., iv..., disturbance_inputs...)
510521
p_start = length(dvs) + length(inputs) + 1
511522
p_end = length(dvs) + length(inputs) + length(ps)
512523
fns = build_function_wrapper(

‎test/downstream/test_disturbance_model.jl

+215
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

+1
Original file line numberDiff line numberDiff line change
@@ -119,6 +119,7 @@ end
119119
@safetestset "Linearization Dummy Derivative Tests" include("downstream/linearization_dd.jl")
120120
@safetestset "Inverse Models Test" include("downstream/inversemodel.jl")
121121
@safetestset "Analysis Points Test" include("downstream/analysis_points.jl")
122+
@safetestset "Analysis Points Test" include("downstream/test_disturbance_model.jl")
122123
end
123124

124125
if GROUP == "All" || GROUP == "FMI"

0 commit comments

Comments
 (0)