add lqi and lqi controller (#127)

* add lqi and lqi controller

* args...

* up tests

Author: baggepinnen

src/RobustAndOptimalControl.jl

@@ -56,7 +56,7 @@ export h2synthesize
 include("h2_design.jl")
 
 export LQGProblem, sensitivity, input_sensitivity, output_sensitivity, comp_sensitivity, input_comp_sensitivity, output_comp_sensitivity, feedback_control, ff_controller, extended_controller, closedloop, static_gain_compensation, G_PS, G_CS, gangoffour
-export lqr3, dare3
+export lqr3, dare3, lqi, lqi_controller
 include("lqg.jl")
 
 export NamedStateSpace, named_ss, expand_symbol, measure, connect, sumblock, splitter

src/lqg.jl

@@ -603,3 +603,141 @@ end
 Base.@deprecate gangoffour2(P, C) gangoffour(P, C)
 Base.@deprecate gangoffourplot2(P, C) gangoffourplot(P, C)
 
+"""
+    lqi(sys::AbstractStateSpace, Q1::AbstractMatrix, Q2::AbstractMatrix, Q3::AbstractMatrix; 
+         integrator_outputs=1:sys.ny, ϵ=0)
+
+Calculate the feedback gain for the LQI (Linear-Quadratic-Integral) cost function:
+```math
+x_a^{T} Q_1 x_a + u^{T} Q_2 u
+```
+where `x_a = [x; e_i]` is the augmented state vector, `e_i` is the integral of the tracking error for the specified outputs.
+
+The system is augmented with integrators on the tracking error for the outputs specified in `integrator_outputs`.
+The augmented system has the form:
+```math
+\\begin{bmatrix} \\dot{x} \\\\ \\dot{x_i} \\end{bmatrix} = 
+\\begin{bmatrix} A & 0 \\\\ -C & 0 \\end{bmatrix} 
+\\begin{bmatrix} x \\\\ x_i \\end{bmatrix} + 
+\\begin{bmatrix} B \\\\ 0 \\end{bmatrix} u
+```
+
+where the integrator dynamics are `ẋᵢ = r-Cx` (tracking error), ensuring that the controller has integral action
+to eliminate steady-state errors.
+
+# Arguments:
+- `sys`: The system to control
+- `Q1`: Augmented state cost matrix (must be positive semi-definite)
+- `Q2`: Control cost matrix (must be positive definite)  
+- `integrator_outputs`: Vector of output indices to add integrators for (default: all outputs)
+- `ϵ`: Small positive number to move integrator poles slightly into the stable region (default: 0)
+
+# Returns:
+- `L`: The optimal feedback gain matrix for the augmented system
+
+# Example:
+```julia
+using ControlSystemsBase, RobustAndOptimalControl
+
+# Create a simple double integrator system
+A = [0 1; 0 0]
+B = [0; 1]
+C = [1 0]
+sys = ss(A, B, C, 0)
+
+# Design LQI controller
+Q1 = diagm([1, 1, 10]) # State cost + error-integral cost
+Q2 = [1;;]             # Control cost  
+
+L = lqi(sys, Q1, Q2)
+```
+
+See also [`lqi_controller`](@ref).
+"""
+function lqi(sys::AbstractStateSpace, Q1::AbstractMatrix, Q2::AbstractMatrix, args...; 
+             integrator_outputs=1:sys.ny, ϵ=0)
+    
+    # Validate inputs
+    all(1 .≤ integrator_outputs .≤ sys.ny) || throw(ArgumentError("All integrator_outputs must be valid output indices"))
+    length(integrator_outputs) == 0 && throw(ArgumentError("At least one output must have an integrator"))
+
+    size(Q1, 1) == sys.nx+length(integrator_outputs) || throw(ArgumentError("Q1 must have size $(sys.nx+length(integrator_outputs))×$(sys.nx+length(integrator_outputs))"))
+    size(Q2, 1) == sys.nu || throw(ArgumentError("Q2 must have size $(sys.nu)×$(sys.nu)"))
+    
+    sys_aug = add_output_integrator(sys, integrator_outputs; ϵ=ϵ, neg=true)
+    lqr(sys_aug, Q1, Q2, args...)
+end
+
+
+"""
+    lqi_controller(G, obs, Q1, Q2)
+
+Return an LQI controller with reference and measurement inputs `[r; y]` designed for the LQI problem where the plant state `x` is augmented with output-error integrators to form the augmented state `x_a = [x; e_i]`.
+
+- `Q1`: Penalty matrix for the augmented state (must be positive semi-definite)
+- `Q2`: Penalty matrix for the control input (must be positive definite)
+- `obs`: An observer for `G` constructed using `observer_predictor(G, ..., output_state=true)`
+
+Example:
+```
+G   = ss([0 32;-31.25 -0.4],[0; 2.236068],[0.0698771 0],0)
+Q   = diagm([0,5])
+R   = [1.0;;]
+K   = kalman(G,Q,R)
+obs = observer_predictor(G, K; output_state=true)
+
+Q1 = diagm([0.488,0,100])
+Q2 = [1/100;;]
+L  = lqi(G,Q1,Q2)
+C = lqi_controller(G, obs, Q1, Q2)
+
+Gn = named_ss(G)
+H = feedback(C, Gn, w1 = :y_plant_r, z2=Gn.y, u1=:y_plant, pos_feedback=true)
+plot(
+    plot(step(H, 50)),
+    gangoffourplot(G, -C[:, 2])
+)
+```
+"""
+function lqi_controller(G, obs, Q1, Q2, args...)
+    L = named_ss(ss(lqi(G, Q1, Q2, args...)), name="L", x=:x_L, y=:y_L, u=:u_L)
+    G isa NamedStateSpace || (G = named_ss(G, name="plant", x=:x_plant, y=:y_plant, u=:u_plant))
+    obs isa NamedStateSpace || (obs = named_ss(obs, name="observer", x=:x_observer, y=:y_observer, u=:u_observer))
+    
+    s = tf('s')
+    (; nx, nu, ny) = G
+    nr = size(L, 2) - nx
+    te = G.timeevol
+    nr = ny
+    aug_state_inds = 1:nx
+    aug_integrator_inds = nx+1:size(L, 2)
+    
+    refs = Symbol.(string.(G.y) .* "_r")
+    feedback_y = Symbol.(string.(G.y) .* "_fb")
+
+    add_feedback = named_ss(ss([I(nr) -I(nr)], te), u=[refs; feedback_y], y=:e)
+    int0 = iscontinuous(G) ? ss(1/s) : ss(c2d(1/s, G.Ts))
+    integrator = named_ss(I(nr) .* int0, u=:e, y=:ie, x=:x_int)
+    unit_gain = named_ss(ss(I(nr), te), u=G.y) # The plant output is not available as input in any of the components, so we introduce this unit gain to introduce a component with a plant-input input. This is then fed into multiple places
+
+    external_inputs = [
+        refs; G.y;
+    ]
+    external_outputs = [
+        L.y;
+    ]
+
+    observer_input_inds = 1:nu
+    observer_output_inds = (1:ny) .+ nu
+
+    connections = [
+        add_feedback.y .=> integrator.u;
+        integrator.y .=> L.u[aug_integrator_inds];
+        obs.y .=> L.u[aug_state_inds];
+        L.y .=> obs.u[observer_input_inds];
+        unit_gain.y .=> obs.u[observer_output_inds];
+        unit_gain.y .=> add_feedback.u[nr+1:end]  
+    ]
+    # Negate obs to output -x̂
+    connect([add_feedback, integrator, L, -obs, unit_gain], connections; external_inputs, external_outputs)
+end

src/model_augmentation.jl

@@ -166,22 +166,32 @@ Gd = add_output_integrator(add_output_differentiator(G), 1)
 
 Note: numerical integration is subject to numerical drift. If the output of the system corresponds to, e.g., a velocity reference and the integral to position reference, consider methods for mitigating this drift.
 """
-function add_output_integrator(sys::AbstractStateSpace{<: Discrete}, ind=1; ϵ=0)
+function add_output_integrator(sys::AbstractStateSpace{<: Discrete}, ind=1; ϵ=0, neg=false)
     int = tf(1.0*sys.Ts, [1, -(1-ϵ)], sys.Ts)
+    neg && (int = int*(-1))
     𝟏 = tf(1.0,sys.Ts)
     𝟎 = tf(0.0,sys.Ts)
     M = [i==j ? 𝟏 : 𝟎 for i = 1:sys.ny, j = 1:sys.ny]
-    M = [M; permutedims([i==ind ? int : 𝟎 for i = 1:sys.ny])]
-    tf(M)*sys
+    M = [M; permutedims([i ∈ ind ? int : 𝟎 for i = 1:sys.ny])]
+    nx = sys.nx
+    nr = length(ind)
+    p = [(1:nx).+nr; 1:nr]
+    T = (1:nx+nr) .== p'
+    similarity_transform(tf(M)*sys, T)
 end
 
-function add_output_integrator(sys::AbstractStateSpace{Continuous}, ind=1; ϵ=0)
+function add_output_integrator(sys::AbstractStateSpace{Continuous}, ind=1; ϵ=0, neg=false)
     int = tf(1.0, [1, ϵ])
+    neg && (int = int*(-1))
     𝟏 = tf(1.0)
     𝟎 = tf(0.0)
     M = [i==j ? 𝟏 : 𝟎 for i = 1:sys.ny, j = 1:sys.ny]
-    M = [M; permutedims([i==ind ? int : 𝟎 for i = 1:sys.ny])]
-    tf(M)*sys
+    M = [M; permutedims([i ∈ ind ? int : 𝟎 for i = 1:sys.ny])]
+    nx = sys.nx
+    nr = length(ind)
+    p = [(1:nx).+nr; 1:nr]
+    T = (1:nx+nr) .== p'
+    similarity_transform(tf(M)*sys, T)
 end
 
 """

test/runtests.jl

@@ -65,6 +65,11 @@ using Test
         include("test_lqg.jl")
     end
 
+    @testset "LQI" begin
+        @info "Testing LQI"
+        include("test_lqi.jl")
+    end
+
     @testset "Named systems" begin
         @info "Testing Named systems"
         include("test_named_systems2.jl")

test/test_augmentation.jl

@@ -96,15 +96,15 @@ Gd2 = [tf(1,1); tf([1, -1], [1], 1)]*tf(G)
 ## Int
 Gd = add_output_integrator(G)
 Gd2 = [tf(1,1); tf(1, [1, -1], 1)]*G
-@test Gd ≈ Gd2
+@test tf(Gd) ≈ tf(Gd2)
 @test sminreal(Gd[1,1]) == G # Exact equivalence should hold here
 @test Gd.nx == 4 # To guard agains changes in realization of tf as ss
 
 
 Gc = ssrand(1,1,3, proper=true)
 Gdc = add_output_integrator(Gc)
 Gd2c = [tf(1); tf(1, [1, 0])]*Gc
-@test Gdc ≈ Gd2c
+@test tf(Gdc) ≈ tf(Gd2c)
 @test sminreal(Gdc[1,1]) == Gc # Exact equivalence should hold here
 @test Gdc.nx == 4 # To guard agains changes in realization of tf as ss
 

test/test_lqi.jl

@@ -0,0 +1,30 @@
+using Test
+using ControlSystemsBase
+using RobustAndOptimalControl
+using LinearAlgebra
+using Plots
+
+
+G = ss([0 32;-31.25 -0.4],[0; 2.236068],[0.0698771 0],0)
+Q = diagm([0,5])
+R = [1.0;;]
+K = kalman(G,Q,R)
+obs = observer_predictor(G,K; output_state=true)
+
+Q1 = diagm([0.488,0,100])
+Q2 = [1/100;;]
+L = lqi(G,Q1,Q2)
+
+C0 = RobustAndOptimalControl.lqi_controller(G, obs, Q1, Q2)
+
+@test C0.nu == 2
+@test 0 ∈ poles(C0)
+
+Gn = named_ss(G)
+H = feedback(C0, Gn, w1 = :y_plant_r, z2=Gn.y, u1=:y_plant, pos_feedback=true)
+
+@test dcgain(H)[2] ≈ 1
+
+res = step(H, 50)
+@test res.y[:, end] ≈ [dcgain(feedback(-C0[:, 2], G))[]; 1.0]
+# plot(res)