diff --git a/Project.toml b/Project.toml index b8052cf..d89a0b5 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "SOLPS2ctrl" uuid = "a531d12f-ac8a-43e8-b6d9-bd121431dd49" authors = ["David Eldon "] -version = "2.2.1" +version = "2.2.2" [deps] Contour = "d38c429a-6771-53c6-b99e-75d170b6e991" diff --git a/docs/src/index.md b/docs/src/index.md index 514fe47..17fb0ae 100644 --- a/docs/src/index.md +++ b/docs/src/index.md @@ -30,7 +30,8 @@ using Pkg Pkg.add("SOLPS2ctrl") ``` -## Top file handling functions +## SOLPS related functionality +### Top file handling functions ```@docs find_files_in_allowed_folders @@ -38,7 +39,7 @@ geqdsk_to_imas! preparation ``` -## Repairing/filling out partial equilibrium files +### Repairing/filling out partial equilibrium files Tools for repairing/filling out partial equilibrium files. @@ -51,18 +52,18 @@ add_rho_to_equilibrium! check_rho_1d ``` -## Extrapolations +### Extrapolations Utilities for extrapolating profiles -### Core profile extrapolations +#### Core profile extrapolations ```@docs extrapolate_core fill_in_extrapolated_core_profile! ``` -### Edge profiles extrapolations +#### Edge profiles extrapolations These functions have not been fully tested and/or supported yet. @@ -71,17 +72,38 @@ mesh_psi_spacing cached_mesh_extension! ``` -## System identification and modeling +### Unit conversion utilities + +```@docs +gas_unit_converter +``` + +## Control related functionality + +This package provides a platform to perform closed loop simulations of controllers with +plant models and actuator models. The key function to run these simulations is `run_closed_loop_sim()` described below. The common architecture for this platform is to create `mutable struct` for each of the actuator, plant, and controller which are +subtypes of provided abstract types `Actuator`, `Plant`, and `Controller` and the +struct itself is callable and performs the required function for a discrete time +instance. See test examples to get an idea of how to use this feature. + +### Plant + +```@docs +Plant +LinearPlant +InputConditioning +InpCondLinPlant +``` + +### System identification and modeling ```@docs -offset_scale -unscale_unoffset system_id system_id_optimal_inp_cond model_evolve ``` -## Actuators +### Actuators ```@docs Actuator @@ -98,26 +120,26 @@ for (i, inp) in enumerate(inp_series) end ``` -## Controllers +### Controllers ```@docs Controller ``` -### Linear Controller +#### Linear Controller ```@docs LinearController ``` -### Predicted Variable Linear Controller +#### Predicted Variable Linear Controller ```@docs PVLC state_prediction_matrices ``` -#### State Prediction Matrix Algebra +##### State Prediction Matrix Algebra At step k: @@ -250,22 +272,22 @@ Then, future state can be predicted by: x_{k+1} = \mathcal{N} \mathcal{L}^{-1} (\vec{Y} - \mathcal{M} \vec{U}) + \mathcal{O} \vec{U} ``` -### Model Predictive Controller +#### Model Predictive Controller ```@docs MPC ``` -## Closed loop simulations +### Closed loop simulations ```@docs -lsim_step -model_step run_closed_loop_sim ``` -## Unit conversion utilities +### Control related utilities ```@docs -gas_unit_converter -``` \ No newline at end of file +lsim_step +offset_scale +unscale_unoffset +``` diff --git a/src/SOLPS2ctrl.jl b/src/SOLPS2ctrl.jl index 0c588ea..4aaf6c2 100644 --- a/src/SOLPS2ctrl.jl +++ b/src/SOLPS2ctrl.jl @@ -10,7 +10,10 @@ export find_files_in_allowed_folders, geqdsk_to_imas!, preparation include("$(@__DIR__)/supersize_profile.jl") include("$(@__DIR__)/repair_eq.jl") include("$(@__DIR__)/unit_utils.jl") +include("$(@__DIR__)/control_utils.jl") +include("$(@__DIR__)/plant.jl") include("$(@__DIR__)/system_id.jl") +include("$(@__DIR__)/actuators.jl") include("$(@__DIR__)/controllers.jl") """ diff --git a/src/actuators.jl b/src/actuators.jl new file mode 100644 index 0000000..32ca680 --- /dev/null +++ b/src/actuators.jl @@ -0,0 +1,100 @@ +import DataStructures: Queue, enqueue!, dequeue! + +export Actuator, DelayedActuator + +""" + Actuator + +Abstract parent type for all actuators. Whenever a user defined actuator is created, +it must be subtype of `Actuator`. + +To create a new actuator with custom function, it must be defined as a mutable stucture +which is daughter of Actuator that contains all settings and state information for +the actuator and the instance itself should be callable to take as input a +`Vector{Float64}` and should return an output of `Vector{Float64}`. + +```julia +mutable struct CustomActuator <: Actuator + settings + state + # ... Anything more +end + +function (ca::CustomActuator)(inp::Vector{Float64})::Vector{Float64} + # perform the actuation calcualtions with inp + # update ca.state if required + return output +end +``` + +**NOTE:** If you need to add a delay in the actuator, add a [`DelayedActuator`](@ref) +instance in the attributes of your `CustomActuator` and just call the DelayedActuator +inside your function call. +""" +abstract type Actuator end + +""" + DelayedActuator{U} + +Implementation of delayed actuation. It stores `delay::Int` for number of time steps of +delay and `buffer::Queue{U}` which stores the delayed actuations in a queue. +Constructor: + + DelayedActuator( + delay::Int; + default_output::T=[0.0], + ) where {T} + +Creates a DelayedActuator{T} instance with `delay` and initializes the `buffer` +pre-filled upto brim with the default_output. +""" +mutable struct DelayedActuator{U} <: Actuator + delay::Int + buffer::Queue{U} + function DelayedActuator( + delay::Int; + default_output::T=[0.0], + ) where {T} + buffer = Queue{T}(delay) + for i ∈ 1:delay + enqueue!(buffer, default_output) + end + return new{T}(delay, buffer) + end +end + +function (da::DelayedActuator)(inp::Union{Float64, Vector{Float64}})::Vector{Float64} + if da.delay > 0 + enqueue!(da.buffer, inp) + return dequeue!(da.buffer) + else + return inp + end +end + +function get_future_inp(act::Actuator) + for fieldname ∈ fieldnames(typeof(act)) + if fieldtype(typeof(act), fieldname) == DelayedActuator + return get_future_inp(getfield(act, fieldname)) + end + end + return Matrix{Float64}(undef, 0, 0) +end + +get_future_inp(act::DelayedActuator) = stack(act.buffer) + +function get_min_delay(act::Actuator)::Int + min_delay = typemax(Int) + has_delay = false + for fieldname ∈ fieldnames(typeof(act)) + if fieldtype(typeof(act), fieldname) == DelayedActuator + min_delay = min(min_delay, getfield(act, fieldname).delay) + has_delay = true + end + end + if has_delay + return min_delay + else + return 0 + end +end diff --git a/src/control_utils.jl b/src/control_utils.jl new file mode 100644 index 0000000..7cfd276 --- /dev/null +++ b/src/control_utils.jl @@ -0,0 +1,60 @@ +import ControlSystemIdentification: PredictionStateSpace +import ControlSystemsBase: StateSpace + +export lsim_step, offset_scale, unscale_unoffset + +""" + lsim_step( + sys::Union{PredictionStateSpace, StateSpace}, u::Vector{Float64}; + x0::Vector{Float64}=zeros(size(sys.A, 1)), + )::Tuple{Vector{Float64}, Vector{Float64}} + +Single step version of [ControlSystemsBase.lsim](https://juliacontrol.github.io/ControlSystems.jl/dev/lib/timefreqresponse/#ControlSystemsBase.lsim-Tuple%7BAbstractStateSpace,%20AbstractVecOrMat,%20AbstractVector%7D) +""" +function lsim_step( + sys::Union{PredictionStateSpace, StateSpace}, u::Vector{Float64}; + x0::Vector{Float64}=zeros(size(sys.A, 1)), +)::Tuple{Vector{Float64}, Vector{Float64}} + x = sys.A * x0 + sys.B * u + y = sys.C * x0 + sys.D * u + return y, x +end + +""" + offset_scale( + val::Union{Float64, Vector{Float64}, Matrix{Float64}}; + offset::Union{Float64, Vector{Float64}}=0.0, + factor::Union{Float64, Vector{Float64}}=1.0, + )::typeof(val) + +Subtract an `offset` and multiply by a `factor`, the `val` to make it nominally in the +range of -1 to 1 (not strictly) for easy identification of system. + + (val .- offset) .* factor +""" +function offset_scale( + val::Union{Float64, Vector{Float64}, Matrix{Float64}}; + offset::Union{Float64, Vector{Float64}}=0.0, + factor::Union{Float64, Vector{Float64}}=1.0, +)::typeof(val) + return (val .- offset) .* factor +end + +""" + unscale_unoffset( + offset_scaled::Union{Float64, Vector{Float64}, Matrix{Float64}}; + offset::Union{Float64, Vector{Float64}}=0.0, + factor::Union{Float64, Vector{Float64}}=1.0, + )::typeof(offset_scaled) + +Undo previously applied offset and scaling. + + offset_scaled ./ factor .+ offset +""" +function unscale_unoffset( + offset_scaled::Union{Float64, Vector{Float64}, Matrix{Float64}}; + offset::Union{Float64, Vector{Float64}}=0.0, + factor::Union{Float64, Vector{Float64}}=1.0, +)::typeof(offset_scaled) + return offset_scaled ./ factor .+ offset +end diff --git a/src/controllers.jl b/src/controllers.jl index 72a57bf..879465e 100644 --- a/src/controllers.jl +++ b/src/controllers.jl @@ -1,58 +1,66 @@ import ControlSystemIdentification: PredictionStateSpace import ControlSystemsBase: StateSpace, Discrete import LinearAlgebra: pinv -import DataStructures: Queue, enqueue!, dequeue!, empty!, first +import DataStructures: Queue, enqueue!, dequeue!, empty! import LsqFit: curve_fit, coef -export lsim_step, model_step, state_prediction_matrices, Actuator, DelayedActuator, - Controller, LinearController, PVLC, MPC, run_closed_loop_sim +export Controller, + LinearController, PVLC, state_prediction_matrices, MPC, run_closed_loop_sim """ - lsim_step( - sys::Union{PredictionStateSpace, StateSpace}, u::Vector{Float64}; - x0::Vector{Float64}=zeros(size(sys.A, 1)), - )::Tuple{Vector{Float64}, Vector{Float64}} + Controller + +Abstract parent type for all controllers. Whenever a user defined controller is created +it must be a subtype of `Controller`. + +To create a new controller algorithm, it should be defined as a mutable structure that +is a daughter of `Controller` and should contain all settings and state information to +be stored. It must be a callable structure that can use any of the following keyword +arguments: -Single step version of [ControlSystemsBase.lsim](https://juliacontrol.github.io/ControlSystems.jl/dev/lib/timefreqresponse/#ControlSystemsBase.lsim-Tuple%7BAbstractStateSpace,%20AbstractVecOrMat,%20AbstractVector%7D) + - `ii::Int`: Iteration index + - `target::Matrix{Float64}`: Target waveform (No. of signals x Time steps) + - `plant_inp::Matrix{Float64}`: Inputs to plant (No. of inputs x Time steps) + - `plant_out::Matrix{Float64}`: Outputs from plant (No. of outputs x Time steps) + - `act::Actuator`: Actuator model in the loop + - `inp_feedforward::Matrix{Float64}`: Feedforward input to the plant (No. of inputs x Time steps) + - `kwargs..`: Required to ignore unused keyword arguments """ -function lsim_step( - sys::Union{PredictionStateSpace, StateSpace}, u::Vector{Float64}; - x0::Vector{Float64}=zeros(size(sys.A, 1)), -)::Tuple{Vector{Float64}, Vector{Float64}} - x = sys.A * x0 + sys.B * u - y = sys.C * x0 + sys.D * u - return y, x -end +abstract type Controller end """ - model_step( - sys::Union{PredictionStateSpace, StateSpace}, - inp::Vector{Float64}; - x0::Vector{Float64}=zeros(size(sys.A, 1)), - inp_cond::Union{Nothing, Function}=nothing, - inp_offset::Float64=0.0, inp_factor::Float64=1.0, - out_offset::Float64=0.0, out_factor::Float64=1.0, - inp_cond_kwargs::Dict{Symbol, T}=Dict{Symbol, Any}(), - )::Tuple{Vector{Float64}, Vector{Float64}} where {T} - -Single step version of [`model_evolve()`](@ref). + LinearController + +Implementation for using any linear controller. It stores +`ctrl_ss::StateSpace{TE} where {TE <: Discrete}` for storing any linear controller as a +discrete state space model using [ControlSystemsBase.ss](https://juliacontrol.github.io/ControlSystems.jl/dev/man/creating_systems/#State-Space-Systems). +It also stores the state vector for the state space model as `ctrl_x0::Vector{Float64}`. +It's call signature is: + + (lc::LinearController)(; + ii::Int, + target::Matrix{Float64}, + plant_out::Matrix{Float64}, + kwargs..., + )::Vector{Float64} + +Calcualtes error as `target[:, ii] .- plant_out[:, ii]` and runs it through +[`lsim_step()`](@ref). """ -function model_step( - sys::Union{PredictionStateSpace, StateSpace}, - inp::Vector{Float64}; - x0::Vector{Float64}=zeros(size(sys.A, 1)), - inp_cond::Union{Nothing, Function}=nothing, - inp_offset::Float64=0.0, inp_factor::Float64=1.0, - out_offset::Float64=0.0, out_factor::Float64=1.0, - inp_cond_kwargs::Dict{Symbol, T}=Dict{Symbol, Any}(), -)::Tuple{Vector{Float64}, Vector{Float64}} where {T} - inp_so = offset_scale(inp; offset=inp_offset, factor=inp_factor) - if !isnothing(inp_cond) - inp_so = inp_cond(inp_so; inp_cond_kwargs...) - end - modeled_out_so, x0 = lsim_step(sys, inp_so; x0) - modeled_out = unscale_unoffset(modeled_out_so; offset=out_offset, factor=out_factor) - return modeled_out, x0 +mutable struct LinearController <: Controller + ctrl_ss::StateSpace{TE} where {TE <: Discrete} + ctrl_x0::Vector{Float64} +end + +function (lc::LinearController)(; + ii::Int, + target::Matrix{Float64}, + plant_out::Matrix{Float64}, + kwargs..., +)::Vector{Float64} + err = target[:, ii] .- plant_out[:, ii] + ctrl_out, lc.ctrl_x0 = lsim_step(lc.ctrl_ss, err; x0=lc.ctrl_x0) + return ctrl_out end function _calculate_LMNO( @@ -124,163 +132,17 @@ function state_prediction_matrices( return Y2x, U2x end -""" - Actuator - -Abstract parent type for all actuators. Whenever a user defined actuator is created, -it must be subtype of `Actuator`. - -To create a new actuator with custom function, it must be defined as a mutable stucture -which is daughter of Actuator that contains all settings and state information for -the actuator and the instance itself should be callable to take as input a -`Vector{Float64}` and should return an output of `Vector{Float64}`. - -```julia -mutable struct CustomActuator <: Actuator - settings - state - # ... Anything more -end - -function (ca::CustomActuator)(inp::Vector{Float64})::Vector{Float64} - # perform the actuation calcualtions with inp - # update ca.state if required - return output -end -``` - -**NOTE:** If you need to add a delay in the actuator, add a [`DelayedActuator`](@ref) -instance in the attributes of your `CustomActuator` and just call the DelayedActuator -inside your function call. -""" -abstract type Actuator end - -""" - DelayedActuator{U} - -Implementation of delayed actuation. It stores `delay::Int` for number of time steps of -delay and `buffer::Queue{U}` which stores the delayed actuations in a queue. -Constructor: - - DelayedActuator( - delay::Int; - default_output::T=[0.0], - ) where {T} - -Creates a DelayedActuator{T} instance with `delay` and initializes the `buffer` -pre-filled upto brim with the default_output. -""" -mutable struct DelayedActuator{U} <: Actuator - delay::Int - buffer::Queue{U} - function DelayedActuator( - delay::Int; - default_output::T=[0.0], - ) where {T} - buffer = Queue{T}(delay) - for i ∈ 1:delay - enqueue!(buffer, default_output) - end - return new{T}(delay, buffer) - end -end - -function (da::DelayedActuator)(inp::Union{Float64, Vector{Float64}})::Vector{Float64} - enqueue!(da.buffer, inp) - return dequeue!(da.buffer) -end - -function get_future_inp(act::Actuator) - for fieldname ∈ fieldnames(typeof(act)) - if fieldtype(typeof(act), fieldname) == DelayedActuator - return get_future_inp(getfield(act, fieldname)) - end - end - return Matrix{Float64}(undef, 0, 0) -end - -get_future_inp(act::DelayedActuator) = stack(act.buffer) - -function get_min_delay(act::Actuator)::Int - min_delay = typemax(Int) - has_delay = false - for fieldname ∈ fieldnames(typeof(act)) - if fieldtype(typeof(act), fieldname) == DelayedActuator - min_delay = min(min_delay, getfield(act, fieldname).delay) - has_delay = true - end - end - if has_delay - return min_delay - else - return 0 - end -end - -""" - Controller - -Abstract parent type for all controllers. Whenever a user defined controller is created -it must be a subtype of `Controller`. - -To create a new controller algorithm, it should be defined as a mutable structure that -is a daughter of `Controller` and should contain all settings and state information to -be stored. It must be a callable structure that can use any of the following keyword -arguments: - - - `ii::Int`: Iteration index - - `target::Matrix{Float64}`: Target waveform (No. of signals x Time steps) - - `plant_inp::Matrix{Float64}`: Inputs to plant (No. of inputs x Time steps) - - `plant_out::Matrix{Float64}`: Outputs from plant (No. of outputs x Time steps) - - `act::Actuator`: Actuator model in the loop - - `inp_cond::Union{Nothing, Function}=nothing`: Input conditioning on plant model - - `inp_cond_kwargs::Dict{Symbol, Any}`: Keyword arguments for `inp_cond` - - `inp_offset::Float64`: Input offset for plant input - - `inp_factor::Float64`: Input factor for plant input - - `out_offset::Float64`: Output offset for plant output - - `out_factor::Float64`: Output factor for plant output - - `kwargs..`: Required to ignore unused keyword arguments -""" -abstract type Controller end - -""" - LinearController - -Implementation for using any linear controller. It stores -`ctrl_ss::StateSpace{TE} where {TE <: Discrete}` for storing any linear controller as a -discrete state space model using [ControlSystemsBase.ss](https://juliacontrol.github.io/ControlSystems.jl/dev/man/creating_systems/#State-Space-Systems). -It also stores the state vector for the state space model as `ctrl_x0::Vector{Float64}`. -It's call signature is: - - (lc::LinearController)(; - ii::Int, - target::Matrix{Float64}, - plant_out::Matrix{Float64}, - kwargs..., - )::Vector{Float64} - -Calcualtes error as `target[:, ii] .- plant_out[:, ii]` and runs it through -[`lsim_step()`](@ref). -""" -mutable struct LinearController <: Controller - ctrl_ss::StateSpace{TE} where {TE <: Discrete} - ctrl_x0::Vector{Float64} -end - -function (lc::LinearController)(; - ii::Int, - target::Matrix{Float64}, - plant_out::Matrix{Float64}, - kwargs..., -)::Vector{Float64} - err = target[:, ii] .- plant_out[:, ii] - ctrl_out, lc.ctrl_x0 = lsim_step(lc.ctrl_ss, err; x0=lc.ctrl_x0) - return ctrl_out -end - """ PVLC + # Constructor + PVLC( + ctrl_ss::StateSpace{TE}, + ctrl_x0::Vector{Float64}, + plant::LinearPlant, + h::Int, + ) where {TE <: Discrete} + Implementation of Predicted Variable Linear Controller (PVLC). It stores `ctrl_ss::StateSpace{TE} where {TE <: Discrete}` for storing any linear controller as a discrete state space model using [ControlSystemsBase.ss](https://juliacontrol.github.io/ControlSystems.jl/dev/man/creating_systems/#State-Space-Systems). @@ -288,14 +150,6 @@ It also stores the state vector for the state space model as `ctrl_x0::Vector{Fl Additionally, it stores the `plant`, the number of steps of history `h` used for state tracking and state prediction matrices `Y2x` and `U2x` from [`state_prediction_matrices()`](@ref). -There is a convinience contructor: - - PVLC( - ctrl_ss::StateSpace{TE}, - ctrl_x0::Vector{Float64}, - plant::Union{PredictionStateSpace, StateSpace}, - h::Int, - ) where {TE <: Discrete} This controller has a call signature: @@ -305,8 +159,6 @@ This controller has a call signature: plant_inp::Matrix{Float64}, plant_out::Matrix{Float64}, act::Actuator, - inp_offset::Float64=0.0, inp_factor::Float64=1.0, - out_offset::Float64=0.0, out_factor::Float64=1.0, kwargs..., )::Vector{Float64} @@ -317,17 +169,19 @@ future `target` value and applies the linear controller `ctrl_ss` there. mutable struct PVLC <: Controller ctrl_ss::StateSpace{TE} where {TE <: Discrete} ctrl_x0::Vector{Float64} - plant::Union{PredictionStateSpace, StateSpace} + plant::LinearPlant h::Int Y2x::Matrix{Float64} U2x::Matrix{Float64} function PVLC( ctrl_ss::StateSpace{TE}, ctrl_x0::Vector{Float64}, - plant::Union{PredictionStateSpace, StateSpace}, + plant::LinearPlant, h::Int, ) where {TE <: Discrete} - Y2x, U2x = state_prediction_matrices(plant, h) + # Take the plant by value + plant = deepcopy(plant) + Y2x, U2x = state_prediction_matrices(plant.sys, h) return new(ctrl_ss, ctrl_x0, plant, h, Y2x, U2x) end end @@ -338,8 +192,6 @@ function (pvlc::PVLC)(; plant_inp::Matrix{Float64}, plant_out::Matrix{Float64}, act::Actuator, - inp_offset::Float64=0.0, inp_factor::Float64=1.0, - out_offset::Float64=0.0, out_factor::Float64=1.0, kwargs..., )::Vector{Float64} ctrl_out = zeros(Float64, size(pvlc.ctrl_ss.C, 1)) @@ -347,16 +199,18 @@ function (pvlc::PVLC)(; lat = size(future_inp, 2) if ii - pvlc.h + 1 > 0 && ii + lat <= size(target, 2) # Gather history of inputs and outputs - U = vec(plant_inp[:, (ii-pvlc.h+1):ii]) - Y = vec(plant_out[:, (ii-pvlc.h+1):ii]) + U = offset_scale( + vec(plant_inp[:, (ii-pvlc.h+1):ii]); + offset=pvlc.plant.inp_offset, factor=pvlc.plant.inp_factor, + ) + Y = offset_scale( + vec(plant_out[:, (ii-pvlc.h+1):ii]); + offset=pvlc.plant.out_offset, factor=pvlc.plant.out_factor, + ) # Estimate current state vector of plant model - est_x = pvlc.Y2x * Y .+ pvlc.U2x * U + pvlc.plant.x = pvlc.Y2x * Y .+ pvlc.U2x * U # Evolve into the future for the delay in actuator - future_out = model_evolve( - pvlc.plant, future_inp; - x0=est_x, - inp_offset, inp_factor, out_offset, out_factor, - ) + future_out = model_evolve(pvlc.plant, future_inp) # Estimate the error signal in future err = target[:, ii+lat] - future_out[:, end] # Apply the linear controller in future @@ -389,20 +243,9 @@ end """ MPC -Implementation of simple leaqt square optimized Model Predictive Controller (MPC). It -stores the `plant`, the number of steps of history `h` used for state tracking and -state prediction matrices `Y2x` and `U2x` from [`state_prediction_matrices()`](@ref). -It stores a current deep copy of actuator instance in `act` to try it during -optimization and it stores setting for least square optimization. It also stores a -`future_evolve` that is created based on `plant`, `act`, and optimization setting and -is the model function that is used for optimization later. This controller stores a -buffer for control outputs, `ctrl_out_buffer` so that it can be called less often and -it can reuse it's previous optimization results. - -There is a convinience contructor: - + # Constructor MPC( - plant::Union{PredictionStateSpace, StateSpace}, + plant::Plant, h::Int, act::Actuator, # Actuator model without delay horizon::Int, # Number of steps in future after latency @@ -412,8 +255,20 @@ There is a convinience contructor: Array{Float64}(undef, 0), Array{Float64}(undef, 0), ), + guess::Union{Symbol, Vector{Float64}}=:zeros, + curve_fit_kwargs::Dict{Symbol, T}=Dict{Symbol, Any}(), ) +Implementation of simple leaqt square optimized Model Predictive Controller (MPC). It +stores the `plant`, the number of steps of history `h` used for state tracking and +state prediction matrices `Y2x` and `U2x` from [`state_prediction_matrices()`](@ref). +It stores a current deep copy of actuator instance in `act` to try it during +optimization and it stores setting for least square optimization. It also stores a +`future_evolve` that is created based on `plant`, `act`, and optimization setting and +is the model function that is used for optimization later. This controller stores a +buffer for control outputs, `ctrl_out_buffer` so that it can be called less often and +it can reuse it's previous optimization results. + This contructor takes in minimum required information to create a self-consistent MPC instance. It sets the other dependent quantities in MPC such as `Y2x`, `U2x`, `min_delay`, and create a `future_evolve` function and initializes the `ctrl_out_buffer`. @@ -423,23 +278,24 @@ after the `min_delay` among all acturators for which the optimization is carried distributed. The gaps between this optimzation points are interpolated linearly in the output. `opt_every` defines the frequency of optimization, i.e. at every `opt_every` call of this controller, the optimization is carried out. This avoids unnecessary -over-calculations and thus results in a faster controller. +over-calculations and thus results in a faster controller. `ctrl_out_bounds` is a tuple +of lower bounds and upper bounds for the control output. `guess` is used to create the +initial guess during least square optimization. If `:last`, it would use the last +controller output as initial setting. If it is a `Vector`, each initialization starts +with this Vector. In all other cases, initial guess is zeros. `curve_fit_kwargs` can be +used to provide keyword arguments that go to `curve_fit`. This controller has a call signature: - (mpc::MPC)(; + function (mpc::MPC)(; ii::Int, target::Matrix{Float64}, plant_inp::Matrix{Float64}, plant_out::Matrix{Float64}, act::Actuator, - inp_cond::Union{Nothing, Function}=nothing, - inp_offset::Float64=0.0, inp_factor::Float64=1.0, - out_offset::Float64=0.0, out_factor::Float64=1.0, - inp_cond_kwargs::Dict{Symbol, T}=Dict{Symbol, Any}(), inp_feedforward::Matrix{Float64}=zeros( Float64, - (size(plant.B, 2), length(target)), + (size(get_sys(plant).B, 2), length(target)), ), kwargs..., )::Vector{Float64} where {T} @@ -452,8 +308,8 @@ for the non-linear least squares fitting which uses Levenberg-Marquardt algorith function performs the optimization every `opt_every` call and uses the stored control output in `ctrl_out_buffer` meanwhile. """ -mutable struct MPC <: Controller - plant::Union{PredictionStateSpace, StateSpace} +mutable struct MPC{U} <: Controller + plant::LinearPlant h::Int Y2x::Matrix{Float64} U2x::Matrix{Float64} @@ -466,8 +322,10 @@ mutable struct MPC <: Controller ctrl_out_bounds::Tuple{Vector{Float64}, Vector{Float64}} future_evolve::Function ctrl_out_buffer::Queue{Vector{Float64}} + guess::Union{Symbol, Vector{Float64}} + curve_fit_kwargs::Dict{Symbol, U} function MPC( - plant::Union{PredictionStateSpace, StateSpace}, + plant::LinearPlant, h::Int, act::Actuator, # Actuator model without delay horizon::Int, # Number of steps in future after latency @@ -477,26 +335,26 @@ mutable struct MPC <: Controller Array{Float64}(undef, 0), Array{Float64}(undef, 0), ), - ) - Y2x, U2x = state_prediction_matrices(plant, h) + guess::Union{Symbol, Vector{Float64}}=:zeros, + curve_fit_kwargs::Dict{Symbol, T}=Dict{Symbol, Any}(), + ) where {T} + # Take the plant and actuator by value + plant = deepcopy(plant) + act = deepcopy(act) + Y2x, U2x = state_prediction_matrices(plant.sys, h) min_delay = get_min_delay(act) function fe( red_steps::StepRange{Int, Int}, red_ctrl_out::Vector{Float64}; mpc::MPC, inp_ff::Matrix{Float64}=zeros( Float64, - size(mpc.plant.B, 2), + size(mpc.plant.sys.B, 2), min_delay + horizon + 1, ), - x0::Vector{Float64}=zeros(Float64, size(mpc.plant.A, 2)), - inp_cond::Union{Nothing, Function}=nothing, - inp_offset::Float64=0.0, inp_factor::Float64=1.0, - out_offset::Float64=0.0, out_factor::Float64=1.0, - inp_cond_kwargs::Dict{Symbol, T}=Dict{Symbol, Any}(), - ) where {T} + ) act = deepcopy(mpc.act) - ninps = size(mpc.plant.B, 2) - nouts = size(mpc.plant.C, 1) + ninps = size(mpc.plant.sys.B, 2) + nouts = size(mpc.plant.sys, 1) ctrl_out = expand(red_steps, red_ctrl_out, ninps, mpc.horizon) # Padding far in future extra zero inputs, these won't make any effect # because of the minium delay @@ -505,20 +363,15 @@ mutable struct MPC <: Controller plant_out = zeros(Float64, nouts, mpc.min_delay + mpc.horizon + 1) for i ∈ 1:(mpc.min_delay+mpc.horizon+1) plant_inp = act(ctrl_out[i] .+ inp_ff[:, i]) - plant_out[:, i], x0 = model_step( - mpc.plant, plant_inp; - x0, - inp_cond, inp_offset, inp_factor, out_offset, out_factor, - inp_cond_kwargs, - ) + plant_out[:, i] = mpc.plant(plant_inp) end - return vec(plant_out) + return vec(plant_out[:, (1+mpc.min_delay):end]) end - last_opt = 0 + last_opt = -opt_every ctrl_out_buffer = Queue{Vector{Float64}}(horizon) - return new( + return new{T}( plant, h, Y2x, U2x, act, horizon, nopt, opt_every, last_opt, - min_delay, ctrl_out_bounds, fe, ctrl_out_buffer, + min_delay, ctrl_out_bounds, fe, ctrl_out_buffer, guess, curve_fit_kwargs, ) end end @@ -529,16 +382,12 @@ function (mpc::MPC)(; plant_inp::Matrix{Float64}, plant_out::Matrix{Float64}, act::Actuator, - inp_cond::Union{Nothing, Function}=nothing, - inp_offset::Float64=0.0, inp_factor::Float64=1.0, - out_offset::Float64=0.0, out_factor::Float64=1.0, - inp_cond_kwargs::Dict{Symbol, T}=Dict{Symbol, Any}(), inp_feedforward::Matrix{Float64}=zeros( Float64, - (size(plant.B, 2), length(target)), + (size(get_sys(plant).B, 2), length(target)), ), kwargs..., -)::Vector{Float64} where {T} +)::Vector{Float64} fut_lookup = mpc.min_delay + mpc.horizon + 1 enough_history = ii - mpc.h + 1 > 0 @@ -550,29 +399,39 @@ function (mpc::MPC)(; red_steps = 1:div(mpc.horizon, mpc.nopt-1):(mpc.horizon+1) lower = repeat(mpc.ctrl_out_bounds[1], length(red_steps)) upper = repeat(mpc.ctrl_out_bounds[2], length(red_steps)) - ninps = size(mpc.plant.B, 2) + ninps = size(mpc.plant.sys.B, 2) # Gather history of inputs and outputs - U = vec(plant_inp[:, (ii-mpc.h+1):ii]) - Y = vec(plant_out[:, (ii-mpc.h+1):ii]) + U = offset_scale( + vec(plant_inp[:, (ii-mpc.h+1):ii]); + offset=mpc.plant.inp_offset, factor=mpc.plant.inp_factor, + ) + Y = offset_scale( + vec(plant_out[:, (ii-mpc.h+1):ii]); + offset=mpc.plant.out_offset, factor=mpc.plant.out_factor, + ) # Estimate current state vector of plant model - x0 = mpc.Y2x * Y .+ mpc.U2x * U + mpc.plant.x = mpc.Y2x * Y .+ mpc.U2x * U # Prepare arguments for cost optimization mpc.act = deepcopy(act) - target_vec = vec(target[:, (ii+1):(ii+fut_lookup)]) - guess = repeat(zeros(Float64, ninps), length(red_steps)) + target_vec = vec(target[:, (ii+1+mpc.min_delay):(ii+fut_lookup)]) + + if mpc.guess == :last && !isempty(mpc.ctrl_out_buffer) + guess = repeat(first(mpc.ctrl_out_buffer), length(red_steps)) + elseif isa(mpc.guess, Vector) + guess = repeat(mpc.guess, length(red_steps)) + else + guess = repeat(zeros(Float64, ninps), length(red_steps)) + end inp_ff = inp_feedforward[:, (ii+1):(ii+fut_lookup)] # Optimize fit_res = curve_fit( - (x, y) -> mpc.future_evolve( - x, y; mpc, inp_ff, x0, - inp_cond, inp_offset, inp_factor, out_offset, out_factor, - inp_cond_kwargs, - ), + (x, y) -> mpc.future_evolve(x, y; mpc, inp_ff), red_steps, target_vec, guess; lower, upper, + mpc.curve_fit_kwargs..., ) # Empty buffer and fill with updated control outputs @@ -585,7 +444,7 @@ function (mpc::MPC)(; mpc.last_opt = ii end if isempty(mpc.ctrl_out_buffer) - return zeros(Float64, size(mpc.plant.B, 2)) + return zeros(Float64, size(mpc.plant.sys.B, 2)) else return dequeue!(mpc.ctrl_out_buffer) end @@ -597,26 +456,22 @@ end act::Actuator, ctrl::Controller, target::Matrix{Float64}; - inp_cond::Union{Nothing, Function}=nothing, - inp_offset::Float64=0.0, inp_factor::Float64=1.0, - out_offset::Float64=0.0, out_factor::Float64=1.0, - inp_cond_kwargs::Dict{Symbol, T}=Dict{Symbol, Any}(), inp_feedforward::Matrix{Float64}=zeros( Float64, - (size(plant.B, 2), size(target, 2)), + (size(get_sys(plant).B, 2), size(target, 2)), ), ctrl_start_ind::Int=1, noise_plant_inp::Matrix{Float64}=zeros( Float64, - (size(plant.B, 2), size(target, 2)), + (size(get_sys(plant).B, 2), size(target, 2)), ), noise_plant_out::Matrix{Float64}=zeros( Float64, - (size(plant.C, 1), size(target, 2)), + (size(get_sys(plant).C, 1), size(target, 2)), ), noise_ctrl_out::Matrix{Float64}=zeros( Float64, - (size(plant.B, 2), size(target, 2)), + (size(get_sys(plant).B, 2), size(target, 2)), ), ) where {T, TE <: Discrete} @@ -630,63 +485,53 @@ and `noise_ctrl_out` allow addition of predefined noise waveforms at the input o output of plant, and the output of controller respectively. """ function run_closed_loop_sim( - plant::Union{PredictionStateSpace{TE}, StateSpace{TE}}, + plant::Plant, act::Actuator, ctrl::Controller, target::Matrix{Float64}; - inp_cond::Union{Nothing, Function}=nothing, - inp_offset::Float64=0.0, inp_factor::Float64=1.0, - out_offset::Float64=0.0, out_factor::Float64=1.0, - inp_cond_kwargs::Dict{Symbol, T}=Dict{Symbol, Any}(), inp_feedforward::Matrix{Float64}=zeros( Float64, - (size(plant.B, 2), size(target, 2)), + (size(get_sys(plant).B, 2), size(target, 2)), ), ctrl_start_ind::Int=1, noise_plant_inp::Matrix{Float64}=zeros( Float64, - (size(plant.B, 2), size(target, 2)), + (size(get_sys(plant).B, 2), size(target, 2)), ), noise_plant_out::Matrix{Float64}=zeros( Float64, - (size(plant.C, 1), size(target, 2)), + (size(get_sys(plant).C, 1), size(target, 2)), ), noise_ctrl_out::Matrix{Float64}=zeros( Float64, - (size(plant.B, 2), size(target, 2)), + (size(get_sys(plant).B, 2), size(target, 2)), ), -) where {T, TE <: Discrete} +) + # Take the plant, actuator, and controller by value + plant = deepcopy(plant) + act = deepcopy(act) + ctrl = deepcopy(ctrl) + # Initialization ctrl_out = zeros(Float64, size(inp_feedforward)) - plant_inp = zeros(Float64, (size(plant.B, 2), length(target))) - plant_out = zeros(Float64, (size(plant.C, 1), length(target))) - x0 = - inv(Matrix{Float64}(I, size(plant.A)...) - plant.A) * - plant.B * inp_feedforward[:, 1] + plant_sys = get_sys(plant) + plant_inp = zeros(Float64, (size(plant_sys.B, 2), length(target))) + plant_out = zeros(Float64, (size(plant_sys.C, 1), length(target))) # Closed loop simulation - for ii ∈ eachindex(target) + for ii ∈ axes(target, 2) # Actuation plant_inp[:, ii] = act(ctrl_out[:, ii] .+ inp_feedforward[:, ii]) .+ noise_plant_inp[:, ii] # Plant response - plant_out[:, ii], x0 = model_step( - plant, plant_inp[:, ii]; - x0, - inp_cond, inp_offset, inp_factor, out_offset, out_factor, - inp_cond_kwargs, - ) + plant_out[:, ii] = plant(plant_inp[:, ii]) plant_out[:, ii] .+= noise_plant_out[:, ii] - # future_inp = get_future_inp(act) # Controller if ii >= ctrl_start_ind && ii < length(target) ctrl_out[:, ii+1] = - ctrl(; - ii, target, plant_inp, plant_out, act, inp_feedforward, - inp_cond, inp_cond_kwargs, - inp_offset, inp_factor, out_offset, out_factor, + ctrl(; ii, target, plant_inp, plant_out, act, inp_feedforward ) .+ noise_ctrl_out[:, ii+1] end end @@ -695,12 +540,6 @@ function run_closed_loop_sim( :act => act, :ctrl => ctrl, :target => target, - :inp_cond => inp_cond, - :inp_offset => inp_offset, - :inp_factor => inp_factor, - :out_offset => out_offset, - :out_factor => out_factor, - :inp_cond_kwargs => inp_cond_kwargs, :inp_feedforward => inp_feedforward, :ctrl_start_ind => ctrl_start_ind, :plant_inp => plant_inp, diff --git a/src/plant.jl b/src/plant.jl new file mode 100644 index 0000000..fd88cc3 --- /dev/null +++ b/src/plant.jl @@ -0,0 +1,159 @@ +import ControlSystemIdentification: PredictionStateSpace +import ControlSystemsBase: StateSpace + +export Plant, + LinearPlant, InputConditioning, InpCondLinPlant, get_linear_plant, get_sys, get_x, + set_x! + +""" + Plant + +Abstract parent type for all plants. Whenever a user defined plant is created, +it must be subtype of `Plant`. + +To create a new plant with custom feautres, it must be defined as a mutable stucture +which is daughter of `Plant` that contains all settings and state information for +the plant and the instance itself should be callable to take as input a +`Vector{Float64}` and should return an output of `Vector{Float64}`. + +```julia +mutable struct CustomPlant <: Plant + settings + state + # ... Anything more +end + +function (cp::CustomPlant)(inp::Vector{Float64})::Vector{Float64} + # perform the plant single step forward calcualtion with inp + # update cp.state if required + return output +end +``` + +**NOTE:** If you need to add a linear system in your plant model, add a +[`LinearPlant`](@ref) instance in the attributes of your `CustomPlant` and just call +the LinearPlant inside your function call. +""" +abstract type Plant end + +""" + LinearPlant + +Implementation of linear system with option of scaling and offseting inputs and outputs. +It stores the system in `sys` and state of the system in `x`. +Constructor: + + LinearPlant( + sys::Union{PredictionStateSpace, StateSpace}, + x=zeros(Float64, size(sys.A, 1)); + inp_offset::Float64=0.0, inp_factor::Float64=1.0, + out_offset::Float64=0.0, out_factor::Float64=1.0, + ) + +Creates a LinearPlant instance with state space system `sys` and state vector `x`. It +also defined input offsetting and scaling with `inp_offset` and `inp_factor`, and +similarly output unscaling and unoffseting with `out_offset` and `out_factor`. +""" +mutable struct LinearPlant <: Plant + sys::Union{PredictionStateSpace, StateSpace} + x::Vector{Float64} + inp_offset::Float64 + inp_factor::Float64 + out_offset::Float64 + out_factor::Float64 + function LinearPlant( + sys::Union{PredictionStateSpace, StateSpace}, + x=zeros(Float64, size(sys.A, 1)); + inp_offset::Float64=0.0, inp_factor::Float64=1.0, + out_offset::Float64=0.0, out_factor::Float64=1.0, + ) + return new(sys, x, inp_offset, inp_factor, out_offset, out_factor) + end +end + +function (lp::LinearPlant)(inp::Vector{Float64})::Vector{Float64} + u = offset_scale(inp; offset=lp.inp_offset, factor=lp.inp_factor) + modeled_out_so, lp.x = lsim_step(lp.sys, u; x0=lp.x) + return unscale_unoffset(modeled_out_so; offset=lp.out_offset, factor=lp.out_factor) +end + +""" + InputConditioning + +Abstract parent type for creating input conditioning to plants. Whenever a user +defined input coniditioning is created it must be subtype of `InputConditioning`. + +To create a new input conditioning with custom feautres, it must be defined as a +mutable stucture which is daughter of `InputConditioning` that contains all settings +and state information for the function and the instance itself should be callable to +take as input a `Vector{Float64}` and kwargs for any parameters used and should return +an output of `Vector{Float64}` which is same size as the input. + +```julia +mutable struct CustomInpCond <: InputConditioning + settings + state + # ... Anything more +end + +function (inp_cond::CustomInpCond)(inp::Vector{Float64}; kwargs...)::Vector{Float64} + # perform the input coniditioning single step forward calcualtion with inp + # Use parameters from kwargs + # update inp_cond.state if required + return output +end +``` + +Note that `inp_cond` must have a call signature +of `inp_cond(inp::Vector{Float64}; kwargs...)::Vector{Float64}` to take input as a +vector for multiple inputs at a particular time instance +""" +abstract type InputConditioning end + +""" + InpCondLinPlant + +Implementation of a LinearPlant with an added input conditioning which could be used to +make it non-linear. +It stores the LinearPlant in `linear_plant`, the input coniditioning function in +`inp_cond` and any keyword arguments for the `inp_cond` in `inp_cond_kwargs`. +Constructor: + + InpCondLinPlant{T}( + linear_plant::LinearPlant, + inp_cond::InputConditioning + inp_cond_kwargs::Dict{Symbol, T} + ) where {T} + +Creates a InpCondLinPlant instance. `inp_cond_kwargs` can be used to have changeable +parameters in the input conditioning which can be used for optimization and fiting. +""" +mutable struct InpCondLinPlant{T} <: Plant + linear_plant::LinearPlant + inp_cond::InputConditioning + inp_cond_kwargs::Dict{Symbol, T} +end + +function (nlp::InpCondLinPlant)(inp::Vector{Float64})::Vector{Float64} + conditioned_inp = nlp.inp_cond(inp; nlp.inp_cond_kwargs...) + return nlp.linear_plant(conditioned_inp) +end + +function get_linear_plant(p::Plant) + for fieldname ∈ fieldnames(typeof(p)) + if fieldtype(typeof(p), fieldname) == LinearPlant + return getfield(p, fieldname) + end + end + return error( + "Plant must use an attribute of type LinearPlant to store the linear plant model", + ) +end + +get_linear_plant(p::LinearPlant) = p + +get_sys(p::Plant) = get_linear_plant(p).sys + +get_x(p::Plant) = get_linear_plant(p).x + +set_x!(p::Plant, x::Vector{Float64}) = setfield!(get_linear_plant(p), :x, x) diff --git a/src/system_id.jl b/src/system_id.jl index 67e2d29..d69b9a0 100644 --- a/src/system_id.jl +++ b/src/system_id.jl @@ -3,48 +3,7 @@ import ControlSystemIdentification: iddata, newpem, PredictionStateSpace import LinearAlgebra: I, inv import LsqFit: curve_fit, coef -export offset_scale, - unscale_unoffset, model_evolve, system_id, - system_id_optimal_inp_cond - -""" - offset_scale( - val::Union{Float64, Vector{Float64}, Matrix{Float64}}; - offset::Union{Float64, Vector{Float64}}=0.0, - factor::Union{Float64, Vector{Float64}}=1.0, - )::typeof(val) - -Subtract an `offset` and multiply by a `factor`, the `val` to make it nominally in the -range of -1 to 1 (not strictly) for easy identification of system. - - (val .- offset) .* factor -""" -function offset_scale( - val::Union{Float64, Vector{Float64}, Matrix{Float64}}; - offset::Union{Float64, Vector{Float64}}=0.0, - factor::Union{Float64, Vector{Float64}}=1.0, -)::typeof(val) - return (val .- offset) .* factor -end - -""" - unscale_unoffset( - offset_scaled::Union{Float64, Vector{Float64}, Matrix{Float64}}; - offset::Union{Float64, Vector{Float64}}=0.0, - factor::Union{Float64, Vector{Float64}}=1.0, - )::typeof(offset_scaled) - -Undo previously applied offset and scaling. - - offset_scaled ./ factor .+ offset -""" -function unscale_unoffset( - offset_scaled::Union{Float64, Vector{Float64}, Matrix{Float64}}; - offset::Union{Float64, Vector{Float64}}=0.0, - factor::Union{Float64, Vector{Float64}}=1.0, -)::typeof(offset_scaled) - return offset_scaled ./ factor .+ offset -end +export model_evolve, system_id, system_id_optimal_inp_cond """ system_id( @@ -52,9 +11,9 @@ end out::Union{Vector{Float64}, Matrix{Float64}}, tt::Vector{Float64}, order::Int; - inp_cond::Union{Nothing, Function}=nothing, inp_offset::Float64=0.0, inp_factor::Float64=1.0, out_offset::Float64=0.0, out_factor::Float64=1.0, + inp_cond::Union{Nothing, InputConditioning}=nothing, inp_cond_kwargs::Dict{Symbol, T}=Dict{Symbol, Any}(), newpem_kwargs::Dict{Symbol, U}=Dict{Symbol, Any}(), verbose::Bool=false, @@ -64,7 +23,7 @@ Perform system identification for a set on input data `inp`, output data `out`, time series vector `tt`. If there are more than one inputs or outputs, provide them as Matrix with first dimension for ports (input or output) and second dimension for time. -If `inp_cond` is provided, it is applied to offseted and scaled input before +If `inp_cond` is provided, it is applied before offsetting and scaling for performing system identification with keywords for this function provided in `inp_cond_kwargs`. @@ -80,25 +39,32 @@ function system_id( out::Union{Vector{Float64}, Matrix{Float64}}, tt::Vector{Float64}, order::Int; - inp_cond::Union{Nothing, Function}=nothing, inp_offset::Float64=0.0, inp_factor::Float64=1.0, out_offset::Float64=0.0, out_factor::Float64=1.0, + inp_cond::Union{Nothing, InputConditioning}=nothing, inp_cond_kwargs::Dict{Symbol, T}=Dict{Symbol, Any}(), newpem_kwargs::Dict{Symbol, U}=Dict{Symbol, Any}(), verbose::Bool=false, ) where {T, U} @assert size(inp)[end] == size(out)[end] == length(tt) - inp_so = offset_scale(inp; offset=inp_offset, factor=inp_factor) - if !isnothing(inp_cond) - inp_so = inp_cond(inp_so; inp_cond_kwargs...) + if isa(inp, Vector) + inp_m = Matrix(inp') + else + inp_m = inp end - u = Matrix{Float64}[] - if isa(inp_so, Vector) - u = Matrix(inp_so') + + if !isnothing(inp_cond) + temp_inst = deepcopy(inp_cond) + inp_co = zeros(Float64, size(inp_m)) + for ii ∈ eachindex(tt) + inp_co[:, ii] = temp_inst(inp_m[:, ii]; inp_cond_kwargs...) + end else - u = inp_so + inp_co = inp_m end + u = offset_scale(inp_co; offset=inp_offset, factor=inp_factor) + out_so = offset_scale(out; offset=out_offset, factor=out_factor) v = Matrix{Float64}[] if isa(out_so, Vector) @@ -119,54 +85,65 @@ function system_id( if !verbose redirect_stdout(original_stdout) end - return sys + linear_plant = LinearPlant(sys; inp_offset, inp_factor, out_offset, out_factor) + if isnothing(inp_cond) + return linear_plant + else + return InpCondLinPlant(linear_plant, inp_cond, inp_cond_kwargs) + end end """ model_evolve( - sys::Union{PredictionStateSpace, StateSpace}, + plant::Plant, inp::Union{Vector{Float64}, Matrix{Float64}}; x0::Union{Nothing, Vector{Float64}}=nothing, - inp_cond::Union{Nothing, Function}=nothing, - inp_offset::Float64=0.0, inp_factor::Float64=1.0, - out_offset::Float64=0.0, out_factor::Float64=1.0, - inp_cond_kwargs::Dict{Symbol, T}=Dict{Symbol, Any}(), - )::Union{Vector{Float64}, Matrix{Float64}} where {T} + initialize_x::Bool=false, + ) -Evolve a state space model `sys` with the input steps `inp`. The input is offseted and -scaled with `inp_offset` and `inp_factor` and the output is unscaled and unoffseted -with `out_offset` and `out_factor`. If a function is provided as `inp_cond` it is -applied to the input after scaling and offseting along with any keywords passed for it. +Evolve a plant model through a time series of inputs. If `inp` is a Matrix, the first +dimension will hold different inputs and second dimension will be along time. If `inp` +is a vector, it would be assumed that the model has a single input. If the model also +happens to have a single output and `inp` is a vector, the returned outputs will be a +vector as well. If `x0` is not provided, the stored state vector of the plant model will +be used for initialization. If `initialize_x` is true, then the state vector of the +plant model would be initialized to match first input value so that no sudden jumps +happen. """ function model_evolve( - sys::Union{PredictionStateSpace, StateSpace}, + plant::Plant, inp::Union{Vector{Float64}, Matrix{Float64}}; x0::Union{Nothing, Vector{Float64}}=nothing, - inp_cond::Union{Nothing, Function}=nothing, - inp_offset::Float64=0.0, inp_factor::Float64=1.0, - out_offset::Float64=0.0, out_factor::Float64=1.0, - inp_cond_kwargs::Dict{Symbol, T}=Dict{Symbol, Any}(), -)::typeof(inp) where {T} - inp_so = offset_scale(inp; offset=inp_offset, factor=inp_factor) - if !isnothing(inp_cond) - inp_so = inp_cond(inp_so; inp_cond_kwargs...) - end - u = Matrix{Float64}[] - if isa(inp_so, Vector) - u = Matrix(inp_so') + initialize_x::Bool=false, +) + if isa(inp, Vector) + inp_m = Matrix(inp') else - u = inp_so + inp_m = inp end + temp_plant = deepcopy(plant) - if isnothing(x0) - x0 = inv(Matrix{Float64}(I, size(sys.A)...) - sys.A) * sys.B * u[:, 1] + lp = get_linear_plant(temp_plant) + + if initialize_x + sys = lp.sys + initial_inp = + offset_scale(inp_m[:, 1]; offset=lp.inp_offset, factor=lp.inp_factor) + x0 = inv(Matrix{Float64}(I, size(sys.A)...) - sys.A) * sys.B * initial_inp end - modeled_out_so, _, x0, _ = lsim(sys, u; x0) - modeled_out = unscale_unoffset(modeled_out_so; offset=out_offset, factor=out_factor) - if isa(inp, Vector) - return modeled_out[1, :] + + if !isnothing(x0) + lp.x = x0 + end + + out_m = zeros(Float64, size(lp.sys.C, 1), size(inp_m, 2)) + for ii ∈ axes(inp_m, 2) + out_m[:, ii] = temp_plant(inp_m[:, ii]) + end + if isa(inp, Vector) && size(out_m, 1) == 1 + return out_m[1, :] else - return modeled_out + return out_m end end @@ -176,13 +153,14 @@ end out::Union{Vector{Float64}, Matrix{Float64}}, tt::Vector{Float64}, order::Int, - inp_cond::Union{Nothing, Function}, + inp_cond::InputConditioning, inp_cond_args_guess::Dict{Symbol, T}; inp_offset::Float64=0.0, inp_factor::Float64=1.0, out_offset::Float64=0.0, out_factor::Float64=1.0, inp_cond_args_lower::Dict{Symbol, V}=Dict{Symbol, Any}(), inp_cond_args_upper::Dict{Symbol, W}=Dict{Symbol, Any}(), newpem_kwargs::Dict{Symbol, U}=Dict{Symbol, Any}(), + curve_fit_kwargs::Dict{Symbol, X}=Dict{Symbol, Any}(), verbose::Bool=false, ) where {T, U, V, W} @@ -190,7 +168,7 @@ Perform system identification for a set on input data `inp`, output data `out`, time series vector `tt`. If there are more than one inputs or outputs, provide them as Matrix with first dimension for ports (input or output) and second dimension for time. -The `inp_cond` is applied to offseted and scaled input before performing system +The `inp_cond` is applied before offsetting and scaling for performing system identification. The `inp_cond_args_guess` is used as initial keyword arguments that provide the parameters of the `inp_cond`. These arguments are then used to find the best fit while iteratively performing [`system_id`](@ref) in each step. @@ -206,55 +184,57 @@ identification. For advanced use, it is recommended to do system identification directly with `newpem` and optimize using your favorite fitting method instead of using this function. -Returns a linear state space model of the `order` and the keyword argument dictionary -containing optimal parameters for `inp_cond` function. +Returns a NonLinInpLinearPlant that has stored best fit parameters for the input +non-linearity and a best fit linear model. """ function system_id_optimal_inp_cond( inp::Union{Vector{Float64}, Matrix{Float64}}, out::Union{Vector{Float64}, Matrix{Float64}}, tt::Vector{Float64}, order::Int, - inp_cond::Function, + inp_cond::InputConditioning, inp_cond_args_guess::Dict{Symbol, T}; inp_offset::Float64=0.0, inp_factor::Float64=1.0, out_offset::Float64=0.0, out_factor::Float64=1.0, inp_cond_args_lower::Dict{Symbol, V}=Dict{Symbol, Any}(), inp_cond_args_upper::Dict{Symbol, W}=Dict{Symbol, Any}(), newpem_kwargs::Dict{Symbol, U}=Dict{Symbol, Any}(), + curve_fit_kwargs::Dict{Symbol, X}=Dict{Symbol, Any}(), verbose::Bool=false, -) where {T, U, V, W} +) where {T, U, V, W, X} key_list = collect(keys(inp_cond_args_guess)) function model(inp, param) inp_cond_kwargs = Dict(key => param[ii] for (ii, key) ∈ enumerate(key_list)) - sys = system_id( + plant = system_id( inp, out, tt, order; - inp_cond, inp_offset, inp_factor, out_offset, out_factor, - inp_cond_kwargs, + inp_offset, inp_factor, out_offset, out_factor, + inp_cond, inp_cond_kwargs, newpem_kwargs, verbose, ) - return model_evolve( - sys, inp; - inp_cond, inp_offset, inp_factor, out_offset, out_factor, inp_cond_kwargs, - ) + return model_evolve(plant, inp; initialize_x=true) end guess = [inp_cond_args_guess[key] for key ∈ key_list] - lower = [ + lower = Vector{Float64}( + [ inp_cond_args_lower[key] for key ∈ key_list if key in keys(inp_cond_args_lower) ] - lower = [ + ) + upper = Vector{Float64}( + [ inp_cond_args_upper[key] for key ∈ key_list if key in keys(inp_cond_args_upper) ] + ) - fit_result = coef(curve_fit(model, inp, out, guess)) + fit_result = + coef(curve_fit(model, inp, out, guess; lower, upper, curve_fit_kwargs...)) opt = Dict{Symbol, T}(key => fit_result[ii] for (ii, key) ∈ enumerate(key_list)) - sys = system_id( + return system_id( inp, out, tt, order; - inp_cond, inp_offset, inp_factor, out_offset, out_factor, - inp_cond_kwargs=opt, + inp_offset, inp_factor, out_offset, out_factor, + inp_cond, inp_cond_kwargs=opt, newpem_kwargs, verbose, ) - return sys, opt end diff --git a/test/runtests.jl b/test/runtests.jl index 46458df..0bd6e2b 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -481,23 +481,49 @@ if isempty(ARGS) || "sysid" in ARGS # Interpolate output data along the uniform time ne_uniform = Interpolations.linear_interpolation(ifo_tt, ne_ifo).(tt) - function inp_cond_step( - gas_so::Float64, - previous_gas_so::Float64, - corr::Float64; - p::Union{Float64, Vector{Float64}}=-0.17, - )::Tuple{Float64, Float64} - if p isa Array - param = p[1] - else - param = p - end - if gas_so + corr > previous_gas_so - corr = corr + (gas_so + corr - previous_gas_so) * param + # Creat an input conditioning structure + mutable struct GasNonLinFlow <: SOLPS2ctrl.InputConditioning + param::Float64 + previous_gas_inp::Float64 + corr::Float64 + end + + # Need to make InputConditioning object a callable instance that takes in the + # input as a Vector{Float64}, param as a keyword argument, and outputs a + # Vector{Float64}. + # When the glas flow is increasing, this function applies a factor `param` to + # the increase and add that as correction for all following calls. + # The correction value `corr` and previous gas input value `previous_gas_inp` + # are updated in the structure during the call. + function (gnlf::GasNonLinFlow)( + gas_inp::Vector{Float64}; + param::Float64=gnlf.param, + )::Vector{Float64} + gnlf.param = param # Update param if changed. + if gas_inp[1] + gnlf.corr > gnlf.previous_gas_inp + gnlf.corr += (gas_inp[1] + gnlf.corr - gnlf.previous_gas_inp) * param end - return gas_so + corr, corr + gnlf.previous_gas_inp = gas_inp[1] + gnlf.corr + return gas_inp .+ gnlf.corr end + # function inp_cond_step( + # gas_so::Float64, + # previous_gas_so::Float64, + # corr::Float64; + # p::Union{Float64, Vector{Float64}}=-0.17, + # )::Tuple{Float64, Float64} + # if p isa Array + # param = p[1] + # else + # param = p + # end + # if gas_so + corr > previous_gas_so + # corr = corr + (gas_so + corr - previous_gas_so) * param + # end + # return gas_so + corr, corr + # end + """ inp_cond(inp_gas; p=0.242) @@ -505,17 +531,17 @@ if isempty(ARGS) || "sysid" in ARGS increase amount is changed by a factor of 'p' of the actual change. This gives a response of gas input when it is rising vs falling and thus constitute a non-linearity. """ - function inp_cond( - gas_so::Array{Float64}; - p::Union{Float64, Vector{Float64}}=-0.17, - )::Array{Float64} - ret_vec = deepcopy(gas_so) - corr = 0.0 - for ii ∈ 2:length(ret_vec) - ret_vec[ii], corr = inp_cond_step(ret_vec[ii], ret_vec[ii-1], corr; p) - end - return ret_vec - end + # function inp_cond( + # gas_so::Array{Float64}; + # p::Union{Float64, Vector{Float64}}=-0.17, + # )::Array{Float64} + # ret_vec = deepcopy(gas_so) + # corr = 0.0 + # for ii ∈ 2:length(ret_vec) + # ret_vec[ii], corr = inp_cond_step(ret_vec[ii], ret_vec[ii-1], corr; p) + # end + # return ret_vec + # end # verbose only during CI testinf verbose = get(ENV, "GITHUB_ACTIONS", "") == "true" @@ -528,27 +554,46 @@ if isempty(ARGS) || "sysid" in ARGS verbose, ) + inp_cond = GasNonLinFlow(-0.2, input_gas[1], 0.0) + inp_cond_args_guess = Dict{Symbol, Any}(:param => -0.2) + # Non-linear input + 3rd order linear fit - nonlin_plant_3, p_opt = system_id_optimal_inp_cond( - input_gas, ne_uniform, tt, 3, inp_cond, Dict{Symbol, Any}(:p => -0.2); + nonlin_plant_3 = system_id_optimal_inp_cond( + input_gas, ne_uniform, tt, 3, inp_cond, inp_cond_args_guess; inp_offset=gas_offset, inp_factor=gas_factor, out_offset=ne_offset, out_factor=ne_factor, verbose, ) - lin_out = model_evolve( - linear_plant_3, input_gas; - inp_offset=gas_offset, inp_factor=gas_factor, - out_offset=ne_offset, out_factor=ne_factor, + lin_out = model_evolve(linear_plant_3, input_gas) + + nonlin_out = model_evolve(nonlin_plant_3, input_gas) + + p1 = scatter(tt, ne_uniform * ne_factor; label="SOLPS Data", color=:black) + plot!( + tt, lin_out * ne_factor; label="Linear 3rd order plant fit", + color=:deepskyblue3, linewidth=2, + ) + plot!( + tt, nonlin_out * ne_factor; label="Non-linear 3rd order plant fit", + color=:orange, linewidth=2, ylabel="ne / cubic meter", + legendposition=(0.25, 0.9), + ) + lin_resd = (lin_out - ne_uniform) + nonlin_resd = (nonlin_out - ne_uniform) + p2 = plot(tt, lin_resd * ne_factor; color=:deepskyblue3) + plot!( + tt, nonlin_resd * ne_factor; + color=:orange, ylabel="Residuals", legend=false, ) - nonlin_out = model_evolve( - nonlin_plant_3, input_gas; - inp_offset=gas_offset, inp_factor=gas_factor, - out_offset=ne_offset, out_factor=ne_factor, - inp_cond=inp_cond, inp_cond_kwargs=p_opt) + p3 = plot(tt, input_gas; ylabel="Input Gas\natoms / s", legend=false) - @test sqrt(Statistics.mean((lin_out - ne_uniform) .^ 2)) < 1.2e18 + l = @layout [a{0.5h}; b{0.25h}; c{0.25h}] + plot(p1, p2, p3; layout=l, suptitle="System Identification") + savefig("$(@__DIR__)/Test_System_Identification.pdf") + + @test sqrt(Statistics.mean((lin_out - ne_uniform) .^ 2)) < 3e18 @test sqrt(Statistics.mean((nonlin_out - ne_uniform) .^ 2)) < 0.4e18 end @@ -614,6 +659,7 @@ if isempty(ARGS) || "controller" in ARGS den = s^2 - 2 * rep * s + abs2 Ts = 0.001 # Sampling period of discrete system plant_model = c2d(ss(abs2 * 3 / den), Ts) + plant = SOLPS2ctrl.LinearPlant(plant_model) delay_steps = 100 delay_model = delay(delay_steps * Ts, Ts) @@ -676,15 +722,19 @@ if isempty(ARGS) || "controller" in ARGS end # Create PVLC and another instance of actuator for this run - pvlc = PVLC(c2d(ss(pid(1.0, 1.0 / 10)), Ts), zeros(Float64, 1), plant_model, 20) - act2 = deepcopy(act) + pvlc = PVLC( + c2d(ss(pid(1.0, 1.0 / 10)), Ts), + zeros(Float64, 1), + plant, + 20, + ) # Tune PVLC controller for no delay pm = -200 gm = -1 while pm < 30 || gm < 3 w = collect(logrange(0.1, 1e3, 400)) - wgm, gm, wpm, pm = margin(plant_model * act2.gain * pvlc.ctrl_ss, w) + wgm, gm, wpm, pm = margin(plant_model * act.gain * pvlc.ctrl_ss, w) pm = pm[1, 1, 1] gm = gm[1, 1, 1] if pm < 30 || gm < 3 @@ -693,13 +743,12 @@ if isempty(ARGS) || "controller" in ARGS end # Create MPC - act3 = deepcopy(act) horizon = 50 # Number of steps in future after latency nopt = 5 # Number of optimization points in horizon window opt_every = 10 # Run cost optimization every opt_every steps mpc = MPC( - plant_model, 20, act3, horizon, nopt, opt_every; - ctrl_out_bounds=act3.bounds .* 2.0 ./ act3.gain, + deepcopy(plant), 20, act, horizon, nopt, opt_every; + ctrl_out_bounds=act.bounds .* 2.0 ./ act.gain, ) # Set a target waveform @@ -723,19 +772,19 @@ if isempty(ARGS) || "controller" in ARGS res = Dict() print("Simulation time $(length(target)) steps, PI: ") @time res["PI"] = run_closed_loop_sim( - plant_model, act, lc, target; + plant, act, lc, target; noise_plant_inp, noise_plant_out, noise_ctrl_out, ) println() print("Simulation time $(length(target)) steps, PVLC: ") @time res["PVLC"] = run_closed_loop_sim( - plant_model, act2, pvlc, target; + plant, act, pvlc, target; noise_plant_inp, noise_plant_out, noise_ctrl_out, ) println() print("Simulation time $(length(target)) steps, MPC: ") @time res["MPC"] = run_closed_loop_sim( - plant_model, act3, mpc, target; + plant, act, mpc, target; noise_plant_inp, noise_plant_out, noise_ctrl_out, ) println()