Merge pull request #1853 from SciML/fb/disturbance_models

add disturbance models

Author: YingboMa

src/inputoutput.jl

@@ -160,15 +160,24 @@ has_var(ex, x) = x ∈ Set(get_variables(ex))
 # Build control function
 
 """
-    (f_oop, f_ip), dvs, p = generate_control_function(sys::AbstractODESystem, inputs = unbound_inputs(sys); implicit_dae = false, ddvs = if implicit_dae
+    (f_oop, f_ip), dvs, p = generate_control_function(
+            sys::AbstractODESystem,
+            inputs             = unbound_inputs(sys),
+            disturbance_inputs = nothing;
+            implicit_dae       = false,
+            simplify           = false,
+        )
 
 For a system `sys` with inputs (as determined by [`unbound_inputs`](@ref) or user specified), generate a function with additional input argument `in`
 ```
-f_oop : (u,in,p,t)      -> rhs
-f_ip  : (uout,u,in,p,t) -> nothing
+f_oop : (x,u,p,t)      -> rhs
+f_ip  : (xout,x,u,p,t) -> nothing
 ```
 The return values also include the remaining states and parameters, in the order they appear as arguments to `f`.
 
+If `disturbance_inputs` is an array of variables, the generated dynamics function will preserve any state and dynamics associated with distrubance inputs, but the distrubance inputs themselves will not be included as inputs to the generated function. The use case for this is to generate dynamics for state observers that estimate the influence of unmeasured disturbances, and thus require state variables for the disturbance model, but without disturbance inputs since the disturbances are not available for measurement.
+See [`add_input_disturbance`](@ref) for a higher-level interface to this functionality.
+
 # Example
 ```
 using ModelingToolkit: generate_control_function, varmap_to_vars, defaults
@@ -179,22 +188,38 @@ t = 0
 f[1](x, inputs, p, t)
 ```
 """
-function generate_control_function(sys::AbstractODESystem, inputs = unbound_inputs(sys);
+function generate_control_function(sys::AbstractODESystem, inputs = unbound_inputs(sys),
+                                   disturbance_inputs = disturbances(sys);
                                    implicit_dae = false,
                                    simplify = false,
                                    kwargs...)
     if isempty(inputs)
         error("No unbound inputs were found in system.")
     end
 
+    if disturbance_inputs !== nothing
+        # add to inputs for the purposes of io processing
+        inputs = [inputs; disturbance_inputs]
+    end
+
     sys, _ = io_preprocessing(sys, inputs, []; simplify, kwargs...)
 
     dvs = states(sys)
     ps = parameters(sys)
     ps = setdiff(ps, inputs)
+    if disturbance_inputs !== nothing
+        # remove from inputs since we do not want them as actual inputs to the dynamics
+        inputs = setdiff(inputs, disturbance_inputs)
+        # ps = [ps; disturbance_inputs]
+    end
     inputs = map(x -> time_varying_as_func(value(x), sys), inputs)
 
     eqs = [eq for eq in equations(sys) if !isdifferenceeq(eq)]
+    if disturbance_inputs !== nothing
+        # Set all disturbance *inputs* to zero (we just want to keep the disturbance state)
+        subs = Dict(disturbance_inputs .=> 0)
+        eqs = [eq.lhs ~ substitute(eq.rhs, subs) for eq in eqs]
+    end
     check_operator_variables(eqs, Differential)
     # substitute x(t) by just x
     rhss = implicit_dae ? [_iszero(eq.lhs) ? eq.rhs : eq.rhs - eq.lhs for eq in eqs] :
@@ -294,3 +319,86 @@ function inputs_to_parameters!(state::TransformationState, io)
     base_params = length(ps)
     return state, (base_params + 1):(base_params + length(new_parameters)) # (1:length(new_parameters)) .+ base_params
 end
+
+"""
+    DisturbanceModel{M}
+
+The structure represents a model of a disturbance, along with the input variable that is affected by the disturbance. See [`add_input_disturbance`](@ref) for additional details and an example.
+
+# Fields:
+- `input`: The variable affected by the disturbance.
+- `model::M`: A model of the disturbance. This is typically an `ODESystem`, but type that implements [`ModelingToolkit.get_disturbance_system`](@ref)`(dist::DisturbanceModel) -> ::ODESystem` is supported.
+"""
+struct DisturbanceModel{M}
+    input::Any
+    model::M
+end
+
+# Point of overloading for libraries, e.g., to be able to support disturbance models from ControlSystemsBase
+function get_disturbance_system(dist::DisturbanceModel{<:ODESystem})
+    dist.model
+end
+
+"""
+    (f_oop, f_ip), augmented_sys, dvs, p = add_input_disturbance(sys, dist::DisturbanceModel)
+
+Add a model of an unmeasured disturbance to `sys`. The disturbance model is an instance of [`DisturbanceModel`](@ref).
+
+The generated dynamics functions `(f_oop, f_ip)` will preserve any state and dynamics associated with disturbance inputs, but the disturbance inputs themselves will not be included as inputs to the generated function. The use case for this is to generate dynamics for state observers that estimate the influence of unmeasured disturbances, and thus require state variables for the disturbance model, but without disturbance inputs since the disturbances are not available for measurement.
+
+`dvs` will be the states of the simplified augmented system, consisting of the states of `sys` as well as the states of the disturbance model.
+
+# Example
+The example below builds a double-mass model and adds an integrating disturbance to the input
+```julia
+using ModelingToolkit
+using ModelingToolkitStandardLibrary.Mechanical.Rotational
+using ModelingToolkitStandardLibrary.Blocks
+using ModelingToolkitStandardLibrary.Blocks: t
+
+# Parameters
+m1 = 1
+m2 = 1
+k  = 1000 # Spring stiffness
+c  = 10   # Damping coefficient
+
+@named inertia1 = Inertia(; J = m1)
+@named inertia2 = Inertia(; J = m2)
+@named spring = Spring(; c = k)
+@named damper = Damper(; d = c)
+@named torque = Torque()
+
+eqs = [
+    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)
+]
+if u !== nothing
+    push!(eqs, connect(torque.tau, u.output))
+    return @named model = ODESystem(eqs, t; systems = [torque, inertia1, inertia2, spring, damper, u])
+end
+model = ODESystem(eqs, t; systems = [torque, inertia1, inertia2, spring, damper], name)
+model_outputs = [model.inertia1.w, model.inertia2.w, model.inertia1.phi, model.inertia2.phi]
+
+# Disturbance model
+@named dmodel = Blocks.StateSpace([0.0], [1.0], [1.0], [0.0]) # An integrating disturbance
+dist = ModelingToolkit.DisturbanceModel(model.torque.tau.u, dmodel)
+(f_oop, f_ip), augmented_sys, dvs, p = ModelingToolkit.add_input_disturbance(model, dist)
+```
+`f_oop` will have an extra state corresponding to the integrator in the disturbance model. This state will not be affected by any input, but will affect the dynamics from where it enters, in this case it will affect additively from `model.torque.tau.u`.
+"""
+function add_input_disturbance(sys, dist::DisturbanceModel)
+    t = get_iv(sys)
+    @variables d(t)=0 [disturbance = true]
+    @variables u(t)=0 [input = true]
+    dsys = get_disturbance_system(dist)
+
+    eqs = [dsys.input.u[1] ~ d
+           dist.input ~ u + dsys.output.u[1]]
+
+    augmented_sys = ODESystem(eqs, t, systems = [sys, dsys], name = gensym(:outer))
+
+    (f_oop, f_ip), dvs, p = generate_control_function(augmented_sys, [u],
+                                                      [d])
+    (f_oop, f_ip), augmented_sys, dvs, p
+end

src/variables.jl

@@ -158,6 +158,10 @@ function isdisturbance(x)
     Symbolics.getmetadata(x, VariableDisturbance, false)
 end
 
+function disturbances(sys)
+    [filter(isdisturbance, states(sys)); filter(isdisturbance, parameters(sys))]
+end
+
 ## Tunable =====================================================================
 struct VariableTunable end
 Symbolics.option_to_metadata_type(::Val{:tunable}) = VariableTunable

test/input_output_handling.jl

@@ -140,21 +140,21 @@ function Mass(; name, m = 1.0, p = 0, v = 0)
     ODESystem(eqs, t, [pos, vel, y], ps; name)
 end
 
-function Spring(; name, k = 1e4)
+function MySpring(; name, k = 1e4)
     ps = @parameters k = k
     @variables x(t) = 0 # Spring deflection
     ODESystem(Equation[], t, [x], ps; name)
 end
 
-function Damper(; name, c = 10)
+function MyDamper(; name, c = 10)
     ps = @parameters c = c
     @variables vel(t) = 0
     ODESystem(Equation[], t, [vel], ps; name)
 end
 
 function SpringDamper(; name, k = false, c = false)
-    spring = Spring(; name = :spring, k)
-    damper = Damper(; name = :damper, c)
+    spring = MySpring(; name = :spring, k)
+    damper = MyDamper(; name = :damper, c)
     compose(ODESystem(Equation[], t; name),
             spring, damper)
 end
@@ -195,3 +195,73 @@ i = findfirst(isequal(u[1]), out)
 eqs = [Differential(t)(x) ~ u]
 @named sys = ODESystem(eqs, t)
 @test_nowarn structural_simplify(sys)
+
+#=
+## Disturbance input handling
+We test that the generated disturbance dynamics is correct by calling the dynamics in two different points that differ in the disturbance state, and check that we get the same result as when we call the linearized dynamics in the same two points. The true system is linear so the linearized dynamics are exact.
+
+The test below builds a double-mass model and adds an integrating disturbance to the input
+=#
+
+using ModelingToolkit
+using ModelingToolkitStandardLibrary.Mechanical.Rotational
+using ModelingToolkitStandardLibrary.Blocks
+@parameters t
+
+# Parameters
+m1 = 1
+m2 = 1
+k = 1000 # Spring stiffness
+c = 10   # Damping coefficient
+
+@named inertia1 = Rotational.Inertia(; J = m1)
+@named inertia2 = Rotational.Inertia(; J = m2)
+@named spring = Rotational.Spring(; c = k)
+@named damper = Rotational.Damper(; d = c)
+@named torque = Rotational.Torque()
+
+function SystemModel(u = nothing; name = :model)
+    eqs = [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)]
+    if u !== nothing
+        push!(eqs, connect(torque.tau, u.output))
+        return @named model = ODESystem(eqs, t;
+                                        systems = [
+                                            torque,
+                                            inertia1,
+                                            inertia2,
+                                            spring,
+                                            damper,
+                                            u,
+                                        ])
+    end
+    ODESystem(eqs, t; systems = [torque, inertia1, inertia2, spring, damper], name)
+end
+
+model = SystemModel() # Model with load disturbance
+model_outputs = [model.inertia1.w, model.inertia2.w, model.inertia1.phi, model.inertia2.phi]
+
+@named dmodel = Blocks.StateSpace([0.0], [1.0], [1.0], [0.0]) # An integrating disturbance
+
+dist = ModelingToolkit.DisturbanceModel(model.torque.tau.u, dmodel)
+(f_oop, f_ip), outersys, dvs, p = ModelingToolkit.add_input_disturbance(model, dist)
+
+@unpack u, d = outersys
+matrices, ssys = linearize(outersys, [u, d], model_outputs)
+
+def = ModelingToolkit.defaults(outersys)
+
+# Create a perturbation in the disturbance state
+dstate = setdiff(dvs, model_outputs)[]
+x_add = ModelingToolkit.varmap_to_vars(merge(Dict(dvs .=> 0), Dict(dstate => 1)), dvs)
+
+x0 = randn(5)
+x1 = copy(x0) + x_add # add disturbance state perturbation
+u = randn(1)
+pn = ModelingToolkit.varmap_to_vars(def, p)
+xp0 = f_oop(x0, u, pn, 0)
+xp1 = f_oop(x1, u, pn, 0)
+
+@test xp0 ≈ matrices.A * x0 + matrices.B * [u; 0]
+@test xp1 ≈ matrices.A * x1 + matrices.B * [u; 0]