add LinearMPC extension

Author: baggepinnen

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"

ext/RobustAndOptimalControlLinearMPCExt.jl

@@ -0,0 +1,95 @@
+"""
+    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
+
+
+"""
+    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 !ControlSystemsBase.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,

test/runtests.jl

@@ -131,4 +131,9 @@ using Test
         include("test_mcm_nugap.jl")
     end
 
+    @testset "LinearMPC extension" begin
+        @info "Testing LinearMPC extension"
+        include("test_linearmpc_ext.jl")
+    end
+
 end

test/test_linearmpc_ext.jl

@@ -0,0 +1,93 @@
+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
+@test mpc.settings.Np == 20
+
+# Simulate and verify it works
+sim = LinearMPC.Simulation(mpc; N=100, r=[1.0, 0])
+
+# Check that simulation ran
+@test size(sim.xs, 2) == 101  # 100 steps + initial state
+@test size(sim.us, 2) == 100
+
+# 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.3  # 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