From ceb9cdc7493f8c622fcfb9821437de2d74ef5e18 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Fri, 7 Feb 2025 09:20:28 +0100 Subject: [PATCH] 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 --- src/systems/analysis_points.jl | 33 ++++ src/systems/diffeqs/odesystem.jl | 13 +- test/downstream/test_disturbance_model.jl | 215 ++++++++++++++++++++++ test/runtests.jl | 1 + 4 files changed, 261 insertions(+), 1 deletion(-) create mode 100644 test/downstream/test_disturbance_model.jl diff --git a/src/systems/analysis_points.jl b/src/systems/analysis_points.jl index 022a0909ed..135bf81729 100644 --- a/src/systems/analysis_points.jl +++ b/src/systems/analysis_points.jl @@ -960,3 +960,36 @@ Compute the (linearized) loop-transfer function in analysis point `ap`, from `ap See also [`get_sensitivity`](@ref), [`get_comp_sensitivity`](@ref), [`open_loop`](@ref). """ get_looptransfer +# + +""" + 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) + +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. +""" +function generate_control_function( + sys::ModelingToolkit.AbstractODESystem, input_ap_name::Union{ + Symbol, Vector{Symbol}, AnalysisPoint, Vector{AnalysisPoint}}, + dist_ap_name::Union{ + Nothing, Symbol, Vector{Symbol}, AnalysisPoint, Vector{AnalysisPoint}} = nothing; + system_modifier = identity, + kwargs...) + input_ap_name = canonicalize_ap(sys, input_ap_name) + u = [] + for input_ap in input_ap_name + sys, (du, _) = open_loop(sys, input_ap) + push!(u, du) + end + if dist_ap_name === nothing + return ModelingToolkit.generate_control_function(system_modifier(sys), u; kwargs...) + end + + dist_ap_name = canonicalize_ap(sys, dist_ap_name) + d = [] + for dist_ap in dist_ap_name + sys, (du, _) = open_loop(sys, dist_ap) + push!(d, du) + end + + ModelingToolkit.generate_control_function(system_modifier(sys), u, d; kwargs...) +end diff --git a/src/systems/diffeqs/odesystem.jl b/src/systems/diffeqs/odesystem.jl index 1590fd6ebd..20d1e495dc 100644 --- a/src/systems/diffeqs/odesystem.jl +++ b/src/systems/diffeqs/odesystem.jl @@ -436,6 +436,8 @@ an array of inputs `inputs` is given, and `param_only` is false for a time-depen """ function build_explicit_observed_function(sys, ts; inputs = nothing, + disturbance_inputs = nothing, + disturbance_argument = false, expression = false, eval_expression = false, eval_module = @__MODULE__, @@ -512,13 +514,22 @@ function build_explicit_observed_function(sys, ts; ps = setdiff(ps, inputs) # Inputs have been converted to parameters by io_preprocessing, remove those from the parameter list inputs = (inputs,) end + if disturbance_inputs !== nothing + # Disturbance inputs may or may not be included as inputs, depending on disturbance_argument + ps = setdiff(ps, disturbance_inputs) + end + if disturbance_argument + disturbance_inputs = (disturbance_inputs,) + else + disturbance_inputs = () + end ps = reorder_parameters(sys, ps) iv = if is_time_dependent(sys) (get_iv(sys),) else () end - args = (dvs..., inputs..., ps..., iv...) + args = (dvs..., inputs..., ps..., iv..., disturbance_inputs...) p_start = length(dvs) + length(inputs) + 1 p_end = length(dvs) + length(inputs) + length(ps) fns = build_function_wrapper( diff --git a/test/downstream/test_disturbance_model.jl b/test/downstream/test_disturbance_model.jl new file mode 100644 index 0000000000..10fcf9fc1f --- /dev/null +++ b/test/downstream/test_disturbance_model.jl @@ -0,0 +1,215 @@ +#= +This file implements and tests a typical workflow for state estimation with disturbance models +The primary subject of the tests is the analysis-point features and the +analysis-point specific method for `generate_control_function`. +=# +using ModelingToolkit, OrdinaryDiffEq, LinearAlgebra, Test +using ModelingToolkitStandardLibrary.Mechanical.Rotational +using ModelingToolkitStandardLibrary.Blocks +using ModelingToolkit: connect +# using Plots + +using ModelingToolkit: t_nounits as t, D_nounits as D + +indexof(sym, syms) = findfirst(isequal(sym), syms) + +## Build the system model ====================================================== +@mtkmodel SystemModel begin + @parameters begin + m1 = 1 + m2 = 1 + k = 10 # Spring stiffness + c = 3 # Damping coefficient + end + @components begin + inertia1 = Inertia(; J = m1, phi = 0, w = 0) + inertia2 = Inertia(; J = m2, phi = 0, w = 0) + spring = Spring(; c = k) + damper = Damper(; d = c) + torque = Torque(use_support = false) + end + @equations begin + connect(torque.flange, inertia1.flange_a) + connect(inertia1.flange_b, spring.flange_a, damper.flange_a) + connect(inertia2.flange_a, spring.flange_b, damper.flange_b) + end +end + +@mtkmodel ModelWithInputs begin + @components begin + input_signal = Blocks.Sine(frequency = 1, amplitude = 1) + disturbance_signal1 = Blocks.Constant(k = 0) + disturbance_signal2 = Blocks.Constant(k = 0) + disturbance_torque1 = Torque(use_support = false) + disturbance_torque2 = Torque(use_support = false) + system_model = SystemModel() + end + @equations begin + connect(input_signal.output, :u, system_model.torque.tau) + connect(disturbance_signal1.output, :d1, disturbance_torque1.tau) + connect(disturbance_signal2.output, :d2, disturbance_torque2.tau) + connect(disturbance_torque1.flange, system_model.inertia1.flange_b) + connect(disturbance_torque2.flange, system_model.inertia2.flange_b) + end +end + +@named model = ModelWithInputs() # Model with load disturbance +ssys = structural_simplify(model) +prob = ODEProblem(ssys, [], (0.0, 10.0)) +sol = solve(prob, Tsit5()) +# plot(sol) + +## +using ControlSystemsBase, ControlSystemsMTK +cmodel = complete(model) +P = cmodel.system_model +lsys = named_ss( + model, [:u, :d1], [P.inertia1.phi, P.inertia2.phi, P.inertia1.w, P.inertia2.w]) + +## +# 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. + +s = tf("s") +dist(; name) = ODESystem(1 / s; name) + +@mtkmodel SystemModelWithDisturbanceModel begin + @components begin + input_signal = Blocks.Sine(frequency = 1, amplitude = 1) + disturbance_signal1 = Blocks.Constant(k = 0) + disturbance_signal2 = Blocks.Constant(k = 0) + disturbance_torque1 = Torque(use_support = false) + disturbance_torque2 = Torque(use_support = false) + disturbance_model = dist() + system_model = SystemModel() + end + @equations begin + connect(input_signal.output, :u, system_model.torque.tau) + connect(disturbance_signal1.output, :d1, disturbance_model.input) + connect(disturbance_model.output, disturbance_torque1.tau) + connect(disturbance_signal2.output, :d2, disturbance_torque2.tau) + connect(disturbance_torque1.flange, system_model.inertia1.flange_b) + connect(disturbance_torque2.flange, system_model.inertia2.flange_b) + end +end + +@named model_with_disturbance = SystemModelWithDisturbanceModel() +# 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 +# lsys2 = named_ss(model_with_disturbance, [:u, :d1], +# [P.inertia1.phi, P.inertia2.phi, P.inertia1.w, P.inertia2.w]) +ssys = structural_simplify(model_with_disturbance) +prob = ODEProblem(ssys, [], (0.0, 10.0)) +sol = solve(prob, Tsit5()) +@test SciMLBase.successful_retcode(sol) +# plot(sol) + +## +# 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. + +dist3(; name) = ODESystem(ss(1 + 10 / s, balance = false); name) + +@mtkmodel SystemModelWithDisturbanceModel begin + @components begin + input_signal = Blocks.Sine(frequency = 1, amplitude = 1) + disturbance_signal1 = Blocks.Constant(k = 0) + disturbance_signal2 = Blocks.Constant(k = 0) + disturbance_torque1 = Torque(use_support = false) + disturbance_torque2 = Torque(use_support = false) + disturbance_model = dist3() + system_model = SystemModel() + + y = Blocks.Add() + angle_sensor = AngleSensor() + output_disturbance = Blocks.Constant(k = 0) + end + @equations begin + connect(input_signal.output, :u, system_model.torque.tau) + connect(disturbance_signal1.output, :d1, disturbance_model.input) + connect(disturbance_model.output, disturbance_torque1.tau) + connect(disturbance_signal2.output, :d2, disturbance_torque2.tau) + connect(disturbance_torque1.flange, system_model.inertia1.flange_b) + connect(disturbance_torque2.flange, system_model.inertia2.flange_b) + + connect(system_model.inertia1.flange_b, angle_sensor.flange) + connect(angle_sensor.phi, y.input1) + connect(output_disturbance.output, :dy, y.input2) + end +end + +@named model_with_disturbance = SystemModelWithDisturbanceModel() +# 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 +# lsys3 = named_ss(model_with_disturbance, [:u, :d1], +# [P.inertia1.phi, P.inertia2.phi, P.inertia1.w, P.inertia2.w]) +ssys = structural_simplify(model_with_disturbance) +prob = ODEProblem(ssys, [], (0.0, 10.0)) +sol = solve(prob, Tsit5()) +@test SciMLBase.successful_retcode(sol) +# plot(sol) + +## Generate function for an augmented Unscented Kalman Filter ===================== +# temp = open_loop(model_with_disturbance, :d) +outputs = [P.inertia1.phi, P.inertia2.phi, P.inertia1.w, P.inertia2.w] +(f_oop1, f_ip), x_sym, p_sym, io_sys = ModelingToolkit.generate_control_function( + model_with_disturbance, [:u], [:d1, :d2, :dy], split = false) + +(f_oop2, f_ip2), x_sym, p_sym, io_sys = ModelingToolkit.generate_control_function( + model_with_disturbance, [:u], [:d1, :d2, :dy], + disturbance_argument = true, split = false) + +measurement = ModelingToolkit.build_explicit_observed_function( + io_sys, outputs, inputs = ModelingToolkit.inputs(io_sys)[1:1]) +measurement2 = ModelingToolkit.build_explicit_observed_function( + io_sys, [io_sys.y.output.u], inputs = ModelingToolkit.inputs(io_sys)[1:1], + disturbance_inputs = ModelingToolkit.inputs(io_sys)[2:end], + disturbance_argument = true) + +op = ModelingToolkit.inputs(io_sys) .=> 0 +x0, p = ModelingToolkit.get_u0_p(io_sys, op, op) +x = zeros(5) +u = zeros(1) +d = zeros(3) +@test f_oop2(x, u, p, t, d) == zeros(5) +@test measurement(x, u, p, 0.0) == [0, 0, 0, 0] +@test measurement2(x, u, p, 0.0, d) == [0] + +# Add to the integrating disturbance input +d = [1, 0, 0] +@test sort(f_oop2(x, u, p, 0.0, d)) == [0, 0, 0, 1, 1] # Affects disturbance state and one velocity +@test measurement2(x, u, p, 0.0, d) == [0] + +d = [0, 1, 0] +@test sort(f_oop2(x, u, p, 0.0, d)) == [0, 0, 0, 0, 1] # Affects one velocity +@test measurement(x, u, p, 0.0) == [0, 0, 0, 0] +@test measurement2(x, u, p, 0.0, d) == [0] + +d = [0, 0, 1] +@test sort(f_oop2(x, u, p, 0.0, d)) == [0, 0, 0, 0, 0] # Affects nothing +@test measurement(x, u, p, 0.0) == [0, 0, 0, 0] +@test measurement2(x, u, p, 0.0, d) == [1] # We have now disturbed the output + +## Further downstream tests that the functions generated above actually have the properties required to use for state estimation +# +# using LowLevelParticleFilters, SeeToDee +# Ts = 0.001 +# discrete_dynamics = SeeToDee.Rk4(f_oop2, Ts) +# nx = length(x_sym) +# nu = 1 +# nw = 2 +# ny = length(outputs) +# R1 = Diagonal([1e-5, 1e-5]) +# R2 = 0.1 * I(ny) +# op = ModelingToolkit.inputs(io_sys) .=> 0 +# x0, p = ModelingToolkit.get_u0_p(io_sys, op, op) +# d0 = LowLevelParticleFilters.SimpleMvNormal(x0, 10.0I(nx)) +# measurement_model = UKFMeasurementModel{Float64, false, false}(measurement, R2; nx, ny) +# kf = UnscentedKalmanFilter{false, false, true, false}( +# discrete_dynamics, measurement_model, R1, d0; nu, Ts, p) + +# tvec = 0:Ts:sol.t[end] +# u = vcat.(Array(sol(tvec, idxs = P.torque.tau.u))) +# y = collect.(eachcol(Array(sol(tvec, idxs = outputs)) .+ 1e-2 .* randn.())) + +# inds = 1:5805 +# res = forward_trajectory(kf, u, y) + +# plot(res, size = (1000, 1000)); +# plot!(sol, idxs = x_sym, sp = (1:nx)', l = :dash); diff --git a/test/runtests.jl b/test/runtests.jl index 966b02cacb..11c78e43ca 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -120,6 +120,7 @@ end @safetestset "Linearization Dummy Derivative Tests" include("downstream/linearization_dd.jl") @safetestset "Inverse Models Test" include("downstream/inversemodel.jl") @safetestset "Analysis Points Test" include("downstream/analysis_points.jl") + @safetestset "Analysis Points Test" include("downstream/test_disturbance_model.jl") end if GROUP == "All" || GROUP == "FMI"