From 3cfc6e5f49d30ae25c20022df3098c5d268a586e Mon Sep 17 00:00:00 2001 From: Anchal Gupta Date: Fri, 20 Jun 2025 16:57:27 -0700 Subject: [PATCH 01/12] Adding state prediction algorithm for PVLC --- docs/src/index.md | 139 +++++++++++++++++++++++++++++++++++++++++++++ src/SOLPS2ctrl.jl | 1 + src/controllers.jl | 84 +++++++++++++++++++++++++++ test/runtests.jl | 53 +++++++++++++++++ 4 files changed, 277 insertions(+) create mode 100644 src/controllers.jl diff --git a/docs/src/index.md b/docs/src/index.md index 6f76813..ccfbc37 100644 --- a/docs/src/index.md +++ b/docs/src/index.md @@ -81,6 +81,145 @@ system_id_optimal_input_cond model_evolve ``` +## Controllers + +```@docs +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} +``` + ## 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..ec4f946 --- /dev/null +++ b/src/controllers.jl @@ -0,0 +1,84 @@ +import ControlSystemIdentification: PredictionStateSpace +import ControlSystemsBase: StateSpace +import LinearAlgebra: pinv +# import ControlSystemsBase: lsim, ss, pid, d2c_exact, d2c, StateSpace + +export lsim_step, state_prediction_matrices + +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 + +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 diff --git a/test/runtests.jl b/test/runtests.jl index 6060d7d..ef0f1ce 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -11,6 +11,7 @@ using ArgParse: ArgParse using IMASggd: IMASggd, get_grid_subset using DelimitedFiles: readdlm using Statistics: mean +using ControlSystemsBase: lsim, ssrand function parse_commandline() # Define newARGS = ["--yourflag"] to run only tests on your flags when including runtests.jl @@ -42,6 +43,9 @@ function parse_commandline() ["--system_id"], Dict(:help => "Test only system identification", :action => :store_true), + ["--state_pred"], + Dict(:help => "Test only state prediction", + :action => :store_true), ) args = ArgParse.parse_args(localARGS, s) if !any(values(args)) # If no flags are set, run all tests @@ -600,3 +604,52 @@ if args["system_id"] @test sqrt(mean((nonlin_out - ne_uniform) .^ 2)) < 0.4e18 end end + +if args["state_pred"] + @testset "state_pred" begin + nu = rand(1:10, 1)[1] + ny = rand(1:nu, 1)[1] + nstates = rand(1:10, 1)[1] + hh = rand(nstates:20, 1)[1] + 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(mean((test_est_Y - test_Y) .^ 2))) + println(sqrt(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 + + 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(mean((test_est_x2 - final_x) .^ 2))) + @test isapprox(test_est_x2, final_x; rtol=noise_scale) + end +end From 4f19c38b7114d18578a3170e2a24a56c70a9f546 Mon Sep 17 00:00:00 2001 From: Anchal Gupta Date: Mon, 23 Jun 2025 13:38:47 -0700 Subject: [PATCH 02/12] Adding basic closed loop simulation infra Added abstract types Actuators and Controllers which must be used to defined mutable structures for different actuators and controllers that should be callable, so that they get caled in run_closed_lim_sim. This has not been tested yet. Tests and further debuggin would happen in next commits. --- Project.toml | 2 + src/controllers.jl | 193 ++++++++++++++++++++++++++++++++++++++++++++- src/system_id.jl | 122 +++++++++++++--------------- test/runtests.jl | 4 +- 4 files changed, 250 insertions(+), 71 deletions(-) diff --git a/Project.toml b/Project.toml index 74f7c26..811459e 100644 --- a/Project.toml +++ b/Project.toml @@ -8,6 +8,7 @@ ArgParse = "c7e460c6-2fb9-53a9-8c5b-16f535851c63" Contour = "d38c429a-6771-53c6-b99e-75d170b6e991" ControlSystemIdentification = "3abffc1c-5106-53b7-b354-a47bfc086282" ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e" +DataStructures = "864edb3b-99cc-5e75-8d2d-829cb0a9cfe8" DelimitedFiles = "8bb1440f-4735-579b-a4ab-409b98df4dab" EFIT = "cda752c5-6b03-55a3-9e33-132a441b0c17" IMAS = "13ead8c1-b7d1-41bb-a6d0-5b8b65ed587a" @@ -29,6 +30,7 @@ ArgParse = "1" Contour = "0.6" ControlSystemIdentification = "2" ControlSystemsBase = "1" +DataStructures = "0.18.22" DelimitedFiles = "1" EFIT = "1" IMAS = "1, 2, 3, 4, 5" diff --git a/src/controllers.jl b/src/controllers.jl index ec4f946..dcb2a85 100644 --- a/src/controllers.jl +++ b/src/controllers.jl @@ -1,6 +1,7 @@ import ControlSystemIdentification: PredictionStateSpace -import ControlSystemsBase: StateSpace +import ControlSystemsBase: StateSpace, Discrete import LinearAlgebra: pinv +using DataStructures: Queue, enqueue!, dequeue! # import ControlSystemsBase: lsim, ss, pid, d2c_exact, d2c, StateSpace export lsim_step, state_prediction_matrices @@ -14,6 +15,24 @@ function lsim_step( return y, x end +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}(), +)::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, u; 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), @@ -82,3 +101,175 @@ function state_prediction_matrices( U2x = (O - Y2x * M) return Y2x, U2x end + +# function latency_jump_part( +# future_flow_rates::Vector{Float64}, ssm::PredictionStateSpace, +# fut_x0::Vector{Float64}; +# gas_offset::Float64=gas_offset, gas_factor::Float64=gas_factor, +# boltzmann_k::Float64=1.380649e-23, Temp::Float64=300.0, +# )::Tuple{Float64, Vector{Float64}} +# fut_gas_atomps = gas_SI_to_aps(future_flow_rates; boltzmann_k, Temp) +# fut_gas_atomsps_scaled = zeros((1, length(fut_gas_atomps))) +# fut_gas_atomsps_scaled[1, :] = +# gas_offset_scale(fut_gas_atomps; gas_offset, gas_factor) +# y_int, _, post_lat_x0, _ = lsim(ssm, fut_gas_atomsps_scaled; x0=fut_x0) +# fut_meas_hf = y_int[1, end] +# return fut_meas_hf, post_lat_x0[:, end] +# end + +abstract type Actuator end + +mutable struct DelayedActuator{U} <: Actuator + delay::Int + default_output::U + buffer::Queue{U} + function DelayedActuator( + delay::Int; + default_output::T=0.0, + ) where {T <: Union{Float64, Vector{Float64}}} + return new{T}(delay, default_output, Queue{T}(delay)) + end +end + +function (da::DelayedActuator)(inp::Union{Float64, Vector{Float64}}) + enqueue!(da.buffer, inp) + if length(da.buffer) <= da.delay + return da.default_output + else + return dequeue!(da.buffer) + end +end + +get_T(::DelayedActuator{T}) where {T} = T + +function has_delay(act::Actuator) + for fieldname ∈ fieldnames(act) + if isa(fieldtype(act, fieldname), DelayedActuator) + return true + end + end + return false +end + +function get_future_inp(act::Actuator) + for fieldname ∈ fieldnames(act) + if isa(fieldtype(act, fieldname), DelayedActuator) + return collect(getfield(act, fieldname).buffer) + end + end + return Matrix{Float64}(undef, 0, 0) +end + +abstract type Controller end + +mutable struct linear_controller <: Controller + ctrl_ss::StateSpace{Discrete} + ctrl_x0::Vector{Float64} +end + +function (lc::linear_controller)(; + 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 + +mutable struct PVLC <: Controller + ctrl_ss::StateSpace{Discrete} + ctrl_x0::Vector{Float64} + h::Int + plant_model::Union{PredictionStateSpace, StateSpace} + Y2x::Matrix{Float64} + U2x::Matrix{Float64} + function PVLC( + ctrl_ss::StateSpace{Discrete}, + ctrl_x0::Vector{Float64}, + plant_model::Union{PredictionStateSpace, StateSpace}, + h::Int=size(sys.A, 1), + ) + Y2x, U2x = state_prediction_matrices(plant_model, h) + return new(ctrl_ss, ctrl_x0, plant_model, h, Y2x, U2x) + end +end + +function (pvlc::PVLC)(; + ii::Int, + target::Matrix{Float64}, + plant_inp::Matrix{Float64}, + plant_out::Matrix{Float64}, + future_inp::Matrix{Float64}, + 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(ctrl_ss.C, 1)) + if length(plant_inp) >= size(pvlc.U2x, 2) + # Gather history of inputs and outputs + U = vec(plant_inp[:, (end-pvlc.h+1):pvlc.h]) + Y = vec(plant_out[:, (end-pvlc.h+1):pvlc.h]) + # Estimate current state vector of plant model + est_x = pvlc.Y2x * Y .+ U2x * U + # Evolve into the future for the delay in actuator + future_out, _ = model_evolve( + plant_model, future_inp; + x0=est_x, + inp_offset, inp_factor, out_offset, out_factor, + ) + # Estimate the error signal in future + err = target[:, ii+size(future_inp, 2)] - 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 run_closed_loop_sim( + plant_model::Union{PredictionStateSpace, StateSpace}, + act::Actuator, + ctrl::Controller, + target::Union{Vector{Float64}, 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_model.B, 2), length(target)), + ), + ctrl_start_ind::Int=1, +) where {T} + if isa(target, Vector) + if size(plant_model.C, 1) == 1 + target = Matrix(target') + else + error("Plant has multiple outputs, target must be Matrix") + end + end + ctrl_out = zeros(Float64, size(inp_feedforward, 1)) + plant_inp = zeros(Float64, (size(plant_model.B, 2), length(target))) + plant_out = zeros(Float64, (size(plant_model.C, 1), length(target))) + x0 = + inv(Matrix{Float64}(I, size(plant_model.A)...) - plant_model.A) * + plant_model.B * inp_feedforward[:, 1] + for ii ∈ eachindex(target) + plant_inp[:, ii] = act(ctrl_out .+ inp_feedforward[:, ii]) + plant_out[:, ii], _ = model_step( + plant_model, plant_inp[:, ii]; + x0, + inp_cond, inp_offset, inp_factor, out_offset, out_factor, + inp_cond_kwargs, + ) + # err = target[:, ii] .- plant_out[:, ii] + future_inp = get_future_inp(act) + if ii >= ctrl_start_ind + ctrl_out = ctrl(; + ii, target, plant_inp, plant_out, future_inp, + inp_offset, inp_factor, out_offset, out_factor, + ) + end + end +end diff --git a/src/system_id.jl b/src/system_id.jl index 8758f76..7e97bb6 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,7 +158,9 @@ 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 @@ -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/runtests.jl b/test/runtests.jl index ef0f1ce..45c37b4 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -580,7 +580,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, @@ -597,7 +597,7 @@ 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(mean((lin_out - ne_uniform) .^ 2)) < 1.2e18 From efe3d02ec34302cf892916df33e98d94ccc2d364 Mon Sep 17 00:00:00 2001 From: Anchal Gupta Date: Tue, 24 Jun 2025 12:17:51 -0700 Subject: [PATCH 03/12] Added test for linear controllers and docs --- .github/workflows/test.yml | 3 + docs/src/index.md | 31 +++++- src/controllers.jl | 207 +++++++++++++++++++++++++++---------- test/.gitignore | 1 + test/runtests.jl | 86 ++++++++++++++- 5 files changed, 272 insertions(+), 56 deletions(-) create mode 100644 test/.gitignore 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/docs/src/index.md b/docs/src/index.md index ccfbc37..3bfe0e5 100644 --- a/docs/src/index.md +++ b/docs/src/index.md @@ -77,12 +77,34 @@ 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 +LinearController +``` + ```@docs state_prediction_matrices ``` @@ -220,6 +242,13 @@ 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} ``` +## Closed loop simulations + +```@docs +lsim_step +model_step +``` + ## Unit conversion utilities ```@docs diff --git a/src/controllers.jl b/src/controllers.jl index dcb2a85..4a3a41b 100644 --- a/src/controllers.jl +++ b/src/controllers.jl @@ -4,8 +4,17 @@ import LinearAlgebra: pinv using DataStructures: Queue, enqueue!, dequeue! # import ControlSystemsBase: lsim, ss, pid, d2c_exact, d2c, StateSpace -export lsim_step, state_prediction_matrices +export lsim_step, model_step, state_prediction_matrices, Actuator, DelayedActuator, + Controller, LinearController, PVLC, 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)), @@ -15,6 +24,19 @@ function lsim_step( 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}; @@ -23,12 +45,12 @@ function model_step( 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}(), -)::Vector{Float64} where {T} +)::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, u; x0) + 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 @@ -102,72 +124,132 @@ function state_prediction_matrices( return Y2x, U2x end -# function latency_jump_part( -# future_flow_rates::Vector{Float64}, ssm::PredictionStateSpace, -# fut_x0::Vector{Float64}; -# gas_offset::Float64=gas_offset, gas_factor::Float64=gas_factor, -# boltzmann_k::Float64=1.380649e-23, Temp::Float64=300.0, -# )::Tuple{Float64, Vector{Float64}} -# fut_gas_atomps = gas_SI_to_aps(future_flow_rates; boltzmann_k, Temp) -# fut_gas_atomsps_scaled = zeros((1, length(fut_gas_atomps))) -# fut_gas_atomsps_scaled[1, :] = -# gas_offset_scale(fut_gas_atomps; gas_offset, gas_factor) -# y_int, _, post_lat_x0, _ = lsim(ssm, fut_gas_atomsps_scaled; x0=fut_x0) -# fut_meas_hf = y_int[1, end] -# return fut_meas_hf, post_lat_x0[:, end] -# 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 - default_output::U buffer::Queue{U} function DelayedActuator( delay::Int; - default_output::T=0.0, - ) where {T <: Union{Float64, Vector{Float64}}} - return new{T}(delay, default_output, Queue{T}(delay)) + 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}}) +function (da::DelayedActuator)(inp::Union{Float64, Vector{Float64}})::Vector{Float64} enqueue!(da.buffer, inp) - if length(da.buffer) <= da.delay - return da.default_output - else - return dequeue!(da.buffer) - end -end - -get_T(::DelayedActuator{T}) where {T} = T - -function has_delay(act::Actuator) - for fieldname ∈ fieldnames(act) - if isa(fieldtype(act, fieldname), DelayedActuator) - return true - end - end - return false + return dequeue!(da.buffer) end function get_future_inp(act::Actuator) - for fieldname ∈ fieldnames(act) - if isa(fieldtype(act, fieldname), DelayedActuator) - return collect(getfield(act, fieldname).buffer) + 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) = collect(act.buffer) + +""" + 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) + - `future_inp::Matrix{Float64}`: Upcoming future known inputs to plant (for delayed actuators) (No. of outputs x Time steps) + - `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 -mutable struct linear_controller <: Controller - ctrl_ss::StateSpace{Discrete} +""" + 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 stoes 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::linear_controller)(; +function (lc::LinearController)(; ii::Int, target::Matrix{Float64}, plant_out::Matrix{Float64}, @@ -179,18 +261,18 @@ function (lc::linear_controller)(; end mutable struct PVLC <: Controller - ctrl_ss::StateSpace{Discrete} + ctrl_ss::StateSpace{TE} where {TE <: Discrete} ctrl_x0::Vector{Float64} h::Int plant_model::Union{PredictionStateSpace, StateSpace} Y2x::Matrix{Float64} U2x::Matrix{Float64} function PVLC( - ctrl_ss::StateSpace{Discrete}, + ctrl_ss::StateSpace{TE}, ctrl_x0::Vector{Float64}, plant_model::Union{PredictionStateSpace, StateSpace}, h::Int=size(sys.A, 1), - ) + ) where {TE <: Discrete} Y2x, U2x = state_prediction_matrices(plant_model, h) return new(ctrl_ss, ctrl_x0, plant_model, h, Y2x, U2x) end @@ -228,7 +310,7 @@ function (pvlc::PVLC)(; end function run_closed_loop_sim( - plant_model::Union{PredictionStateSpace, StateSpace}, + plant_model::Union{PredictionStateSpace{TE}, StateSpace{TE}}, act::Actuator, ctrl::Controller, target::Union{Vector{Float64}, Matrix{Float64}}; @@ -241,7 +323,7 @@ function run_closed_loop_sim( (size(plant_model.B, 2), length(target)), ), ctrl_start_ind::Int=1, -) where {T} +) where {T, TE <: Discrete} if isa(target, Vector) if size(plant_model.C, 1) == 1 target = Matrix(target') @@ -249,15 +331,15 @@ function run_closed_loop_sim( error("Plant has multiple outputs, target must be Matrix") end end - ctrl_out = zeros(Float64, size(inp_feedforward, 1)) + ctrl_out = zeros(Float64, size(inp_feedforward)) plant_inp = zeros(Float64, (size(plant_model.B, 2), length(target))) plant_out = zeros(Float64, (size(plant_model.C, 1), length(target))) x0 = inv(Matrix{Float64}(I, size(plant_model.A)...) - plant_model.A) * plant_model.B * inp_feedforward[:, 1] for ii ∈ eachindex(target) - plant_inp[:, ii] = act(ctrl_out .+ inp_feedforward[:, ii]) - plant_out[:, ii], _ = model_step( + plant_inp[:, ii] = act(ctrl_out[:, ii] .+ inp_feedforward[:, ii]) + plant_out[:, ii], x0 = model_step( plant_model, plant_inp[:, ii]; x0, inp_cond, inp_offset, inp_factor, out_offset, out_factor, @@ -265,11 +347,28 @@ function run_closed_loop_sim( ) # err = target[:, ii] .- plant_out[:, ii] future_inp = get_future_inp(act) - if ii >= ctrl_start_ind - ctrl_out = ctrl(; + if ii >= ctrl_start_ind && ii < length(target) + ctrl_out[:, ii+1] = ctrl(; ii, target, plant_inp, plant_out, future_inp, inp_offset, inp_factor, out_offset, out_factor, ) end end + return Dict( + :plant_model => plant_model, + :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, + ) 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/runtests.jl b/test/runtests.jl index 45c37b4..9fd04b6 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -11,7 +11,7 @@ using ArgParse: ArgParse using IMASggd: IMASggd, get_grid_subset using DelimitedFiles: readdlm using Statistics: mean -using ControlSystemsBase: lsim, ssrand +using ControlSystemsBase: lsim, ssrand, tf, c2d, ss, pid, margin function parse_commandline() # Define newARGS = ["--yourflag"] to run only tests on your flags when including runtests.jl @@ -46,6 +46,9 @@ function parse_commandline() ["--state_pred"], Dict(:help => "Test only state prediction", :action => :store_true), + ["--controller"], + Dict(:help => "Test only controllers", + :action => :store_true), ) args = ArgParse.parse_args(localARGS, s) if !any(values(args)) # If no flags are set, run all tests @@ -653,3 +656,84 @@ if args["state_pred"] @test isapprox(test_est_x2, final_x; rtol=noise_scale) end end + +if args["controller"] + @testset "controller" begin + # Random 2-pole plant model with resonance between 100 and 200 Hz + # and Q value between 10 to 30 and a gain of 3.0 + s = tf('s') + imp = rand(200.0:0.001:300.0) + rep = rand(-20:0.001:-10) + 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) + + # Define an actuator for the plant model + # Actuator must be a mutable struct and daughter of Actuator type + mutable struct TestAct <: Actuator + latency::DelayedActuator + gain::Float64 + end + # And all actuators must be callable + # Here we make this a simple gain actuator with some latency + (ta::TestAct)(inp::Vector{Float64}) = ta.latency(inp .* ta.gain) + # Now create an instance of this actuator with gain 5 and delay of 10 time steps + act = TestAct(DelayedActuator(10), 5.0) + + # 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 + w = collect(logrange(0.1, 1e3, 400)) + pm = -200 + gm = -1 + while pm < 45 || gm < 3 + wgm, gm, wpm, pm = margin(plant_model * act.gain * lc.ctrl_ss, w) + pm = pm[1, 1, 1] + gm = gm[1, 1, 1] + if pm < 45 || gm < 3 + lc.ctrl_ss *= 0.9 # Reduce gain slightly + end + end + + # 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 = Interpolations.linear_interpolation(set_points_tt, set_points).(tt) + + # Run closed loop simulation + res = run_closed_loop_sim(plant_model, act, lc, target) + + p1 = plot(tt, res[:target][1, :]; color=:black, label="Target", linestyle=:dash) + plot!( + tt, res[:plant_out][1, :]; + linewidth=2, color=:deepskyblue3, + label="PI", ylabel="Plant Output", xformatter=_ -> "", topmargin=10Plots.mm, + ) + p2 = plot( + tt, res[:plant_inp][1, :]; + linewidth=2, color=:deepskyblue3, + label="PI", ylabel="Plant Input", xformatter=_ -> "", + ) + p3 = plot( + tt, res[:ctrl_out][1, :]; + linewidth=2, color=:deepskyblue3, + label="PI", ylabel="Controller\nOutput", xlabel="Time / s", + ) + 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") + + @test mean((res[:target][1, :] .- res[:plant_out][1, :]) .^ 2) < 0.1 + end +end From 6c3756d34030b2373b1a5ba3f84ffa2cdafaf346 Mon Sep 17 00:00:00 2001 From: Anchal Gupta Date: Tue, 24 Jun 2025 17:58:32 -0700 Subject: [PATCH 04/12] Added test for PVLC and docs --- docs/src/index.md | 6 ++- src/controllers.jl | 85 +++++++++++++++++++++++++++++++++++++------ src/system_id.jl | 2 +- test/runtests.jl | 91 ++++++++++++++++++++++++++++++++++------------ 4 files changed, 147 insertions(+), 37 deletions(-) diff --git a/docs/src/index.md b/docs/src/index.md index 3bfe0e5..091edbd 100644 --- a/docs/src/index.md +++ b/docs/src/index.md @@ -105,11 +105,14 @@ Controller LinearController ``` +### Predicted Variable Linear Controller + ```@docs +PVLC state_prediction_matrices ``` -### State Prediction Matrix Algebra +#### State Prediction Matrix Algebra At step k: @@ -247,6 +250,7 @@ x_{k+1} = \mathcal{N} \mathcal{L}^{-1} (\vec{Y} - \mathcal{M} \vec{U}) + \mathca ```@docs lsim_step model_step +run_closed_loop_sim ``` ## Unit conversion utilities diff --git a/src/controllers.jl b/src/controllers.jl index 4a3a41b..de598ff 100644 --- a/src/controllers.jl +++ b/src/controllers.jl @@ -199,7 +199,7 @@ function get_future_inp(act::Actuator) return Matrix{Float64}(undef, 0, 0) end -get_future_inp(act::DelayedActuator) = collect(act.buffer) +get_future_inp(act::DelayedActuator) = stack(act.buffer) """ Controller @@ -231,7 +231,7 @@ abstract type Controller end 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 stoes the state vector for the state space model as `ctrl_x0::Vector{Float64}`. +It also stores the state vector for the state space model as `ctrl_x0::Vector{Float64}`. It's call signature is: (lc::LinearController)(; @@ -260,18 +260,54 @@ function (lc::LinearController)(; 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_model`, 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_model::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}, + future_inp::Matrix{Float64}, + 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} - h::Int plant_model::Union{PredictionStateSpace, StateSpace} + h::Int Y2x::Matrix{Float64} U2x::Matrix{Float64} function PVLC( ctrl_ss::StateSpace{TE}, ctrl_x0::Vector{Float64}, plant_model::Union{PredictionStateSpace, StateSpace}, - h::Int=size(sys.A, 1), + h::Int, ) where {TE <: Discrete} Y2x, U2x = state_prediction_matrices(plant_model, h) return new(ctrl_ss, ctrl_x0, plant_model, h, Y2x, U2x) @@ -288,27 +324,52 @@ function (pvlc::PVLC)(; out_offset::Float64=0.0, out_factor::Float64=1.0, kwargs..., )::Vector{Float64} - ctrl_out = zeros(Float64, size(ctrl_ss.C, 1)) - if length(plant_inp) >= size(pvlc.U2x, 2) + ctrl_out = zeros(Float64, size(pvlc.ctrl_ss.C, 1)) + 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[:, (end-pvlc.h+1):pvlc.h]) - Y = vec(plant_out[:, (end-pvlc.h+1):pvlc.h]) + 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 .+ U2x * U + est_x = pvlc.Y2x * Y .+ pvlc.U2x * U # Evolve into the future for the delay in actuator - future_out, _ = model_evolve( - plant_model, future_inp; + future_out = model_evolve( + pvlc.plant_model, future_inp; x0=est_x, inp_offset, inp_factor, out_offset, out_factor, ) # Estimate the error signal in future - err = target[:, ii+size(future_inp, 2)] - future_out[:, end] + 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 +""" + run_closed_loop_sim( + plant_model::Union{PredictionStateSpace{TE}, StateSpace{TE}}, + act::Actuator, + ctrl::Controller, + target::Union{Vector{Float64}, 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_model.B, 2), length(target)), + ), + ctrl_start_ind::Int=1, + ) where {T, TE <: Discrete} + +Generic function to run closed loop simulations with provided `plant_model`, 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. +""" function run_closed_loop_sim( plant_model::Union{PredictionStateSpace{TE}, StateSpace{TE}}, act::Actuator, diff --git a/src/system_id.jl b/src/system_id.jl index 7e97bb6..67e2d29 100644 --- a/src/system_id.jl +++ b/src/system_id.jl @@ -163,7 +163,7 @@ function model_evolve( 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 diff --git a/test/runtests.jl b/test/runtests.jl index 9fd04b6..1d031bb 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -11,7 +11,7 @@ using ArgParse: ArgParse using IMASggd: IMASggd, get_grid_subset using DelimitedFiles: readdlm using Statistics: mean -using ControlSystemsBase: lsim, ssrand, tf, c2d, ss, pid, margin +using ControlSystemsBase: lsim, ssrand, delay, tf, c2d, ss, pid, margin function parse_commandline() # Define newARGS = ["--yourflag"] to run only tests on your flags when including runtests.jl @@ -637,8 +637,8 @@ if args["state_pred"] test_est_Y = LL * x0 + MM * test_U test_est_x = NN * x0 + OO * test_U - println(sqrt(mean((test_est_Y - test_Y) .^ 2))) - println(sqrt(mean((test_est_x - final_x) .^ 2))) + # println(sqrt(mean((test_est_Y - test_Y) .^ 2))) + # println(sqrt(mean((test_est_x - final_x) .^ 2))) @test isapprox(test_est_Y, test_Y) @test isapprox(test_est_x, final_x) @@ -652,7 +652,7 @@ if args["state_pred"] YY2x, UU2x = state_prediction_matrices(rnd_model, hh) test_est_x2 = YY2x * test_Y_mod + UU2x * test_U_mod - println(sqrt(mean((test_est_x2 - final_x) .^ 2))) + # println(sqrt(mean((test_est_x2 - final_x) .^ 2))) @test isapprox(test_est_x2, final_x; rtol=noise_scale) end end @@ -660,14 +660,16 @@ end if args["controller"] @testset "controller" begin # Random 2-pole plant model with resonance between 100 and 200 Hz - # and Q value between 10 to 30 and a gain of 3.0 + # 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(-20:0.001:-10) + 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 @@ -679,24 +681,42 @@ if args["controller"] # Here we make this a simple gain actuator with some latency (ta::TestAct)(inp::Vector{Float64}) = ta.latency(inp .* ta.gain) # Now create an instance of this actuator with gain 5 and delay of 10 time steps - act = TestAct(DelayedActuator(10), 5.0) + act = TestAct(DelayedActuator(delay_steps), 5.0) # 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 + # Reduce controller gain to ensure stability taking into account the delay w = collect(logrange(0.1, 1e3, 400)) pm = -200 gm = -1 - while pm < 45 || gm < 3 - wgm, gm, wpm, pm = margin(plant_model * act.gain * lc.ctrl_ss, w) + 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 < 45 || gm < 3 + 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 = TestAct(DelayedActuator(delay_steps), 5.0) + + # 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 + # 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] @@ -704,23 +724,40 @@ if args["controller"] target = Interpolations.linear_interpolation(set_points_tt, set_points).(tt) # Run closed loop simulation - res = run_closed_loop_sim(plant_model, act, lc, target) + res = Dict() + res["PI"] = run_closed_loop_sim(plant_model, act, lc, target) + res["PVLC"] = run_closed_loop_sim(plant_model, act2, pvlc, target) - p1 = plot(tt, res[:target][1, :]; color=:black, label="Target", linestyle=:dash) + p1 = plot( + tt, res["PI"][:target][1, :]; + label="Target", linestyle=:dash, color=:black, + ) plot!( - tt, res[:plant_out][1, :]; - linewidth=2, color=:deepskyblue3, - label="PI", ylabel="Plant Output", xformatter=_ -> "", topmargin=10Plots.mm, + tt, res["PI"][:plant_out][1, :]; + label="PI", linewidth=2, color=:deepskyblue3, + ) + plot!( + tt, res["PVLC"][:plant_out][1, :]; + label="PVLC", ylabel="Plant Output", xformatter=_ -> "", + linewidth=2, color=:orange, ) p2 = plot( - tt, res[:plant_inp][1, :]; - linewidth=2, color=:deepskyblue3, - label="PI", ylabel="Plant Input", xformatter=_ -> "", + 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, ) p3 = plot( - tt, res[:ctrl_out][1, :]; - linewidth=2, color=:deepskyblue3, - label="PI", ylabel="Controller\nOutput", xlabel="Time / s", + 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, ) l = @layout [a{0.5h}; b{0.25h}; c{0.25h}] plot( @@ -734,6 +771,14 @@ if args["controller"] savefig("$(@__DIR__)/Closed_Loop_Sim_Results.pdf") - @test mean((res[:target][1, :] .- res[:plant_out][1, :]) .^ 2) < 0.1 + residual_PVLC = sqrt( + mean((res["PVLC"][:target][1, :] .- res["PVLC"][:plant_out][1, :]) .^ 2), + ) + + residual_LC = sqrt( + mean((res["PI"][:target][1, :] .- res["PI"][:plant_out][1, :]) .^ 2), + ) + + @test residual_PVLC < residual_LC end end From c79791bbb9aad6e3ad7a5dd6f0e7721b12dd6081 Mon Sep 17 00:00:00 2001 From: Anchal Gupta Date: Wed, 25 Jun 2025 16:41:53 -0700 Subject: [PATCH 05/12] Separating test dependencies and removing ArgParse Individual tests now should be calle using: ``` julia --project test/test.jl help ``` test/runtests.jl now can be called from Pkg.test only as test specific dependencies have been moved to test/Project.toml. --- Project.toml | 9 ---- test/Project.toml | 6 +++ test/runtests.jl | 108 +++++++++++++--------------------------------- test/test.jl | 88 +++++++++++++++++++++++++++++++++++++ 4 files changed, 123 insertions(+), 88 deletions(-) create mode 100644 test/Project.toml create mode 100644 test/test.jl diff --git a/Project.toml b/Project.toml index 811459e..6de79f5 100644 --- a/Project.toml +++ b/Project.toml @@ -4,12 +4,10 @@ authors = ["David Eldon "] version = "2.2.0" [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" DataStructures = "864edb3b-99cc-5e75-8d2d-829cb0a9cfe8" -DelimitedFiles = "8bb1440f-4735-579b-a4ab-409b98df4dab" EFIT = "cda752c5-6b03-55a3-9e33-132a441b0c17" IMAS = "13ead8c1-b7d1-41bb-a6d0-5b8b65ed587a" IMASggd = "b7b5e640-9b39-4803-84eb-376048795def" @@ -18,20 +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" DataStructures = "0.18.22" -DelimitedFiles = "1" EFIT = "1" IMAS = "1, 2, 3, 4, 5" IMASggd = "3.3" @@ -40,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/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 1d031bb..91e8275 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -1,65 +1,11 @@ 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 using ControlSystemsBase: lsim, ssrand, delay, tf, c2d, ss, pid, margin -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), - ["--state_pred"], - Dict(:help => "Test only state prediction", - :action => :store_true), - ["--controller"], - Dict(:help => "Test only controllers", - :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() - """ make_test_profile() @@ -123,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 @@ -158,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)) @@ -213,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() @@ -228,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) @@ -241,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] @@ -275,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() @@ -326,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() @@ -348,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/") .* [ @@ -446,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 = [ @@ -476,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...") @@ -602,18 +548,18 @@ if args["system_id"] out_offset=ne_offset, out_factor=ne_factor, inp_cond=inp_cond, inp_cond_kwargs=p_opt) - @test sqrt(mean((lin_out - ne_uniform) .^ 2)) < 1.2e18 + @test sqrt(Statistics.mean((lin_out - ne_uniform) .^ 2)) < 1.2e18 - @test sqrt(mean((nonlin_out - ne_uniform) .^ 2)) < 0.4e18 + @test sqrt(Statistics.mean((nonlin_out - ne_uniform) .^ 2)) < 0.4e18 end end -if args["state_pred"] +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 = rand(nstates:20, 1)[1] + hh = 20 rnd_model = ssrand(ny, nu, nstates; proper=false, stable=true, @@ -637,8 +583,8 @@ if args["state_pred"] test_est_Y = LL * x0 + MM * test_U test_est_x = NN * x0 + OO * test_U - # println(sqrt(mean((test_est_Y - test_Y) .^ 2))) - # println(sqrt(mean((test_est_x - final_x) .^ 2))) + # 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) @@ -652,12 +598,12 @@ if args["state_pred"] YY2x, UU2x = state_prediction_matrices(rnd_model, hh) test_est_x2 = YY2x * test_Y_mod + UU2x * test_U_mod - # println(sqrt(mean((test_est_x2 - final_x) .^ 2))) - @test isapprox(test_est_x2, final_x; rtol=noise_scale) + # println(sqrt(Statistics.mean((test_est_x2 - final_x) .^ 2))) + @test isapprox(test_est_x2, final_x; rtol=10 * noise_scale) end end -if args["controller"] +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 @@ -772,11 +718,15 @@ if args["controller"] savefig("$(@__DIR__)/Closed_Loop_Sim_Results.pdf") residual_PVLC = sqrt( - mean((res["PVLC"][:target][1, :] .- res["PVLC"][:plant_out][1, :]) .^ 2), + Statistics.mean( + (res["PVLC"][:target][1, :] .- res["PVLC"][:plant_out][1, :]) .^ 2, + ), ) residual_LC = sqrt( - mean((res["PI"][:target][1, :] .- res["PI"][:plant_out][1, :]) .^ 2), + Statistics.mean( + (res["PI"][:target][1, :] .- res["PI"][:plant_out][1, :]) .^ 2, + ), ) @test residual_PVLC < residual_LC 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 From 990dd8460b9423ff625e9ac4fd4173e2d474f194 Mon Sep 17 00:00:00 2001 From: Anchal Gupta Date: Wed, 25 Jun 2025 17:23:49 -0700 Subject: [PATCH 06/12] Adding instructions for developers --- README.md | 58 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 58 insertions(+) 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 + +``` From a00c4e828b53489992894d9e9c1a7e99e77fca44 Mon Sep 17 00:00:00 2001 From: Anchal Gupta Date: Thu, 26 Jun 2025 11:33:16 -0700 Subject: [PATCH 07/12] Added noise injection ports in simulation --- src/controllers.jl | 42 ++++++++++++++++++++++++++---------------- 1 file changed, 26 insertions(+), 16 deletions(-) diff --git a/src/controllers.jl b/src/controllers.jl index de598ff..0dd5eba 100644 --- a/src/controllers.jl +++ b/src/controllers.jl @@ -351,7 +351,7 @@ end plant_model::Union{PredictionStateSpace{TE}, StateSpace{TE}}, act::Actuator, ctrl::Controller, - target::Union{Vector{Float64}, Matrix{Float64}}; + 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, @@ -374,24 +374,29 @@ function run_closed_loop_sim( plant_model::Union{PredictionStateSpace{TE}, StateSpace{TE}}, act::Actuator, ctrl::Controller, - target::Union{Vector{Float64}, Matrix{Float64}}; + 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_model.B, 2), length(target)), + (size(plant_model.B, 2), size(target, 2)), ), ctrl_start_ind::Int=1, + noise_plant_inp::Matrix{Float64}=zeros( + Float64, + (size(plant_model.B, 2), size(target, 2)), + ), + noise_plant_out::Matrix{Float64}=zeros( + Float64, + (size(plant_model.C, 1), size(target, 2)), + ), + noise_ctrl_out::Matrix{Float64}=zeros( + Float64, + (size(plant_model.B, 2), size(target, 2)), + ), ) where {T, TE <: Discrete} - if isa(target, Vector) - if size(plant_model.C, 1) == 1 - target = Matrix(target') - else - error("Plant has multiple outputs, target must be Matrix") - end - end ctrl_out = zeros(Float64, size(inp_feedforward)) plant_inp = zeros(Float64, (size(plant_model.B, 2), length(target))) plant_out = zeros(Float64, (size(plant_model.C, 1), length(target))) @@ -399,20 +404,22 @@ function run_closed_loop_sim( inv(Matrix{Float64}(I, size(plant_model.A)...) - plant_model.A) * plant_model.B * inp_feedforward[:, 1] for ii ∈ eachindex(target) - plant_inp[:, ii] = act(ctrl_out[:, ii] .+ inp_feedforward[:, ii]) + plant_inp[:, ii] = + act(ctrl_out[:, ii] .+ inp_feedforward[:, ii]) .+ noise_plant_inp[:, ii] plant_out[:, ii], x0 = model_step( plant_model, plant_inp[:, ii]; x0, inp_cond, inp_offset, inp_factor, out_offset, out_factor, inp_cond_kwargs, ) - # err = target[:, ii] .- plant_out[:, ii] + plant_out[:, ii] .+= noise_plant_out[:, ii] future_inp = get_future_inp(act) if ii >= ctrl_start_ind && ii < length(target) - ctrl_out[:, ii+1] = ctrl(; - ii, target, plant_inp, plant_out, future_inp, - inp_offset, inp_factor, out_offset, out_factor, - ) + ctrl_out[:, ii+1] = + ctrl(; + ii, target, plant_inp, plant_out, future_inp, + inp_offset, inp_factor, out_offset, out_factor, + ) .+ noise_ctrl_out[:, ii+1] end end return Dict( @@ -431,5 +438,8 @@ function run_closed_loop_sim( :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 From 4b41787e095ae0a7eedfa9c56db45b6313f05105 Mon Sep 17 00:00:00 2001 From: Anchal Gupta Date: Thu, 26 Jun 2025 16:55:48 -0700 Subject: [PATCH 08/12] Added MPC algorithm --- src/controllers.jl | 199 +++++++++++++++++++++++++++++++++++++++++++-- test/runtests.jl | 4 +- 2 files changed, 194 insertions(+), 9 deletions(-) diff --git a/src/controllers.jl b/src/controllers.jl index 0dd5eba..921ba89 100644 --- a/src/controllers.jl +++ b/src/controllers.jl @@ -1,11 +1,10 @@ import ControlSystemIdentification: PredictionStateSpace import ControlSystemsBase: StateSpace, Discrete import LinearAlgebra: pinv -using DataStructures: Queue, enqueue!, dequeue! -# import ControlSystemsBase: lsim, ss, pid, d2c_exact, d2c, StateSpace +using DataStructures: Queue, enqueue!, dequeue!, empty!, first export lsim_step, model_step, state_prediction_matrices, Actuator, DelayedActuator, - Controller, LinearController, PVLC, run_closed_loop_sim + Controller, LinearController, PVLC, MPC, run_closed_loop_sim """ lsim_step( @@ -201,6 +200,22 @@ 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 @@ -216,7 +231,9 @@ arguments: - `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) - - `future_inp::Matrix{Float64}`: Upcoming future known inputs to plant (for delayed actuators) (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 @@ -286,7 +303,7 @@ This controller has a call signature: target::Matrix{Float64}, plant_inp::Matrix{Float64}, plant_out::Matrix{Float64}, - future_inp::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..., @@ -319,12 +336,13 @@ function (pvlc::PVLC)(; target::Matrix{Float64}, plant_inp::Matrix{Float64}, plant_out::Matrix{Float64}, - future_inp::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 @@ -346,6 +364,162 @@ function (pvlc::PVLC)(; 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, :] = linear_interpolation(steps, red_mat[i, :]).(1:(horizon+1)) + end + return [mat[:, i] for i ∈ 1:(horizon+1)] +end + +mutable struct MPC <: Controller + plant_model::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_model::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_model, 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_model.B, 2), + min_delay + horizon + 1, + ), + x0::Vector{Float64}=zeros(Float64, size(mpc.plant_model.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_model.B, 2) + nouts = size(mpc.plant_model.C, 1) + ctrl_out = expand(red_steps, red_ctrl_out, ninps, mpc.horizon) + 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[:, ii], x0 = model_step( + mpc.plant_model, 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_model, 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}, + 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_model.B, 2), length(target)), + ), + act::Actuator, + 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 buf_empty + prev_ctrl_out = zeros(Float64, size(mpc.ctrl_ss.C, 1)) + else + prev_ctrl_out = first(mpc.ctrl_out_buffer) + end + + 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_model.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:(ii+fut_lookup)]) + guess = repeat(prev_ctrl_out, length(red_steps)) + inp_ff = inp_feedforward[:, ii:(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, coeff(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.ctrl_ss.C, 1)) + else + return dequeue!(mpc.ctrl_out_buffer) + end +end + """ run_closed_loop_sim( plant_model::Union{PredictionStateSpace{TE}, StateSpace{TE}}, @@ -397,15 +571,21 @@ function run_closed_loop_sim( (size(plant_model.B, 2), size(target, 2)), ), ) where {T, TE <: Discrete} + # Initialization ctrl_out = zeros(Float64, size(inp_feedforward)) plant_inp = zeros(Float64, (size(plant_model.B, 2), length(target))) plant_out = zeros(Float64, (size(plant_model.C, 1), length(target))) x0 = inv(Matrix{Float64}(I, size(plant_model.A)...) - plant_model.A) * plant_model.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_model, plant_inp[:, ii]; x0, @@ -413,11 +593,14 @@ function run_closed_loop_sim( inp_cond_kwargs, ) plant_out[:, ii] .+= noise_plant_out[:, ii] - future_inp = get_future_inp(act) + # 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, future_inp, + 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 diff --git a/test/runtests.jl b/test/runtests.jl index 91e8275..670bcdf 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -667,7 +667,9 @@ if isempty(ARGS) || "controller" in ARGS 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 = Interpolations.linear_interpolation(set_points_tt, set_points).(tt) + target = Matrix{Float64}( + Interpolations.linear_interpolation(set_points_tt, set_points).(tt)', + ) # Run closed loop simulation res = Dict() From 838f42cf7efcd0b4fce3cd5b9c2da9b59995818e Mon Sep 17 00:00:00 2001 From: Anchal Gupta Date: Mon, 30 Jun 2025 11:46:12 -0700 Subject: [PATCH 09/12] Added testing and docs of MPC Now tests include noise injection at all the points and also compares the residual of MPC with PVLC. --- docs/src/index.md | 11 +++ src/controllers.jl | 175 ++++++++++++++++++++++++++++++++++----------- test/runtests.jl | 64 +++++++++++++++-- 3 files changed, 203 insertions(+), 47 deletions(-) diff --git a/docs/src/index.md b/docs/src/index.md index 091edbd..514fe47 100644 --- a/docs/src/index.md +++ b/docs/src/index.md @@ -102,6 +102,11 @@ end ```@docs Controller +``` + +### Linear Controller + +```@docs LinearController ``` @@ -245,6 +250,12 @@ 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 + +```@docs +MPC +``` + ## Closed loop simulations ```@docs diff --git a/src/controllers.jl b/src/controllers.jl index 921ba89..6bc5900 100644 --- a/src/controllers.jl +++ b/src/controllers.jl @@ -1,7 +1,8 @@ import ControlSystemIdentification: PredictionStateSpace import ControlSystemsBase: StateSpace, Discrete import LinearAlgebra: pinv -using DataStructures: Queue, enqueue!, dequeue!, empty!, first +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 @@ -284,7 +285,7 @@ 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_model`, the number of steps of history `h` used for +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: @@ -292,7 +293,7 @@ There is a convinience contructor: PVLC( ctrl_ss::StateSpace{TE}, ctrl_x0::Vector{Float64}, - plant_model::Union{PredictionStateSpace, StateSpace}, + plant::Union{PredictionStateSpace, StateSpace}, h::Int, ) where {TE <: Discrete} @@ -316,18 +317,18 @@ 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_model::Union{PredictionStateSpace, StateSpace} + plant::Union{PredictionStateSpace, StateSpace} h::Int Y2x::Matrix{Float64} U2x::Matrix{Float64} function PVLC( ctrl_ss::StateSpace{TE}, ctrl_x0::Vector{Float64}, - plant_model::Union{PredictionStateSpace, StateSpace}, + plant::Union{PredictionStateSpace, StateSpace}, h::Int, ) where {TE <: Discrete} - Y2x, U2x = state_prediction_matrices(plant_model, h) - return new(ctrl_ss, ctrl_x0, plant_model, h, Y2x, U2x) + Y2x, U2x = state_prediction_matrices(plant, h) + return new(ctrl_ss, ctrl_x0, plant, h, Y2x, U2x) end end @@ -352,7 +353,7 @@ function (pvlc::PVLC)(; est_x = pvlc.Y2x * Y .+ pvlc.U2x * U # Evolve into the future for the delay in actuator future_out = model_evolve( - pvlc.plant_model, future_inp; + pvlc.plant, future_inp; x0=est_x, inp_offset, inp_factor, out_offset, out_factor, ) @@ -373,13 +374,86 @@ function expand( red_mat = reshape(red_vec, ninps, length(steps)) mat = zeros(Float64, ninps, horizon + 1) for i ∈ 1:ninps - mat[i, :] = linear_interpolation(steps, red_mat[i, :]).(1:(horizon+1)) + 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_model::Union{PredictionStateSpace, StateSpace} + plant::Union{PredictionStateSpace, StateSpace} h::Int Y2x::Matrix{Float64} U2x::Matrix{Float64} @@ -393,7 +467,7 @@ mutable struct MPC <: Controller future_evolve::Function ctrl_out_buffer::Queue{Vector{Float64}} function MPC( - plant_model::Union{PredictionStateSpace, StateSpace}, + plant::Union{PredictionStateSpace, StateSpace}, h::Int, act::Actuator, # Actuator model without delay horizon::Int, # Number of steps in future after latency @@ -404,32 +478,35 @@ mutable struct MPC <: Controller Array{Float64}(undef, 0), ), ) - Y2x, U2x = state_prediction_matrices(plant_model, h) + 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_model.B, 2), + size(mpc.plant.B, 2), min_delay + horizon + 1, ), - x0::Vector{Float64}=zeros(Float64, size(mpc.plant_model.A, 2)), + 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_model.B, 2) - nouts = size(mpc.plant_model.C, 1) + 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[:, ii], x0 = model_step( - mpc.plant_model, plant_inp; + plant_out[:, i], x0 = model_step( + mpc.plant, plant_inp; x0, inp_cond, inp_offset, inp_factor, out_offset, out_factor, inp_cond_kwargs, @@ -440,7 +517,7 @@ mutable struct MPC <: Controller last_opt = 0 ctrl_out_buffer = Queue{Vector{Float64}}(horizon) return new( - plant_model, h, Y2x, U2x, act, horizon, nopt, opt_every, last_opt, + plant, h, Y2x, U2x, act, horizon, nopt, opt_every, last_opt, min_delay, ctrl_out_bounds, fe, ctrl_out_buffer, ) end @@ -451,15 +528,15 @@ function (mpc::MPC)(; 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_model.B, 2), length(target)), + (size(plant.B, 2), length(target)), ), - act::Actuator, kwargs..., )::Vector{Float64} where {T} fut_lookup = mpc.min_delay + mpc.horizon + 1 @@ -470,7 +547,7 @@ function (mpc::MPC)(; buf_empty = isempty(mpc.ctrl_out_buffer) if buf_empty - prev_ctrl_out = zeros(Float64, size(mpc.ctrl_ss.C, 1)) + prev_ctrl_out = zeros(Float64, size(mpc.plant.B, 2)) else prev_ctrl_out = first(mpc.ctrl_out_buffer) end @@ -479,7 +556,7 @@ 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_model.B, 2) + ninps = size(mpc.plant.B, 2) # Gather history of inputs and outputs U = vec(plant_inp[:, (ii-mpc.h+1):ii]) @@ -489,9 +566,9 @@ function (mpc::MPC)(; # Prepare arguments for cost optimization mpc.act = deepcopy(act) - target_vec = vec(target[:, ii:(ii+fut_lookup)]) + target_vec = vec(target[:, (ii+1):(ii+fut_lookup)]) guess = repeat(prev_ctrl_out, length(red_steps)) - inp_ff = inp_feedforward[:, ii:(ii+fut_lookup)] + inp_ff = inp_feedforward[:, (ii+1):(ii+fut_lookup)] # Optimize fit_res = curve_fit( @@ -506,7 +583,7 @@ function (mpc::MPC)(; # Empty buffer and fill with updated control outputs empty!(mpc.ctrl_out_buffer) - for co ∈ expand(red_steps, coeff(fit_res), ninps, mpc.horizon) + for co ∈ expand(red_steps, coef(fit_res), ninps, mpc.horizon) enqueue!(mpc.ctrl_out_buffer, co) end end @@ -514,7 +591,7 @@ function (mpc::MPC)(; mpc.last_opt = ii end if isempty(mpc.ctrl_out_buffer) - return zeros(Float64, size(mpc.ctrl_ss.C, 1)) + return zeros(Float64, size(mpc.plant.B, 2)) else return dequeue!(mpc.ctrl_out_buffer) end @@ -522,7 +599,7 @@ end """ run_closed_loop_sim( - plant_model::Union{PredictionStateSpace{TE}, StateSpace{TE}}, + plant::Union{PredictionStateSpace{TE}, StateSpace{TE}}, act::Actuator, ctrl::Controller, target::Matrix{Float64}; @@ -532,20 +609,34 @@ end inp_cond_kwargs::Dict{Symbol, T}=Dict{Symbol, Any}(), inp_feedforward::Matrix{Float64}=zeros( Float64, - (size(plant_model.B, 2), length(target)), + (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_model`, actuator +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. +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_model::Union{PredictionStateSpace{TE}, StateSpace{TE}}, + plant::Union{PredictionStateSpace{TE}, StateSpace{TE}}, act::Actuator, ctrl::Controller, target::Matrix{Float64}; @@ -555,29 +646,29 @@ function run_closed_loop_sim( inp_cond_kwargs::Dict{Symbol, T}=Dict{Symbol, Any}(), inp_feedforward::Matrix{Float64}=zeros( Float64, - (size(plant_model.B, 2), size(target, 2)), + (size(plant.B, 2), size(target, 2)), ), ctrl_start_ind::Int=1, noise_plant_inp::Matrix{Float64}=zeros( Float64, - (size(plant_model.B, 2), size(target, 2)), + (size(plant.B, 2), size(target, 2)), ), noise_plant_out::Matrix{Float64}=zeros( Float64, - (size(plant_model.C, 1), size(target, 2)), + (size(plant.C, 1), size(target, 2)), ), noise_ctrl_out::Matrix{Float64}=zeros( Float64, - (size(plant_model.B, 2), size(target, 2)), + (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_model.B, 2), length(target))) - plant_out = zeros(Float64, (size(plant_model.C, 1), length(target))) + 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_model.A)...) - plant_model.A) * - plant_model.B * inp_feedforward[:, 1] + inv(Matrix{Float64}(I, size(plant.A)...) - plant.A) * + plant.B * inp_feedforward[:, 1] # Closed loop simulation for ii ∈ eachindex(target) @@ -587,7 +678,7 @@ function run_closed_loop_sim( # Plant response plant_out[:, ii], x0 = model_step( - plant_model, plant_inp[:, ii]; + plant, plant_inp[:, ii]; x0, inp_cond, inp_offset, inp_factor, out_offset, out_factor, inp_cond_kwargs, @@ -606,7 +697,7 @@ function run_closed_loop_sim( end end return Dict( - :plant_model => plant_model, + :plant => plant, :act => act, :ctrl => ctrl, :target => target, diff --git a/test/runtests.jl b/test/runtests.jl index 670bcdf..e031560 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -663,6 +663,13 @@ if isempty(ARGS) || "controller" in ARGS end end + # Create MPC + act3 = TestAct(DelayedActuator(delay_steps), 5.0) + 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) + # 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] @@ -671,14 +678,39 @@ if isempty(ARGS) || "controller" in ARGS 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() - res["PI"] = run_closed_loop_sim(plant_model, act, lc, target) - res["PVLC"] = run_closed_loop_sim(plant_model, act2, pvlc, target) + 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", linestyle=:dash, color=:black, + label="Target", linewidth=2, linestyle=:dash, color=:black, ) plot!( tt, res["PI"][:plant_out][1, :]; @@ -689,6 +721,11 @@ if isempty(ARGS) || "controller" in ARGS label="PVLC", ylabel="Plant Output", xformatter=_ -> "", linewidth=2, color=:orange, ) + 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, @@ -698,6 +735,11 @@ if isempty(ARGS) || "controller" in ARGS 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, + ) p3 = plot( tt, res["PI"][:ctrl_out][1, :]; label="PI", linewidth=2, color=:deepskyblue3, @@ -707,6 +749,11 @@ if isempty(ARGS) || "controller" in ARGS 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, + ) l = @layout [a{0.5h}; b{0.25h}; c{0.25h}] plot( p1, p2, p3; @@ -719,18 +766,25 @@ if isempty(ARGS) || "controller" in ARGS 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_LC = sqrt( + residual_MPC = sqrt( Statistics.mean( - (res["PI"][:target][1, :] .- res["PI"][:plant_out][1, :]) .^ 2, + (res["MPC"][:target][1, :] .- res["MPC"][:plant_out][1, :]) .^ 2, ), ) @test residual_PVLC < residual_LC + @test residual_MPC < residual_PVLC end end From b13a5885607ac028a1738de578cbe0ba62982fec Mon Sep 17 00:00:00 2001 From: Anchal Gupta Date: Mon, 30 Jun 2025 13:11:01 -0700 Subject: [PATCH 10/12] Adding test with non-linear actuator --- src/controllers.jl | 4 ++-- test/runtests.jl | 46 ++++++++++++++++++++++++++++++++++++++++------ 2 files changed, 42 insertions(+), 8 deletions(-) diff --git a/src/controllers.jl b/src/controllers.jl index 6bc5900..93a5a10 100644 --- a/src/controllers.jl +++ b/src/controllers.jl @@ -407,7 +407,7 @@ There is a convinience contructor: 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 + 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), @@ -472,7 +472,7 @@ mutable struct MPC <: Controller 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 + 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), diff --git a/test/runtests.jl b/test/runtests.jl index e031560..8267ae2 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -621,13 +621,42 @@ if isempty(ARGS) || "controller" in ARGS # 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 a simple gain actuator with some latency - (ta::TestAct)(inp::Vector{Float64}) = ta.latency(inp .* ta.gain) + # 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), 5.0) + 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)) @@ -648,7 +677,7 @@ if isempty(ARGS) || "controller" in ARGS # 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 = TestAct(DelayedActuator(delay_steps), 5.0) + act2 = deepcopy(act) # Tune PVLC controller for no delay pm = -200 @@ -664,11 +693,14 @@ if isempty(ARGS) || "controller" in ARGS end # Create MPC - act3 = TestAct(DelayedActuator(delay_steps), 5.0) + 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) + mpc = MPC( + plant_model, 20, act3, horizon, nopt, opt_every; + ctrl_out_bounds=act3.bounds .* 1.1 ./ act3.gain, + ) # Set a target waveform tt = collect(0:Ts:2) @@ -739,6 +771,7 @@ if isempty(ARGS) || "controller" in ARGS 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, :]; @@ -753,6 +786,7 @@ if isempty(ARGS) || "controller" in ARGS 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( From 7d426b509980afc4b4eb2ea6648674cbe724efe9 Mon Sep 17 00:00:00 2001 From: Anchal Gupta Date: Mon, 30 Jun 2025 14:36:47 -0700 Subject: [PATCH 11/12] Fixed initial guess to zeros; more stable --- src/controllers.jl | 8 +------- test/runtests.jl | 6 +++--- 2 files changed, 4 insertions(+), 10 deletions(-) diff --git a/src/controllers.jl b/src/controllers.jl index 93a5a10..72a57bf 100644 --- a/src/controllers.jl +++ b/src/controllers.jl @@ -546,12 +546,6 @@ function (mpc::MPC)(; opt_step = ii > mpc.opt_every + mpc.last_opt buf_empty = isempty(mpc.ctrl_out_buffer) - if buf_empty - prev_ctrl_out = zeros(Float64, size(mpc.plant.B, 2)) - else - prev_ctrl_out = first(mpc.ctrl_out_buffer) - end - 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)) @@ -567,7 +561,7 @@ function (mpc::MPC)(; # Prepare arguments for cost optimization mpc.act = deepcopy(act) target_vec = vec(target[:, (ii+1):(ii+fut_lookup)]) - guess = repeat(prev_ctrl_out, length(red_steps)) + guess = repeat(zeros(Float64, ninps), length(red_steps)) inp_ff = inp_feedforward[:, (ii+1):(ii+fut_lookup)] # Optimize diff --git a/test/runtests.jl b/test/runtests.jl index 8267ae2..46458df 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -699,7 +699,7 @@ if isempty(ARGS) || "controller" in ARGS 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 .* 1.1 ./ act3.gain, + ctrl_out_bounds=act3.bounds .* 2.0 ./ act3.gain, ) # Set a target waveform @@ -750,8 +750,8 @@ if isempty(ARGS) || "controller" in ARGS ) plot!( tt, res["PVLC"][:plant_out][1, :]; - label="PVLC", ylabel="Plant Output", xformatter=_ -> "", - linewidth=2, color=:orange, + label="PVLC", ylabel="Plant Output", linewidth=2, color=:orange, + xformatter=_ -> "", topmargin=10Plots.mm, ) plot!( tt, res["MPC"][:plant_out][1, :]; From a3669081e3aac5af8d98eca090469d71407dc5c2 Mon Sep 17 00:00:00 2001 From: Anchal Gupta Date: Mon, 30 Jun 2025 14:41:42 -0700 Subject: [PATCH 12/12] Releasing v2.2.1 with controllers and testbed This release has generic closed loop simulation platform to test out control algorithms with plant models and actuator models. All linear plant models with an additional possibility of applying some input conditioning to it that can be used to make it non-linear. All actuation functions can be wrapped into an Actuator object which is what this platform uses. Similarly, all control algorithms can be wrapped as well as long as they only use the described keyword arguments. For convinience, childrens of Actuators and Controllers are provided. A DelayedActuator type is provided that can be used to add a latency anywhere in the actuator. It is recommended to use this instance to insert latency for compatibility with provided controllers. A LinearController is provided that can emulate any linear controller. See test example to see use for PI controller. Novel PVLC and MPC algorithms have been provided that help fight against delayed and non-linear actuator restrictions. See test example for a quick glance. --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index 6de79f5..b8052cf 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.0" +version = "2.2.1" [deps] Contour = "d38c429a-6771-53c6-b99e-75d170b6e991"