diff --git a/Project.toml b/Project.toml index 3a656890..f4e844fb 100644 --- a/Project.toml +++ b/Project.toml @@ -23,6 +23,12 @@ Statistics = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" UUIDs = "cf7118a7-6976-5b1a-9a39-7adc72f591a4" UnPack = "3a884ed6-31ef-47d7-9d2a-63182c4928ed" +[weakdeps] +LinearMPC = "82e1c212-e1a2-49d2-b26a-a31d6968e3bd" + +[extensions] +RobustAndOptimalControlLinearMPCExt = "LinearMPC" + [compat] ChainRulesCore = "1" ComponentArrays = "0.9, 0.10, 0.11, 0.12, 0.13, 0.14, 0.15" @@ -30,6 +36,7 @@ ControlSystemsBase = "1.17" DescriptorSystems = "1.2" Distributions = "0.25" GenericSchur = "0.5.2" +LinearMPC = "0.7" MatrixEquations = "2" MatrixPencils = "1" MonteCarloMeasurements = "1.0" @@ -44,9 +51,10 @@ julia = "1.7" [extras] FiniteDiff = "6a86dc24-6348-571c-b903-95158fe2bd41" GenericLinearAlgebra = "14197337-ba66-59df-a3e3-ca00e7dcff7a" +LinearMPC = "82e1c212-e1a2-49d2-b26a-a31d6968e3bd" Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80" Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" Zygote = "e88e6eb3-aa80-5325-afca-941959d7151f" [targets] -test = ["Test", "Plots", "Zygote", "FiniteDiff", "GenericLinearAlgebra"] +test = ["Test", "Plots", "LinearMPC", "Zygote", "FiniteDiff", "GenericLinearAlgebra"] diff --git a/docs/Project.toml b/docs/Project.toml index db590b6f..e6b72b55 100644 --- a/docs/Project.toml +++ b/docs/Project.toml @@ -3,6 +3,7 @@ ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e" DisplayAs = "0b91fe84-8a4c-11e9-3e1d-67c38462b6d6" Documenter = "e30172f5-a6a5-5a46-863b-614d45cd2de4" ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210" +LinearMPC = "82e1c212-e1a2-49d2-b26a-a31d6968e3bd" MonteCarloMeasurements = "0987c9cc-fe09-11e8-30f0-b96dd679fdca" Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80" RobustAndOptimalControl = "21fd56a4-db03-40ee-82ee-a87907bee541" diff --git a/docs/src/index.md b/docs/src/index.md index 4e8b8b05..277e3362 100644 --- a/docs/src/index.md +++ b/docs/src/index.md @@ -297,3 +297,23 @@ A video tutorial on how to perform closed-loop analysis is available [here](http - [`hess_form`](@ref) - [`frequency_separation`](@ref) - [`RobustAndOptimalControl.blockdiagonalize`](@ref) + +## MPC +This package includes an extension for [LinearMPC.jl](https://darnstrom.github.io/LinearMPC.jl/stable/) that allows you to convert an [`LQGProblem`](@ref) into a linear MPC controller with added constraints. Example: +```@example lqg_mpc +using RobustAndOptimalControl, LinearMPC, LinearAlgebra + +sys = ss([1 0.1; 0 1], [0; 0.1], [1 0], 0, 0.1) +lqg = LQGProblem(sys, I(2), I(1), I(2), 0.01*I(1)) + +mpc = LinearMPC.MPC(lqg; N=20, umin=[-0.3], umax=[0.3]) + +sim = LinearMPC.Simulation(mpc, N=100, r=[1.0, 0]) + +using Plots +plot(sim) +``` + +See [Example: `lqg_mpc_disturbance.jl`](https://github.com/JuliaControl/RobustAndOptimalControl.jl/blob/master/examples/lqg_mpc_disturbance.jl) for a more detailed example. + +See the [docs for LinearMPC.jl](https://darnstrom.github.io/LinearMPC.jl/stable/) for more details on how to use the MPC controller and modify its settings. \ No newline at end of file diff --git a/examples/lqg_mpc_disturbance.jl b/examples/lqg_mpc_disturbance.jl new file mode 100644 index 00000000..38398180 --- /dev/null +++ b/examples/lqg_mpc_disturbance.jl @@ -0,0 +1,106 @@ +#= +This file demonstrates +- Adding integral action to an LQG controller by means of state augmentation +- Conversion of the LQG controller into an MPC contorller with added constraints +=# +using RobustAndOptimalControl, ControlSystemsBase, Plots, LinearAlgebra +Ts = 1 # Sample time +G = c2d(ss(tf(1, [10, 1])), Ts) # Process model + +nx = G.nx +nu = G.nu +ny = G.ny +x0 = zeros(G.nx) # Initial condition + +Q1 = 100diagm(ones(G.nx)) # state cost matrix +Q2 = 0.01diagm(ones(nu)) # control cost matrix + +R1 = 0.001I(nx) # State noise covariance +R2 = I(ny) # measurement noise covariance +prob = LQGProblem(G, Q1, Q2, R1, R2) + +disturbance = (x, t) -> t * Ts ≥ 10 # This is our load disturbance, a step at ``t = 10`` +Gcl = G_PS(prob) # This forms the transfer function from load disturbance to output +res = lsim(Gcl, disturbance, 100) +plot(res) + +Gd1 = add_low_frequency_disturbance(G, ϵ = 1e-6, measurement=false) +Gd2 = add_low_frequency_disturbance(G, ϵ = 1e-6, measurement=true) # The ϵ moves the integrator pole slightly into the stable region +plots = map([Gd1, Gd2]) do Gd + nx = Gd.nx + + C = Gd.C + Q1 = 100C'diagm(ones(G.nx)) * C # state cost matrix + x0 = zeros(nx) + + R1 = diagm([0.001, 1]) + R2 = I(ny) + prob = LQGProblem(Gd, Q1, Q2, R1, R2) + Gcl = [G_PS(prob); -comp_sensitivity(prob)] # -comp_sensitivity(prob) is the same as the transfer function from load disturbance to control signal + res = lsim(Gcl, disturbance, 100) + f1 = plot(res, ylabel=["y" "u"]); ylims!((-0.05, 0.3), sp = 1) + + w = exp10.(LinRange(-3, log10(pi / Ts), 200)) + f2 = gangoffourplot(prob, w, lab = "", legend = :bottomright) + + R1 = diagm([0.001, 0.2]) # Reduce the noise on the integrator state from 1 to 0.2 + R2 = I(ny) + prob = LQGProblem(Gd, Q1, Q2, R1, R2) + + Gcl = [G_PS(prob); -comp_sensitivity(prob)] + res = lsim(Gcl, disturbance, 100) + f3 = plot(res, ylabel=["y" "u"]); ylims!((-0.05, 0.3), sp = 1) + f4 = gangoffourplot(prob, w, lab = "", legend = :bottomright) + + plot(f1, f2, f3, f4, titlefontsize=10) +end + +plot(plots..., size=(1200,1000)) + + + +# ============================================================================== +## LinearMPC +# The example below illustrate how we can convert the LQGProblem into an MPC contorller by loading LinearMPC.jl +# We then perform a rather low-level simulation with a manual loop, where we form the observer `obs` and step the plant +# ============================================================================== + +using LinearMPC + +Gd = add_low_frequency_disturbance(G, ϵ = 1e-6, measurement=false) + +C = Gd.C +Q1 = 100diagm([1.0]) # output cost matrix +R1 = diagm([0.001, 1]) # Dynamics noise covariance +R2 = I(ny) # Measurement noise covariance +Gde = ExtendedStateSpace(Gd, B1=I) # Since B1=I, R1 has size determined by state dimension, but C1=C, so Q1 has size determined by the output dimension +prob = LQGProblem(Gde, Q1, Q2, R1, R2) +obs = observer_predictor(prob, direct=false) # If a predictor is used, the observer update should be carried out in the end of the loop, if we use the filter below, we should instead perform the observer update in the beginning of the loop directly after obtaining the new measurement but before computing a new control signal. +obs = observer_filter(prob) + +mpc = LinearMPC.MPC(prob; N=20, umin=[-3], umax=[3]) +x = zeros(G.nx) # True plant state +xh = zeros(Gd.nx) # Observer state +X = [x[]] # For storage +U = Float64[] +u_mpc = [0.0] +for i = 1:50 + global x, xh, u_mpc + y = G.C*x # Compute the true measurement output + xh = obs.A * xh + obs.B*[u_mpc; y] # Predict one step with the observer, u here is the control signal from the previous iteration, if using the predictor, use u from the current iteration and perform the observer update in the end of the loop instead + u_disturbance = i * Ts ≥ 10 ? 1.0 : 0.0 + r = [1.0] # output reference + u_mpc = LinearMPC.compute_control(mpc, xh; r) # Call MPC optimizer with estimated state + u_tot = u_mpc .+ u_disturbance # Total input is control signal + disturbance + x = G.A*x + G.B*u_tot # Advance the true plant state + push!(X, x[]) # Store data for plotting + push!(U, u_mpc[]) +end + +using Test +@test (G.C*X[end])[] ≈ 1 rtol=1e-4 + +plot(X*G.C', layout=2, sp=1, label="\$y\$") +plot!(U, sp=2, label="\$u\$") +hline!([1.0 3.0], linestyle=:dash, color=:black, label=["Reference" "\$u_{max}\$"], sp=[1 2]) + diff --git a/ext/RobustAndOptimalControlLinearMPCExt.jl b/ext/RobustAndOptimalControlLinearMPCExt.jl new file mode 100644 index 00000000..9cf912b7 --- /dev/null +++ b/ext/RobustAndOptimalControlLinearMPCExt.jl @@ -0,0 +1,96 @@ +""" + RobustAndOptimalControlLinearMPCExt + +This extension allows you to convert an `LQGProblem` from RobustAndOptimalControl.jl to a `LinearMPC.MPC` object from LinearMPC.jl. +""" +module RobustAndOptimalControlLinearMPCExt + +using LinearMPC +using LinearAlgebra +using RobustAndOptimalControl: LQGProblem +using ControlSystemsBase: isdiscrete + + +""" + LinearMPC.MPC(prob::LQGProblem; N, Nc=N, Q3=nothing, Qf=nothing, + umin=nothing, umax=nothing, ymin=nothing, ymax=nothing, kwargs...) + +Convert an `LQGProblem` from RobustAndOptimalControl.jl to a `LinearMPC.MPC` object. + +# Arguments +- `prob::LQGProblem`: The LQG problem to convert +- `N::Int`: Prediction horizon (required) +- `Nc::Int`: Control horizon (default: `N`) +- `Q3`: Input rate cost matrix (default: zeros) +- `Qf`: Terminal state cost matrix (default: none) +- `umin`, `umax`: Input bounds (default: none) +- `ymin`, `ymax`: Output bounds (default: none) +- `kwargs...`: Additional arguments passed to LinearMPC.MPC + +# Notes +- Only discrete-time systems are supported +- The Kalman filter/observer from LQGProblem is not converted +- Uses C1 (performance output) as the controlled output matrix +- Cost matrices Q1, Q2 from LQGProblem map to Q, R in LinearMPC + +# Example +```julia +using RobustAndOptimalControl, LinearMPC, LinearAlgebra + +sys = ss([1 0.1; 0 1], [0; 0.1], [1 0], 0, 0.1) +lqg = LQGProblem(sys, I(2), I(1), I(2), 0.01*I(1)) +mpc = LinearMPC.MPC(lqg; N=20, umin=[-0.3], umax=[0.3]) + +sim = LinearMPC.Simulation(mpc, N=100, r=[1.0, 0]) + +using Plots +plot(sim) +``` +""" +function LinearMPC.MPC(prob::LQGProblem; + N::Int, + Nc::Int = N, + Q3 = zeros(0,0), + Qf = zeros(0,0), + umin = zeros(0), + umax = zeros(0), + ymin = zeros(0), + ymax = zeros(0), + kwargs... +) + # Validate discrete-time system + if !isdiscrete(prob) + error("Only discrete-time systems are supported. Got a continuous-time system.") + end + + # Extract system matrices + F = Matrix(prob.A) # State transition matrix + G = Matrix(prob.B2) # Control input matrix + C = Matrix(prob.C1) # Performance output matrix + + # Get sampling time + Ts = prob.Ts + + # Get dimensions + nu = size(G, 2) + ny = size(C, 1) + + # Create the LinearMPC.MPC object + mpc = LinearMPC.MPC(F, G; Ts, C, Np=N, Nc, kwargs...) + + # Set objective + Q = Matrix(prob.Q1) + R = Matrix(prob.Q2) + Rr = Matrix(Q3) + + LinearMPC.set_objective!(mpc; Q, R, Rr, Qf) + LinearMPC.set_bounds!(mpc; umin, umax, ymin, ymax) + + # Set labels from system names + # sys = prob.sys + # set_labels!(mpc; x=Symbol.(state_names(sys)), u=Symbol.(input_names(sys))) + + return mpc +end + +end # module \ No newline at end of file diff --git a/src/ExtendedStateSpace.jl b/src/ExtendedStateSpace.jl index 6c74588a..084953e6 100644 --- a/src/ExtendedStateSpace.jl +++ b/src/ExtendedStateSpace.jl @@ -599,7 +599,7 @@ The conversion from a regular statespace object to an `ExtendedStateSpace` creat i.e., the system and performance mappings are identical, `system_mapping(se) == performance_mapping(se) == s`. However, all matrices `B1, B2, C1, C2; D11, D12, D21, D22` are overridable by a corresponding keyword argument. In this case, the controlled outputs are the same as measured outputs. -Related: `se = convert(ExtendedStateSpace, s::StateSpace)` produces an `ExtendedStateSpace` with empty `performance_mapping` from w->z such that `ss(se) == s`. +Related: `se = convert(ExtendedStateSpace, s::StateSpace)` produces an `ExtendedStateSpace` with empty `performance_mapping` from w->z such that `ss(se) == s`. `ExtendedStateSpace(sys, B1=I, C1=I)` leads to a system where all state variables are affected by noise, and all are considered performance outputs, this corresponds to the traditional use of the functions `lqr` and `kalman`. """ function ExtendedStateSpace(s::AbstractStateSpace; A = s.A, diff --git a/src/lqg.jl b/src/lqg.jl index d7e8b50b..f049fcfd 100644 --- a/src/lqg.jl +++ b/src/lqg.jl @@ -354,6 +354,18 @@ function ControlSystemsBase.observer_controller(l::LQGProblem, L::AbstractMatrix ss(Ac, Bc, Cc, Dc, l.timeevol) end +function ControlSystemsBase.observer_predictor(l::LQGProblem, K::Union{AbstractMatrix, Nothing} = nothing; direct = false, kwargs...) + P = system_mapping(l, identity) + if K === nothing + K = kalman(l; direct) + end + observer_predictor(P, K; kwargs...) +end + +function ControlSystemsBase.observer_filter(l::LQGProblem, K = kalman(l); kwargs...) + P = system_mapping(l, identity) + observer_filter(P, K; kwargs...) +end """ ff_controller(sys, L, K; comp_dc = true) diff --git a/test/runtests.jl b/test/runtests.jl index ebad5ece..1b631ccf 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -126,9 +126,19 @@ using Test include("../examples/uncertain.jl") end + @testset "lqg_mpc_disturbance" begin + @info "Testing LQG MPC example" + include("../examples/lqg_mpc_disturbance.jl") + end + @testset "mcm_nugap" begin @info "Testing mcm_nugap" include("test_mcm_nugap.jl") end + @testset "LinearMPC extension" begin + @info "Testing LinearMPC extension" + include("test_linearmpc_ext.jl") + end + end diff --git a/test/test_linearmpc_ext.jl b/test/test_linearmpc_ext.jl new file mode 100644 index 00000000..67b39426 --- /dev/null +++ b/test/test_linearmpc_ext.jl @@ -0,0 +1,89 @@ +using Test +using LinearAlgebra +using LinearMPC +using RobustAndOptimalControl + +# Test basic LQGProblem conversion (from docstring example) +Ts = 0.1 +sys = ss([1 Ts; 0 1], [0; Ts], [1 0], 0, Ts) + +# Create LQGProblem: Q1=state cost, Q2=input cost, R1=process noise, R2=measurement noise +lqg = LQGProblem(sys, I(2), I(1), I(2), 0.01*I(1)) + +# Convert to LinearMPC with constraints +mpc = LinearMPC.MPC(lqg; N=20, umin=[-0.3], umax=[0.3]) + +# Check basic properties +@test mpc.model.Ts == Ts +@test mpc.model.F == sys.A +@test mpc.model.G == sys.B + +# Simulate and verify it works +sim = LinearMPC.Simulation(mpc; N=100, r=[1.0, 0]) + + +# Check constraint satisfaction +@test all(sim.us .>= -0.3 - 1e-6) +@test all(sim.us .<= 0.3 + 1e-6) + +# Check that output converges toward reference +final_output = (sys.C * sim.xs[:, end])[1] +@test abs(final_output - 1.0) < 0.1 # Should be close to reference + +@testset "LQGProblem with Q3 (input rate penalty)" begin + # Test with input rate cost + Q3 = 0.5 * I(1) + mpc_q3 = LinearMPC.MPC(lqg; N=15, Q3, umin=[-0.5], umax=[0.5]) + + sim_q3 = LinearMPC.Simulation(mpc_q3; N=50, r=[0.5, 0]) + + # With Q3, control should be smoother (smaller rate of change) + du = diff(sim_q3.us, dims=2) + max_rate = maximum(abs.(du)) + @test max_rate < 0.1 # Rate should be limited due to Q3 penalty +end + +@testset "LQGProblem with terminal cost Qf" begin + # Test with terminal cost + Qf = 10.0 * I(2) # Higher terminal cost + mpc_qf = LinearMPC.MPC(lqg; N=10, Qf, umin=[-1.0], umax=[1.0]) + + sim_qf = LinearMPC.Simulation(mpc_qf; N=30, r=[0.3, 0]) + + # Should still work and converge + final_output = (sys.C * sim_qf.xs[:, end])[1] + @test abs(final_output - 0.3) < 0.1 +end + +@testset "LQGProblem MIMO system" begin + # Test with 2-input 2-output system + A = [0.9 0.1; 0.05 0.95] + B = [1.0 0.0; 0.0 1.0] + C = [1.0 0.0; 0.0 1.0] + D = zeros(2, 2) + sys_mimo = ss(A, B, C, D, Ts) + + lqg_mimo = LQGProblem(sys_mimo, I(2), I(2), I(2), 0.01*I(2)) + mpc_mimo = LinearMPC.MPC(lqg_mimo; N=15, + umin=[-0.5, -0.5], umax=[0.5, 0.5]) + + sim_mimo = LinearMPC.Simulation(mpc_mimo; N=50, r=[0.3, -0.2]) + + # Check dimensions + @test size(sim_mimo.us, 1) == 2 + @test size(sim_mimo.xs, 1) == 2 + + # Check constraints on both inputs + @test all(sim_mimo.us[1, :] .>= -0.5 - 1e-6) + @test all(sim_mimo.us[1, :] .<= 0.5 + 1e-6) + @test all(sim_mimo.us[2, :] .>= -0.5 - 1e-6) + @test all(sim_mimo.us[2, :] .<= 0.5 + 1e-6) +end + +@testset "LQGProblem continuous-time error" begin + # Test that continuous-time systems throw an error + sys_cont = ss([0 1; -1 -1], [0; 1], [1 0], 0) # Continuous-time + lqg_cont = LQGProblem(sys_cont, I(2), I(1), I(2), 0.01*I(1)) + + @test_throws ErrorException LinearMPC.MPC(lqg_cont; N=10) +end