add LinearMPC extension

Project.toml

@@ -23,13 +23,20 @@ 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"
 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"]

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"

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.

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])
+

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

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,

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)

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