diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 2775b63..09f173f 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -23,6 +23,9 @@ jobs: with: use_coverage: true use_FuseRegistry: false + upload_artifact: true + artifact_name: test_result_images + artifact_path: test/*.pdf # Optional inputs for artifact uploading # For example, uncomment the following 4 lines to upload test result images that # will be generated by test/runtests.jl in test directory diff --git a/Project.toml b/Project.toml index 74f7c26..b8052cf 100644 --- a/Project.toml +++ b/Project.toml @@ -1,14 +1,13 @@ name = "SOLPS2ctrl" uuid = "a531d12f-ac8a-43e8-b6d9-bd121431dd49" authors = ["David Eldon "] -version = "2.2.0" +version = "2.2.1" [deps] -ArgParse = "c7e460c6-2fb9-53a9-8c5b-16f535851c63" Contour = "d38c429a-6771-53c6-b99e-75d170b6e991" ControlSystemIdentification = "3abffc1c-5106-53b7-b354-a47bfc086282" ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e" -DelimitedFiles = "8bb1440f-4735-579b-a4ab-409b98df4dab" +DataStructures = "864edb3b-99cc-5e75-8d2d-829cb0a9cfe8" EFIT = "cda752c5-6b03-55a3-9e33-132a441b0c17" IMAS = "13ead8c1-b7d1-41bb-a6d0-5b8b65ed587a" IMASggd = "b7b5e640-9b39-4803-84eb-376048795def" @@ -17,19 +16,15 @@ JSON = "682c06a0-de6a-54ab-a142-c8b1cf79cde6" LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" LsqFit = "2fda8390-95c7-5789-9bda-21331edee243" PhysicalConstants = "5ad8b20f-a522-5ce9-bfc9-ddf1d5bda6ab" -PlotUtils = "995b91a9-d308-5afd-9ec6-746e21dbc043" -Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80" SOLPS2imas = "09becab6-0636-4c23-a92a-2b3723265c31" Statistics = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" -Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" Unitful = "1986cc42-f94f-5a68-af5c-568840ba703d" [compat] -ArgParse = "1" Contour = "0.6" ControlSystemIdentification = "2" ControlSystemsBase = "1" -DelimitedFiles = "1" +DataStructures = "0.18.22" EFIT = "1" IMAS = "1, 2, 3, 4, 5" IMASggd = "3.3" @@ -38,8 +33,6 @@ JSON = "0.21" LinearAlgebra = "1" LsqFit = "0.15.1" PhysicalConstants = "0.2" -PlotUtils = "1" -Plots = "1" SOLPS2imas = "2.2" Statistics = "1" Unitful = "1" diff --git a/README.md b/README.md index 815d794..4c9c613 100644 --- a/README.md +++ b/README.md @@ -26,3 +26,61 @@ Pkg.add("SOLPS2ctrl") ## Examples Refer to the instructions on this [wiki page](https://github.com/ProjectTorreyPines/SOLPS2ctrl.jl/wiki/Demo) to see how to run `examples/demo.ipynb`. + +## For developers + +If you are contributing to this project, you would need to install [dvc](https://dvc.org/) to fetch sample files for testing. Once installed, please configure your ssh so that you can ssh into omega tunneling through cybele without requiring to enter password. This is optional but will make it much easier for you. + +Once you have completed above steps, inside the git repo, simply do: +```bash +dvc pull +``` + +This would download the sample files in the `samples` directory. Then to run tests, you would first need to instantiate the project: +```bash +julia --project +``` +Then press `]`: +```julia + _ + _ _ _(_)_ | Documentation: https://docs.julialang.org + (_) | (_) (_) | + _ _ _| |_ __ _ | Type "?" for help, "]?" for Pkg help. + | | | | | | |/ _` | | + | | |_| | | | (_| | | Version 1.11.5 (2025-04-14) + _/ |\__'_|_|_|\__'_| | Official https://julialang.org/ release +|__/ | + +julia> ] +``` +Then type: +```julia +(SOLPS2ctrl) pkg> instantiate +``` +Once the package has been instantiated, you can run the tests using: +```julia +(SOLPS2ctrl) pkg> test +``` +This would run all the tests though. To run specific tests, you can do following from the command line to see help options (this works after you ahve instantiated the project like mentioned above): +```bash +% julia --project test/test.jl help +Usage (from inside SOLPS2ctrl.jl): +julia --project test/test.jl [units] [core] [edge] [heavy] [repair] [geqdsk] [prep] [sysid] [state] [controller] [h] [help] + +Run tests. Default is all tests. + +Optional arguments: + units Test unit conversion utilities + core Test core profile extension + edge Test edge profile extension + heavy Test heavy utilities + repair Test repair_eq + geqdsk Test geqdsk_to_imas + prep Test preparation + sysid Test system id + state Test state prediction + controller Test linear and PVLC controllers + h Show this help message and exit + help Show this help message and exit + +``` diff --git a/docs/src/index.md b/docs/src/index.md index 6f76813..514fe47 100644 --- a/docs/src/index.md +++ b/docs/src/index.md @@ -77,10 +77,193 @@ cached_mesh_extension! offset_scale unscale_unoffset system_id -system_id_optimal_input_cond +system_id_optimal_inp_cond model_evolve ``` +## Actuators + +```@docs +Actuator +DelayedActuator +``` +For example: +```@example +using SOLPS2ctrl: DelayedActuator +# Actuator with delay of 3 steps +dact = DelayedActuator(3; default_output=[0.0, 0.0]) +inp_series = [ones(2) * i * 0.1 for i in 1:6] +for (i, inp) in enumerate(inp_series) + println("Calling $(i)th time, return value is $(dact(inp))") +end +``` + +## Controllers + +```@docs +Controller +``` + +### Linear Controller + +```@docs +LinearController +``` + +### Predicted Variable Linear Controller + +```@docs +PVLC +state_prediction_matrices +``` + +#### State Prediction Matrix Algebra + +At step k: + +```math +\begin{split} +y_k &= C x_k + D u_k\\ +x_k &= A x_{k-1} + B u_{k-1}\\ +x_{k+1} &= A x_{k} + B u_{k} +\end{split} +``` + +Therefore to h steps in history: + +For all output estimates: + +```math +\begin{split} +y_k & = C x_k + D u_k \\ + & = C (A x_{k-1} + B u_{k-1}) + D u_k \\ + & = C (A (A x_{k-2} + B u_{k-2}) + B u_{k-1}) + D u_k \\ + & = ... \\ + & = C A^{h-1} x_{k-(h-1)} + C A^{h-2} B u_{k-(h-1)} + C A^{h-3} B u_{k-(h-2)} + ... + CAB u_{k-2} + CB u_{k-1} + D u_k \\ +\end{split} +``` + +```math +\begin{split} +y_k &= C A^{h-1} x_{k-(h-1)} + C A^{h-2} B u_{k-(h-1)} + C A^{h-3} B u_{k-(h-2)} + ... + CAB u_{k-2} + CB u_{k-1} + D u_k \\ +y_{k-1} &= C A^{h-2} x_{k-(h-1)} + C A^{h-3} B u_{k-(h-1)} + C A^{h-4} B u_{k-(h-2)} + ... + CAB u_{k-3} + CB u_{k-2} + D u_{k-1} \\ + +... &= ... \\ +y_{k-i} &= C A^{h-1-i} x_{k-(h-1)} + C A^{h-2-i} B u_{k-(h-1)} + C A^{h-3-i} B u_{k-(h-2)} + ... + CB u_{k-i-1} + D u_{k-i} \\ +... &= ... \\ +y_{k-(h-3)} &= C A^2 x_{k-(h-1)} + CAB u_{k-(h-1)}+ CB u_{k-(h-2)} + D u_{k-(h-3)} \\ +y_{k-(h-2)} &= C A x_{k-(h-1)} + CB u_{k-(h-1)} + D u_{k-(h-2)} \\ +y_{k-(h-1)} &= C x_{k-(h-1)} + D u_{k-(h-1)} +\end{split} +``` + +For predicted next state: + +```math +\begin{split} +x_{k+1} & = A x_{k} + B u_{k} \\ + & = A (A x_{k-1} + B u_{k-1}) + B u_{k} \\ + & = A (A (A x_{k-2} + B u_{k-2}) + B u_{k-1}) + B u_{k} \\ + & = ... \\ + & = A^h x_{k-(h-1)} + A^{h-1} B u_{k-(h-1)} + A^{h-2} B u_{k-(h-2)} + ... + A^2 B u_{k-2} + A B u_{k-1} + B u_{k} \\ +\end{split} +``` + +In terms of mega-matrices, define mega-vectors of inputs and outputs: + +```math +\vec{Y} = \begin{bmatrix} + y_{k-(h-1)} \\ + y_{k-(h-2)} \\ + ... \\ + y_{k-1} \\ + y_{k} + \end{bmatrix} +``` + +Note that for multiple outputs, each output vector will be stacked vertically to create a single column. + +```math +\vec{U} = \begin{bmatrix} + u_{k-(h-1)} \\ + u_{k-(h-2)} \\ + ... \\ + u_{k-1} \\ + u_{k} + \end{bmatrix} +``` + +Note that for multiple inputs, each input vector will be stacked vertically to create a single column. + +Then, from $x_{k-(h-1)}$ and $\vec{U}$, we get $\vec{Y}$ and predicted state $x_{k+1}$: +```math +\vec{Y} = \mathcal{L} x_{k-(h-1)} + \mathcal{M} \vec{U} +``` +```math +x_{k+1} = \mathcal{N} x_{k-(h-1)} + \mathcal{O} \vec{U} +``` + +Where the mega-matrices $\mathcal{L}$, $\mathcal{M}$, $\mathcal{N}$, $\mathcal{O}$ are: + +``\mathcal{L}`` is a matrix with (h x no. of outputs) rows and state-space order columns: + +```math +\mathcal{L} = \begin{bmatrix} + C... \\ + CA... \\ + CA^2... \\ + ... \\ + CA^{h-2}... \\ + CA^{h-1}... + \end{bmatrix} +``` + +``\mathcal{M}`` is a matrix with (h x no. of outputs) rows and (h x no. of inputs) columns: + +```math +\mathcal{M} = \begin{bmatrix} + D & 0 & 0 & ... & 0 & 0 & 0 & 0 & \\ + CB & D & 0 & ... & 0 & 0 & 0 & 0 & \\ + CAB & CB & D & ... & 0 & 0 & 0 & 0 & \\ + ... & ... & ... & ... & ... & ... & ... & ... & ... \\ + C A^{h-4} B & C A^{h-5} B & C A^{h-6} B & ... & CAB & CB & D & 0 & 0 \\ + C A^{h-3} B & C A^{h-4} B & C A^{h-5} B & ... & CA^2B & CAB & CB & D & 0 \\ + C A^{h-2} B & C A^{h-3} B & C A^{h-4} B & ... & CA^3B & CA^2B & CAB & CB & D \\ + \end{bmatrix} +``` + +``\mathcal{N}`` is a square matrix with state-space order columns and rows: + +```math +\mathcal{N} = A^{h} +``` + +``\mathcal{O}`` is a matrix with state-space order rows and (h x no. of inputs) columns: + +```math +\mathcal{O} = \begin{bmatrix} A^{h-1} B & A^{h-2} B & A^{h-3} B & ... & A^2 B & AB & B \end{bmatrix} +``` + +Then, future state can be predicted by: + +```math +x_{k+1} = \mathcal{N} \mathcal{L}^{-1} (\vec{Y} - \mathcal{M} \vec{U}) + \mathcal{O} \vec{U} +``` + +### Model Predictive Controller + +```@docs +MPC +``` + +## Closed loop simulations + +```@docs +lsim_step +model_step +run_closed_loop_sim +``` + ## Unit conversion utilities ```@docs diff --git a/src/SOLPS2ctrl.jl b/src/SOLPS2ctrl.jl index 04cc4fa..0c588ea 100644 --- a/src/SOLPS2ctrl.jl +++ b/src/SOLPS2ctrl.jl @@ -11,6 +11,7 @@ include("$(@__DIR__)/supersize_profile.jl") include("$(@__DIR__)/repair_eq.jl") include("$(@__DIR__)/unit_utils.jl") include("$(@__DIR__)/system_id.jl") +include("$(@__DIR__)/controllers.jl") """ find_files_in_allowed_folders( diff --git a/src/controllers.jl b/src/controllers.jl new file mode 100644 index 0000000..72a57bf --- /dev/null +++ b/src/controllers.jl @@ -0,0 +1,713 @@ +import ControlSystemIdentification: PredictionStateSpace +import ControlSystemsBase: StateSpace, Discrete +import LinearAlgebra: pinv +import DataStructures: Queue, enqueue!, dequeue!, empty!, first +import LsqFit: curve_fit, coef + +export lsim_step, model_step, state_prediction_matrices, Actuator, DelayedActuator, + Controller, LinearController, PVLC, 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}} + +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 + +""" + 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). +""" +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 +end + +function _calculate_LMNO( + sys::Union{PredictionStateSpace, StateSpace}, + h::Int=size(sys.A, 1), +) + A = sys.A + B = sys.B + C = sys.C + D = sys.D + order = size(A, 1) + ninps = size(B, 2) + nouts = size(C, 1) + + L = zeros((h * nouts, order)) + M = zeros((h * nouts, h * ninps)) + O = zeros((order, h * ninps)) + Atoim1 = Matrix{Float64}(I, (order, order)) + + for j ∈ 1:h + # println("Filling M with D: ", (nouts*(j-1)+1):(nouts*j), ", ", (nouts*(j-1)+1):(nouts*j)) + M[(nouts*(j-1)+1):(nouts*j), (ninps*(j-1)+1):(ninps*j)] = D + end + for i ∈ 1:h + # println("i = ", i) + if i > 1 + Atoim1 = Atoim1 * A + end + CAtoim1 = C * Atoim1 + # println("Filling L[$((nouts*(i-1)+1):(nouts*i)), :] = CA^$(i-1)") + L[(nouts*(i-1)+1):(nouts*i), :] = CAtoim1 + + for j ∈ (i+1):h + # println("Filling M[$((nouts*(j-1)+1):(nouts*j)), $((ninps*(j-1-i)+1):(ninps*(j-i)))] = CA^$(i-1)B") + M[(nouts*(j-1)+1):(nouts*j), (ninps*(j-1-i)+1):(ninps*(j-i))] = CAtoim1 * B + end + # println("Filling O[:, $((ninps*(h-i)+1):(ninps*(h-i+1)))] = A^$(i-1)B") + O[:, (ninps*(h-i)+1):(ninps*(h-i+1))] = Atoim1 * B + end + N = Atoim1 * A + return L, M, N, O +end + +""" + state_prediction_matrices( + sys::Union{PredictionStateSpace, StateSpace}, + h::Int=size(sys.A, 1), + ) + +Calculate state prediction matrices for a linear system `sys` given `h` steps of input +and output history. This function returns two matrices, `Y2x` and `U2x` that can be +used to calculate least square fitted estimate of the current state vector of the +system. +`Y2x` has size: (State size of `sys`) x (No. of outputs times h) +`U2x` has size: (State size of `sys`) x (No. of inputs times h) +The estimated state vector is obtained by: +Y2x * Y + U2x * U +where Y is all the `h` outputs of system stacked into a single vector. +and U is all the `h` inputs to the system stacked into a single vector. +""" +function state_prediction_matrices( + sys::Union{PredictionStateSpace, StateSpace}, + h::Int=size(sys.A, 1), +) + L, M, N, O = _calculate_LMNO(sys, h) + Linv = pinv(L) + Y2x = N * Linv + U2x = (O - Y2x * M) + 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 + +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). +It also stores the state vector for the state space model as `ctrl_x0::Vector{Float64}`. +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: + + (pvlc::PVLC)(; + ii::Int, + target::Matrix{Float64}, + 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} + +Tracks the state vector of the plant using `h` steps of history from `plant_inp` and +`plant_out` and uses it to calculate future output of the plant. It compares it with a +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} + h::Int + Y2x::Matrix{Float64} + U2x::Matrix{Float64} + function PVLC( + ctrl_ss::StateSpace{TE}, + ctrl_x0::Vector{Float64}, + plant::Union{PredictionStateSpace, StateSpace}, + h::Int, + ) where {TE <: Discrete} + Y2x, U2x = state_prediction_matrices(plant, h) + return new(ctrl_ss, ctrl_x0, plant, h, Y2x, U2x) + end +end + +function (pvlc::PVLC)(; + ii::Int, + target::Matrix{Float64}, + 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)) + future_inp = get_future_inp(act) + 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]) + # Estimate current state vector of plant model + est_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, + ) + # Estimate the error signal in future + err = target[:, ii+lat] - future_out[:, end] + # Apply the linear controller in future + ctrl_out, pvlc.ctrl_x0 = lsim_step(pvlc.ctrl_ss, err; x0=pvlc.ctrl_x0) + end + return ctrl_out +end + +function expand( + steps::StepRange{Int, Int}, + red_vec::Vector{Float64}, + ninps::Int, + horizon::Int, +)::Vector{Vector{Float64}} + red_mat = reshape(red_vec, ninps, length(steps)) + mat = zeros(Float64, ninps, horizon + 1) + for i ∈ 1:ninps + mat[i, :] = + Interpolations.linear_interpolation( + steps, + red_mat[i, :]; + extrapolation_bc=Interpolations.Line(), + ).( + 1:(horizon+1), + ) + end + return [mat[:, i] for i ∈ 1:(horizon+1)] +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: + + MPC( + plant::Union{PredictionStateSpace, StateSpace}, + h::Int, + act::Actuator, # Actuator model without delay + horizon::Int, # Number of steps in future after latency + nopt::Int, # Number of optimization points in horizon window + opt_every::Int; # Run cost optimization every opt_every steps + ctrl_out_bounds::Tuple{Vector{Float64}, Vector{Float64}}=( + Array{Float64}(undef, 0), + Array{Float64}(undef, 0), + ), + ) + +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`. +`act` needs to be a deepcopy of the actuator instance. `horizon` is the number of steps +after the `min_delay` among all acturators for which the optimization is carried out. +`nopt` is the number of optimization points taken along the `horizon` which are lineary +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. + +This controller has a call signature: + + (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)), + ), + kwargs..., + )::Vector{Float64} where {T} + +Tracks the state vector of the plant using `h` steps of history from `plant_inp` and +`plant_out` and uses it along with `act` to run an optimization to match `target` in +future after the minimum delay from all the actuators. This function uses +[`curve_fit()`](https://julianlsolvers.github.io/LsqFit.jl/latest/api/#LsqFit.curve_fit) +for the non-linear least squares fitting which uses Levenberg-Marquardt algorithm. This +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} + h::Int + Y2x::Matrix{Float64} + U2x::Matrix{Float64} + act::Actuator # Actuator model without delay + horizon::Int # Number of steps in future after latency + nopt::Int # Number of optimization points in horizon window + opt_every::Int # Run cost optimization every opt_every steps + last_opt::Int # Step index when optimization ran last time + min_delay::Int # Minimum delay in all actuators + ctrl_out_bounds::Tuple{Vector{Float64}, Vector{Float64}} + future_evolve::Function + ctrl_out_buffer::Queue{Vector{Float64}} + function MPC( + plant::Union{PredictionStateSpace, StateSpace}, + h::Int, + act::Actuator, # Actuator model without delay + horizon::Int, # Number of steps in future after latency + nopt::Int, # Number of optimization points in horizon window + opt_every::Int; # Run cost optimization every opt_every steps + ctrl_out_bounds::Tuple{Vector{Float64}, Vector{Float64}}=( + Array{Float64}(undef, 0), + Array{Float64}(undef, 0), + ), + ) + Y2x, U2x = state_prediction_matrices(plant, 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), + 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) + 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 + ctrl_out = [ctrl_out; repeat(zeros(Float64, ninps), mpc.min_delay)] + plant_inp = zeros(Float64, ninps) + 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, + ) + end + return vec(plant_out) + end + last_opt = 0 + ctrl_out_buffer = Queue{Vector{Float64}}(horizon) + return new( + plant, h, Y2x, U2x, act, horizon, nopt, opt_every, last_opt, + min_delay, ctrl_out_bounds, fe, ctrl_out_buffer, + ) + end +end + +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)), + ), + kwargs..., +)::Vector{Float64} where {T} + fut_lookup = mpc.min_delay + mpc.horizon + 1 + + enough_history = ii - mpc.h + 1 > 0 + enough_future = ii + fut_lookup <= size(target, 2) + opt_step = ii > mpc.opt_every + mpc.last_opt + buf_empty = isempty(mpc.ctrl_out_buffer) + + if enough_history && enough_future && (opt_step || buf_empty) + 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) + + # Gather history of inputs and outputs + U = vec(plant_inp[:, (ii-mpc.h+1):ii]) + Y = vec(plant_out[:, (ii-mpc.h+1):ii]) + # Estimate current state vector of plant model + x0 = 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)) + 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, + ), + red_steps, target_vec, guess; + lower, upper, + ) + + # Empty buffer and fill with updated control outputs + empty!(mpc.ctrl_out_buffer) + for co ∈ expand(red_steps, coef(fit_res), ninps, mpc.horizon) + enqueue!(mpc.ctrl_out_buffer, co) + end + end + if opt_step + mpc.last_opt = ii + end + if isempty(mpc.ctrl_out_buffer) + return zeros(Float64, size(mpc.plant.B, 2)) + else + return dequeue!(mpc.ctrl_out_buffer) + end +end + +""" + run_closed_loop_sim( + plant::Union{PredictionStateSpace{TE}, StateSpace{TE}}, + 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)), + ), + ctrl_start_ind::Int=1, + noise_plant_inp::Matrix{Float64}=zeros( + Float64, + (size(plant.B, 2), size(target, 2)), + ), + noise_plant_out::Matrix{Float64}=zeros( + Float64, + (size(plant.C, 1), size(target, 2)), + ), + noise_ctrl_out::Matrix{Float64}=zeros( + Float64, + (size(plant.B, 2), size(target, 2)), + ), + ) where {T, TE <: Discrete} + +Generic function to run closed loop simulations with provided `plant`, actuator +`act`, controller `ctrl`, and target waveform `target`. The length of simulation is +determined by provided `target`. Keyword arguments are possible for providing +adjustments to inputs and outputs of the plant model as explained in +[`model_evolve()`](@ref). Additionally, `ctrl_start_ind` can be provided to start +control loop at an arbitrary point in the loop. `noise_plant_inp`, `noise_plant_out`, +and `noise_ctrl_out` allow addition of predefined noise waveforms at the input of plant, +output of plant, and the output of controller respectively. +""" +function run_closed_loop_sim( + plant::Union{PredictionStateSpace{TE}, StateSpace{TE}}, + 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)), + ), + ctrl_start_ind::Int=1, + noise_plant_inp::Matrix{Float64}=zeros( + Float64, + (size(plant.B, 2), size(target, 2)), + ), + noise_plant_out::Matrix{Float64}=zeros( + Float64, + (size(plant.C, 1), size(target, 2)), + ), + noise_ctrl_out::Matrix{Float64}=zeros( + Float64, + (size(plant.B, 2), size(target, 2)), + ), +) where {T, TE <: Discrete} + # 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] + + # Closed loop simulation + for ii ∈ eachindex(target) + # 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] .+= 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, + ) .+ noise_ctrl_out[:, ii+1] + end + end + return Dict( + :plant => plant, + :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, + :plant_out => plant_out, + :ctrl_out => ctrl_out, + :noise_plant_inp => noise_plant_inp, + :noise_plant_out => noise_plant_out, + :noise_ctrl_out => noise_ctrl_out, + ) +end diff --git a/src/system_id.jl b/src/system_id.jl index 8758f76..67e2d29 100644 --- a/src/system_id.jl +++ b/src/system_id.jl @@ -4,7 +4,9 @@ import LinearAlgebra: I, inv import LsqFit: curve_fit, coef export offset_scale, - unscale_unoffset, model_evolve, system_id, system_id_optimal_input_cond + unscale_unoffset, model_evolve, system_id, + system_id_optimal_inp_cond + """ offset_scale( val::Union{Float64, Vector{Float64}, Matrix{Float64}}; @@ -50,10 +52,10 @@ end out::Union{Vector{Float64}, Matrix{Float64}}, tt::Vector{Float64}, order::Int; - input_cond::Union{Nothing, Function}=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, - input_cond_kwargs::Dict{Symbol, T}=Dict{Symbol, Any}(), + inp_cond_kwargs::Dict{Symbol, T}=Dict{Symbol, Any}(), newpem_kwargs::Dict{Symbol, U}=Dict{Symbol, Any}(), verbose::Bool=false, ) where {T, U} @@ -61,9 +63,10 @@ end Perform system identification for a set on input data `inp`, output data `out`, and 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 `input_cond` is provided, it is applied to offseted and scaled input before + +If `inp_cond` is provided, it is applied to offseted and scaled input before performing system identification with keywords for this function provided in -`input_cond_kwargs`. +`inp_cond_kwargs`. This function uses [ControlSystemIdentification.newpem](https://baggepinnen.github.io/ControlSystemIdentification.jl/stable/ss/#ControlSystemIdentification.newpem) to perform the system identification. Any additional keywords for this function should @@ -77,17 +80,17 @@ function system_id( out::Union{Vector{Float64}, Matrix{Float64}}, tt::Vector{Float64}, order::Int; - input_cond::Union{Nothing, Function}=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, - input_cond_kwargs::Dict{Symbol, T}=Dict{Symbol, Any}(), + 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(input_cond) - inp_so = input_cond(inp_so; input_cond_kwargs...) + if !isnothing(inp_cond) + inp_so = inp_cond(inp_so; inp_cond_kwargs...) end u = Matrix{Float64}[] if isa(inp_so, Vector) @@ -123,10 +126,11 @@ end model_evolve( sys::Union{PredictionStateSpace, StateSpace}, inp::Union{Vector{Float64}, Matrix{Float64}}; - input_cond::Union{Nothing, Function}=nothing, + 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, - input_cond_kwargs::Dict{Symbol, T}=Dict{Symbol, Any}(), + inp_cond_kwargs::Dict{Symbol, T}=Dict{Symbol, Any}(), )::Union{Vector{Float64}, Matrix{Float64}} where {T} Evolve a state space model `sys` with the input steps `inp`. The input is offseted and @@ -137,14 +141,15 @@ applied to the input after scaling and offseting along with any keywords passed function model_evolve( sys::Union{PredictionStateSpace, StateSpace}, inp::Union{Vector{Float64}, Matrix{Float64}}; - input_cond::Union{Nothing, Function}=nothing, + 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, - input_cond_kwargs::Dict{Symbol, T}=Dict{Symbol, Any}(), + 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(input_cond) - inp_so = input_cond(inp_so; input_cond_kwargs...) + if !isnothing(inp_cond) + inp_so = inp_cond(inp_so; inp_cond_kwargs...) end u = Matrix{Float64}[] if isa(inp_so, Vector) @@ -153,10 +158,12 @@ function model_evolve( u = inp_so end - x0 = inv(Matrix{Float64}(I, size(sys.A)...) - sys.A) * sys.B * u[:, 1] + if isnothing(x0) + x0 = inv(Matrix{Float64}(I, size(sys.A)...) - sys.A) * sys.B * u[:, 1] + end modeled_out_so, _, x0, _ = lsim(sys, u; x0) modeled_out = unscale_unoffset(modeled_out_so; offset=out_offset, factor=out_factor) - if size(modeled_out)[1] == 1 + if isa(inp, Vector) return modeled_out[1, :] else return modeled_out @@ -164,17 +171,17 @@ function model_evolve( end """ - system_id_optimal_input_cond( + system_id_optimal_inp_cond( inp::Union{Vector{Float64}, Matrix{Float64}}, out::Union{Vector{Float64}, Matrix{Float64}}, tt::Vector{Float64}, order::Int, - input_cond::Union{Nothing, Function}, - input_cond_args_guess::Dict{Symbol, T}; + inp_cond::Union{Nothing, Function}, + 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, - input_cond_args_lower::Dict{Symbol, V}=Dict{Symbol, Any}(), - input_cond_args_upper::Dict{Symbol, W}=Dict{Symbol, Any}(), + 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}(), verbose::Bool=false, ) where {T, U, V, W} @@ -183,9 +190,9 @@ 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 `input_cond` is applied to offseted and scaled input before performing system -identification. The `input_cond_args_guess` is used as initial keyword arguments that -provide the parameters of the `input_cond`. These arguments are then used to find the +The `inp_cond` is applied to offseted and scaled input before 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. This function uses [ControlSystemIdentification.newpem](https://baggepinnen.github.io/ControlSystemIdentification.jl/stable/ss/#ControlSystemIdentification.newpem) @@ -200,75 +207,54 @@ For advanced use, it is recommended to do system identification directly with `n 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 `input_cond` function. +containing optimal parameters for `inp_cond` function. """ -function system_id_optimal_input_cond( +function system_id_optimal_inp_cond( inp::Union{Vector{Float64}, Matrix{Float64}}, out::Union{Vector{Float64}, Matrix{Float64}}, tt::Vector{Float64}, order::Int, - input_cond::Function, - input_cond_args_guess::Dict{Symbol, T}; + inp_cond::Function, + 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, - input_cond_args_lower::Dict{Symbol, V}=Dict{Symbol, Any}(), - input_cond_args_upper::Dict{Symbol, W}=Dict{Symbol, Any}(), + 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}(), verbose::Bool=false, ) where {T, U, V, W} - key_list = collect(keys(input_cond_args_guess)) + key_list = collect(keys(inp_cond_args_guess)) function model(inp, param) - input_cond_kwargs = Dict(key => param[ii] for (ii, key) ∈ enumerate(key_list)) + inp_cond_kwargs = Dict(key => param[ii] for (ii, key) ∈ enumerate(key_list)) sys = system_id( - inp, - out, - tt, - order; - input_cond, - inp_offset, - inp_factor, - out_offset, - out_factor, - input_cond_kwargs, - newpem_kwargs, - verbose, + inp, out, tt, order; + inp_cond, inp_offset, inp_factor, out_offset, out_factor, + inp_cond_kwargs, + newpem_kwargs, verbose, ) return model_evolve( - sys, - inp; - input_cond, - inp_offset, - inp_factor, - out_offset, - out_factor, + sys, inp; + inp_cond, inp_offset, inp_factor, out_offset, out_factor, inp_cond_kwargs, ) end - guess = [input_cond_args_guess[key] for key ∈ key_list] + guess = [inp_cond_args_guess[key] for key ∈ key_list] lower = [ - input_cond_args_lower[key] for - key ∈ key_list if key in keys(input_cond_args_lower) + inp_cond_args_lower[key] for + key ∈ key_list if key in keys(inp_cond_args_lower) ] lower = [ - input_cond_args_upper[key] for - key ∈ key_list if key in keys(input_cond_args_upper) + 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)) opt = Dict{Symbol, T}(key => fit_result[ii] for (ii, key) ∈ enumerate(key_list)) sys = system_id( - inp, - out, - tt, - order; - input_cond, - inp_offset, - inp_factor, - out_offset, - out_factor, - input_cond_kwargs=opt, - newpem_kwargs, - verbose, + inp, out, tt, order; + inp_cond, inp_offset, inp_factor, out_offset, out_factor, + inp_cond_kwargs=opt, + newpem_kwargs, verbose, ) return sys, opt end diff --git a/test/.gitignore b/test/.gitignore new file mode 100644 index 0000000..f08278d --- /dev/null +++ b/test/.gitignore @@ -0,0 +1 @@ +*.pdf \ No newline at end of file diff --git a/test/Project.toml b/test/Project.toml new file mode 100644 index 0000000..11dfbfe --- /dev/null +++ b/test/Project.toml @@ -0,0 +1,6 @@ +[deps] +ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e" +DelimitedFiles = "8bb1440f-4735-579b-a4ab-409b98df4dab" +Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80" +Printf = "de0858da-6303-5e67-8744-51eddeeeb8d7" +Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" diff --git a/test/runtests.jl b/test/runtests.jl index 6060d7d..46458df 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -1,57 +1,10 @@ using SOLPS2ctrl -using SOLPS2ctrl: SOLPS2ctrl -using SOLPS2imas: SOLPS2imas -using IMAS: IMAS -using EFIT: EFIT +using SOLPS2ctrl: + SOLPS2ctrl, SOLPS2imas, IMAS, IMASggd, EFIT, Unitful, Interpolations, Statistics using Plots using Test -using Unitful: Unitful -using Interpolations: Interpolations -using ArgParse: ArgParse -using IMASggd: IMASggd, get_grid_subset using DelimitedFiles: readdlm -using Statistics: mean - -function parse_commandline() - # Define newARGS = ["--yourflag"] to run only tests on your flags when including runtests.jl - localARGS = (@isdefined(newARGS) && newARGS !== nothing) ? newARGS : ARGS # Thanks https://stackoverflow.com/a/44978474/6605826 - s = ArgParse.ArgParseSettings(; description="Run tests. Default is all tests.") - - ArgParse.add_arg_table!(s, - ["--unit_utils"], - Dict(:help => "Test only unit conversion utilities", - :action => :store_true), - ["--core_profile_extension"], - Dict(:help => "Test only core profile extension", - :action => :store_true), - ["--edge_profile_extension"], - Dict(:help => "Test only edge profile extension", - :action => :store_true), - ["--heavy_utilities"], - Dict(:help => "Test only heavy utilities", - :action => :store_true), - ["--repair_eq"], - Dict(:help => "Test only repair_eq", - :action => :store_true), - ["--geqdsk_to_imas"], - Dict(:help => "Test only geqdsk_to_imas", - :action => :store_true), - ["--preparation"], - Dict(:help => "Test only preparation", - :action => :store_true), - ["--system_id"], - Dict(:help => "Test only system identification", - :action => :store_true), - ) - args = ArgParse.parse_args(localARGS, s) - if !any(values(args)) # If no flags are set, run all tests - for k ∈ keys(args) - args[k] = true - end - end - return args -end -args = parse_commandline() +using ControlSystemsBase: lsim, ssrand, delay, tf, c2d, ss, pid, margin """ make_test_profile() @@ -116,7 +69,7 @@ function define_default_sample_set() return b2fgmtry, b2time, b2mn, eqdsk end -if args["unit_utils"] +if isempty(ARGS) || "units" in ARGS @testset "Unit conversion utilities" begin # Gas unit converter flow_tls = 40.63 * Unitful.Torr * Unitful.L / Unitful.s @@ -151,7 +104,7 @@ if args["unit_utils"] end end -if args["core_profile_extension"] +if isempty(ARGS) || "core" in ARGS @testset "core_profile_extension" begin # Just the basic profile extrapolator ------------------ edge_rho = Array(LinRange(0.88, 1.0, 18)) @@ -206,7 +159,7 @@ if args["core_profile_extension"] end end -if args["edge_profile_extension"] +if isempty(ARGS) || "edge" in ARGS @testset "edge_profile_extension" begin # Test for getting mesh spacing b2fgmtry, b2time, b2mn, eqdsk = define_default_sample_set() @@ -221,7 +174,7 @@ if args["edge_profile_extension"] grid_ggd = dd.edge_profiles.grid_ggd[grid_ggd_idx] extended_subs = 1:5 orig_subs = [ - deepcopy(get_grid_subset(grid_ggd, i)) for + deepcopy(IMASggd.get_grid_subset(grid_ggd, i)) for i ∈ extended_subs ] cfn = SOLPS2ctrl.cached_mesh_extension!(dd, eqdsk, b2fgmtry; clear_cache=true) @@ -234,9 +187,9 @@ if args["edge_profile_extension"] ) for j ∈ extended_subs orig_sub = orig_subs[j] - std_sub = get_grid_subset(grid_ggd, -j) - all_sub = get_grid_subset(grid_ggd, j) - ext_sub = get_grid_subset(grid_ggd, -200 - j) + std_sub = IMASggd.get_grid_subset(grid_ggd, -j) + all_sub = IMASggd.get_grid_subset(grid_ggd, j) + ext_sub = IMASggd.get_grid_subset(grid_ggd, -200 - j) orig_indices = [ele.object[1].index for ele ∈ orig_sub.element] std_indices = [ele.object[1].index for ele ∈ std_sub.element] all_indices = [ele.object[1].index for ele ∈ all_sub.element] @@ -268,7 +221,7 @@ if args["edge_profile_extension"] end end -if args["heavy_utilities"] +if isempty(ARGS) || "heavy" in ARGS @testset "heavy_utilities" begin # Test for finding files in allowed folders file_list = define_default_sample_set() @@ -319,7 +272,7 @@ if args["heavy_utilities"] end end -if args["repair_eq"] +if isempty(ARGS) || "repair" in ARGS @testset "repair_eq" begin # Prepare sample dd = IMAS.dd() @@ -341,7 +294,7 @@ if args["repair_eq"] end end -if args["geqdsk_to_imas"] +if isempty(ARGS) || "geqdsk" in ARGS @testset "geqdsk_to_imas" begin sample_files = (splitdir(pathof(SOLPS2ctrl))[1] * "/../sample/") .* [ @@ -439,7 +392,7 @@ if args["geqdsk_to_imas"] end end -if args["preparation"] +if isempty(ARGS) || "prep" in ARGS @testset "preparation" begin eqdsk_file = "geqdsk_iter_small_sample" sample_paths = [ @@ -469,7 +422,7 @@ if args["preparation"] end end -if args["system_id"] +if isempty(ARGS) || "sysid" in ARGS @testset "system_id" begin case = "$(@__DIR__)/../sample/D3D_Lore_Time_Dep" println("Reading interferometer data...") @@ -576,7 +529,7 @@ if args["system_id"] ) # Non-linear input + 3rd order linear fit - nonlin_plant_3, p_opt = system_id_optimal_input_cond( + nonlin_plant_3, p_opt = system_id_optimal_inp_cond( input_gas, ne_uniform, tt, 3, inp_cond, Dict{Symbol, Any}(:p => -0.2); inp_offset=gas_offset, inp_factor=gas_factor, out_offset=ne_offset, out_factor=ne_factor, @@ -593,10 +546,279 @@ if args["system_id"] nonlin_plant_3, input_gas; inp_offset=gas_offset, inp_factor=gas_factor, out_offset=ne_offset, out_factor=ne_factor, - input_cond=inp_cond, input_cond_kwargs=p_opt) + inp_cond=inp_cond, inp_cond_kwargs=p_opt) + + @test sqrt(Statistics.mean((lin_out - ne_uniform) .^ 2)) < 1.2e18 + + @test sqrt(Statistics.mean((nonlin_out - ne_uniform) .^ 2)) < 0.4e18 + end +end + +if isempty(ARGS) || "state" in ARGS + @testset "state_pred" begin + nu = rand(1:10, 1)[1] + ny = rand(1:nu, 1)[1] + nstates = rand(1:10, 1)[1] + hh = 20 + rnd_model = ssrand(ny, nu, nstates; + proper=false, + stable=true, + Ts=0.001, + ) + LL, MM, NN, OO = SOLPS2ctrl._calculate_LMNO(rnd_model, hh) + + x0 = rand(Float64, size(rnd_model.A, 1)) + test_u = randn((size(rnd_model.B, 2), hh)) + test_y, _, test_x, _ = lsim(rnd_model, test_u; x0) + final_x = rnd_model.A * test_x[:, end] + rnd_model.B * test_u[:, end] + + test_U = zeros(size(rnd_model.B, 2) * hh) + test_Y = zeros(size(rnd_model.C, 1) * hh) + for i ∈ 1:hh + test_U[(size(rnd_model.B, 2)*(i-1)+1):(size(rnd_model.B, 2)*i)] = + test_u[:, i] + test_Y[(size(rnd_model.C, 1)*(i-1)+1):(size(rnd_model.C, 1)*i)] = + test_y[:, i] + end + test_est_Y = LL * x0 + MM * test_U + test_est_x = NN * x0 + OO * test_U + + # println(sqrt(Statistics.mean((test_est_Y - test_Y) .^ 2))) + # println(sqrt(Statistics.mean((test_est_x - final_x) .^ 2))) + @test isapprox(test_est_Y, test_Y) + @test isapprox(test_est_x, final_x) + + noise_scale = 1e-3 + + set_u_off = randn() * noise_scale + test_U_mod = test_U .+ set_u_off - @test sqrt(mean((lin_out - ne_uniform) .^ 2)) < 1.2e18 + set_y_off = randn() * noise_scale + test_Y_mod = test_Y .+ set_y_off + + YY2x, UU2x = state_prediction_matrices(rnd_model, hh) + test_est_x2 = YY2x * test_Y_mod + UU2x * test_U_mod + # println(sqrt(Statistics.mean((test_est_x2 - final_x) .^ 2))) + @test isapprox(test_est_x2, final_x; rtol=10 * noise_scale) + end +end + +if isempty(ARGS) || "controller" in ARGS + @testset "controller" begin + # Random 2-pole plant model with resonance between 100 and 200 Hz + # and Q value between 1 to 6 and a gain of 3.0 + s = tf('s') + imp = rand(200.0:0.001:300.0) + rep = rand(-200:0.001:-50) + abs2 = rep^2 + imp^2 + den = s^2 - 2 * rep * s + abs2 + Ts = 0.001 # Sampling period of discrete system + plant_model = c2d(ss(abs2 * 3 / den), Ts) + delay_steps = 100 + delay_model = delay(delay_steps * Ts, Ts) + + # Define an actuator for the plant model + # Actuator must be a mutable struct and daughter of Actuator type + mutable struct TestAct <: Actuator + latency::DelayedActuator + bounds::Tuple{Vector{Float64}, Vector{Float64}} + gain::Float64 + end + # And all actuators must be callable + # Here we make this non-linear actuator where the gain is shaped as arctan + # as a function of inp and gets clipped at bounds. + # (ta::TestAct)(inp::Vector{Float64}) = ta.latency(inp .* ta.gain) + function (ta::TestAct)(inp::Vector{Float64}) + w = 0.125 .* (ta.bounds[2] - ta.bounds[1]) + return ta.latency( + clamp( + ta.gain .* w .* atan.(inp ./ w), + ta.bounds[1], + ta.bounds[2], + ), + ) + end + # Now create an instance of this actuator with gain 5 and delay of 10 time steps + act = TestAct(DelayedActuator(delay_steps), ([-0.3], [0.3]), 5.0) + + test_act = deepcopy(act) + test_range = -0.1:0.001:0.1 + test_inp = [collect(test_range); zeros(test_act.latency.delay)] + test_inp = Matrix(test_inp') + test_out = zeros(size(test_inp)) + for ii ∈ eachindex(test_inp[1, :]) + test_out[:, ii] = test_act(test_inp[:, ii]) + end + plot( + test_inp[1:length(test_range)], test_out[(test_act.latency.delay+1):end]; + xlabel="Input Command to Actuator", + ylabel="Output from Actuator", + linewidth=2, + legend=false, + ) + savefig("$(@__DIR__)/Test_Actuator_Function.pdf") + + # Finally, design a linear controller using pid + lc = LinearController(c2d(ss(pid(0.1, 0.1 / 10)), Ts), zeros(Float64, 1)) + + # Reduce controller gain to ensure stability taking into account the delay + w = collect(logrange(0.1, 1e3, 400)) + pm = -200 + gm = -1 + while pm < 30 || gm < 3 + wgm, gm, wpm, pm = + margin(plant_model * act.gain * delay_model * lc.ctrl_ss, w) + pm = pm[1, 1, 1] + gm = gm[1, 1, 1] + if pm < 30 || gm < 3 + lc.ctrl_ss *= 0.9 # Reduce gain slightly + end + 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) + + # 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) + pm = pm[1, 1, 1] + gm = gm[1, 1, 1] + if pm < 30 || gm < 3 + pvlc.ctrl_ss *= 0.9 + end + 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, + ) + + # Set a target waveform + tt = collect(0:Ts:2) + set_points_tt = [0.0, 0.1, 0.5, 0.7, 1.3, 1.5, 1.7, 2.0] + set_points = [0.0, 0.0, 1.0, 1.0, -0.5, -0.5, 0.0, 0.0] + target = Matrix{Float64}( + Interpolations.linear_interpolation(set_points_tt, set_points).(tt)', + ) + + # Create some noise + noise_std = 1e-2 + noise_plant_inp = + randn(Float64, (size(plant_model.B, 2), size(target, 2))) * noise_std + noise_plant_out = + randn(Float64, (size(plant_model.C, 1), size(target, 2))) * noise_std + noise_ctrl_out = + randn(Float64, (size(plant_model.B, 2), size(target, 2))) * noise_std + + # Run closed loop simulation + res = Dict() + print("Simulation time $(length(target)) steps, PI: ") + @time res["PI"] = run_closed_loop_sim( + plant_model, 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; + 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; + noise_plant_inp, noise_plant_out, noise_ctrl_out, + ) + println() + + p1 = plot( + tt, res["PI"][:target][1, :]; + label="Target", linewidth=2, linestyle=:dash, color=:black, + ) + plot!( + tt, res["PI"][:plant_out][1, :]; + label="PI", linewidth=2, color=:deepskyblue3, + ) + plot!( + tt, res["PVLC"][:plant_out][1, :]; + label="PVLC", ylabel="Plant Output", linewidth=2, color=:orange, + xformatter=_ -> "", topmargin=10Plots.mm, + ) + plot!( + tt, res["MPC"][:plant_out][1, :]; + label="MPC", ylabel="Plant Output", xformatter=_ -> "", + linewidth=2, color=:darkgreen, + ) + p2 = plot( + tt, res["PI"][:plant_inp][1, :]; + label="PI", linewidth=2, color=:deepskyblue3, + ) + plot!( + tt, res["PVLC"][:plant_inp][1, :]; + label="PVLC", ylabel="Plant Input", xformatter=_ -> "", + linewidth=2, color=:orange, + ) + plot!( + tt, res["MPC"][:plant_inp][1, :]; + label="MPC", ylabel="Plant Input", xformatter=_ -> "", + linewidth=2, color=:darkgreen, + legend=false, + ) + p3 = plot( + tt, res["PI"][:ctrl_out][1, :]; + label="PI", linewidth=2, color=:deepskyblue3, + ) + plot!( + tt, res["PVLC"][:ctrl_out][1, :]; + label="PVLC", ylabel="Controller\nOutput", xlabel="Time / s", + linewidth=2, color=:orange, + ) + plot!( + tt, res["MPC"][:ctrl_out][1, :]; + label="MPC", ylabel="Controller\nOutput", xlabel="Time / s", + linewidth=2, color=:darkgreen, + legend=false, + ) + l = @layout [a{0.5h}; b{0.25h}; c{0.25h}] + plot( + p1, p2, p3; + layout=l, + suptitle=( + "Closed Loop Simulation Results\n" * + "Actuator has delay of $(act.latency.delay * Ts)s." + ), + ) + + savefig("$(@__DIR__)/Closed_Loop_Sim_Results.pdf") + + residual_LC = sqrt( + Statistics.mean( + (res["PI"][:target][1, :] .- res["PI"][:plant_out][1, :]) .^ 2, + ), + ) + + residual_PVLC = sqrt( + Statistics.mean( + (res["PVLC"][:target][1, :] .- res["PVLC"][:plant_out][1, :]) .^ 2, + ), + ) + + residual_MPC = sqrt( + Statistics.mean( + (res["MPC"][:target][1, :] .- res["MPC"][:plant_out][1, :]) .^ 2, + ), + ) - @test sqrt(mean((nonlin_out - ne_uniform) .^ 2)) < 0.4e18 + @test residual_PVLC < residual_LC + @test residual_MPC < residual_PVLC end end diff --git a/test/test.jl b/test/test.jl new file mode 100644 index 0000000..9c3b013 --- /dev/null +++ b/test/test.jl @@ -0,0 +1,88 @@ +using Pkg +using Printf + +pkg_name = "SOLPS2ctrl" + +valid_arguments = [ + ("units", "Test unit conversion utilities"), + ("core", "Test core profile extension"), + ("edge", "Test edge profile extension"), + ("heavy", "Test heavy utilities"), + ("repair", "Test repair_eq"), + ("geqdsk", "Test geqdsk_to_imas"), + ("prep", "Test preparation"), + ("sysid", "Test system id"), + ("state", "Test state prediction"), + ("controller", "Test linear and PVLC controllers"), + ("h", "Show this help message and exit"), + ("help", "Show this help message and exit"), +] + +function get_options_string( + valid_arguments::Vector{Tuple{String, String}}; + indent::Int=4, +) + keys_arr = [k for (k, v) ∈ valid_arguments] + values_arr = [v for (k, v) ∈ valid_arguments] + + # Determine the maximum width of each column (keys and values) + max_width_keys = maximum(length.(string.(keys_arr))) + max_width_values = maximum(length.(string.(values_arr))) + + # Create the format string with indentation + format_str = repeat(" ", indent) * "%-$(max_width_keys)s %-$(max_width_values)s\n" + + # Create a Format object + fmt = Printf.Format(format_str) + + # Build the combined string + combined_string = "" + for (key, value) ∈ valid_arguments + combined_string *= Printf.format(fmt, string(key), string(value)) + end + + return combined_string +end + +function check_args( + args::Vector{String}, + valid_arguments::Vector{Tuple{String, String}}, +) + valid_keys = Set(t[1] for t ∈ valid_arguments) + + any_valid = false + for arg ∈ args + if arg in valid_keys + any_valid = true + else + println("Error: Invalid argument found: \"", arg, "\"") + println() + return false + end + end + + if !any_valid && !isempty(args) + println("Error: No valid arguments found.") + return false + end + return true +end + +usage_string = "Usage (from inside $(pkg_name).jl): \njulia --project test/test.jl " +usage_string *= "[" * join([k for (k, v) ∈ valid_arguments], "] [") * "]\n\n" +usage_string *= "Run tests. Default is all tests.\n\nOptional arguments:\n" +usage_string *= get_options_string(valid_arguments) + +if "h" in ARGS || "help" in ARGS || !check_args(ARGS, valid_arguments) + println(usage_string) + exit() +end + +try + Pkg.test("$(pkg_name)"; test_args=ARGS) +catch e + println("Error occured:") + println(e) + println() + println(usage_string) +end