Skip to content

Commit 31059f4

Browse files
baggepinnenbaggepinnen
committed
add utilities and tests for disturbance modeling
1 parent 083a639 commit 31059f4

File tree

4 files changed

+265
-3
lines changed

4 files changed

+265
-3
lines changed

‎src/systems/analysis_points.jl

+34
Original file line numberDiff line numberDiff line change
@@ -944,3 +944,37 @@ Compute the (linearized) loop-transfer function in analysis point `ap`, from `ap
944944
945945
See also [`get_sensitivity`](@ref), [`get_comp_sensitivity`](@ref), [`open_loop`](@ref).
946946
""" get_looptransfer
947+
#
948+
949+
"""
950+
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)
951+
952+
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.
953+
"""
954+
function generate_control_function(
955+
sys::ModelingToolkit.AbstractODESystem, input_ap_name::Union{
956+
Symbol, Vector{Symbol}, AnalysisPoint, Vector{AnalysisPoint}},
957+
dist_ap_name::Union{
958+
Nothing, Symbol, Vector{Symbol}, AnalysisPoint, Vector{AnalysisPoint}} = nothing;
959+
system_modifier = identity,
960+
kwargs...)
961+
input_ap_name = canonicalize_ap(input_ap_name)
962+
u = []
963+
for input_ap in input_ap_name
964+
sys, (du, _) = open_loop(sys, input_ap)
965+
push!(u, du)
966+
end
967+
if dist_ap_name === nothing
968+
return ModelingToolkit.generate_control_function(system_modifier(sys), u; kwargs...)
969+
end
970+
971+
dist_ap_name = canonicalize_ap(dist_ap_name)
972+
d = []
973+
for dist_ap in dist_ap_name
974+
sys, (du, _) = open_loop(sys, dist_ap)
975+
push!(d, du)
976+
end
977+
978+
ModelingToolkit.generate_control_function(system_modifier(sys), u, d; kwargs...)
979+
end
980+

‎src/systems/diffeqs/odesystem.jl

+15-3
Original file line numberDiff line numberDiff line change
@@ -449,6 +449,8 @@ an array of inputs `inputs` is given, and `param_only` is false for a time-depen
449449
"""
450450
function build_explicit_observed_function(sys, ts;
451451
inputs = nothing,
452+
disturbance_inputs = nothing,
453+
disturbance_argument = false,
452454
expression = false,
453455
eval_expression = false,
454456
eval_module = @__MODULE__,
@@ -575,12 +577,15 @@ function build_explicit_observed_function(sys, ts;
575577
if inputs !== nothing
576578
ps = setdiff(ps, inputs) # Inputs have been converted to parameters by io_preprocessing, remove those from the parameter list
577579
end
580+
if disturbance_inputs !== nothing
581+
ps = setdiff(ps, disturbance_inputs)
582+
end
578583
_ps = ps
579584
if ps isa Tuple
580585
ps = DestructuredArgs.(unwrap.(ps), inbounds = !checkbounds)
581586
elseif has_index_cache(sys) && get_index_cache(sys) !== nothing
582587
ps = DestructuredArgs.(reorder_parameters(get_index_cache(sys), unwrap.(ps)))
583-
if isempty(ps) && inputs !== nothing
588+
if isempty(ps) && (inputs !== nothing || disturbance_inputs !== nothing)
584589
ps = (:EMPTY,)
585590
end
586591
else
@@ -601,13 +606,20 @@ function build_explicit_observed_function(sys, ts;
601606
args = param_only ? [ipts, ps..., ivs...] : [dvs..., ipts, ps..., ivs...]
602607
p_start += 1
603608
end
609+
if disturbance_argument
610+
disturbance_inputs = unwrap.(disturbance_inputs)
611+
dist_inputs = DestructuredArgs(disturbance_inputs, inbounds = !checkbounds)
612+
args = [args; dist_inputs]
613+
end
604614
pre = get_postprocess_fbody(sys)
605615

606616
array_wrapper = if param_only
607-
wrap_array_vars(sys, ts; ps = _ps, dvs = nothing, inputs, history = is_dde(sys)) .∘
617+
wrap_array_vars(sys, ts; ps = _ps, dvs = nothing, inputs,
618+
history = is_dde(sys), extra_args = (disturbance_inputs,)) .∘
608619
wrap_parameter_dependencies(sys, isscalar)
609620
else
610-
wrap_array_vars(sys, ts; ps = _ps, inputs, history = is_dde(sys)) .∘
621+
wrap_array_vars(sys, ts; ps = _ps, inputs, history = is_dde(sys),
622+
extra_args = (disturbance_inputs,)) .∘
611623
wrap_parameter_dependencies(sys, isscalar)
612624
end
613625
mtkparams_wrapper = wrap_mtkparameters(sys, isscalar, p_start)

‎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
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
@@ -112,6 +112,7 @@ end
112112
@safetestset "Linearization Dummy Derivative Tests" include("downstream/linearization_dd.jl")
113113
@safetestset "Inverse Models Test" include("downstream/inversemodel.jl")
114114
@safetestset "Analysis Points Test" include("downstream/analysis_points.jl")
115+
@safetestset "Analysis Points Test" include("downstream/test_disturbance_model.jl")
115116
end
116117

117118
if GROUP == "All" || GROUP == "Extensions"

0 commit comments

Comments
 (0)