add and test continuous-time output integrator augmentation

src/model_augmentation.jl

@@ -84,7 +84,7 @@ Augment `sys` with a resonant disturbance model.
 - `Ai`: The affected state
 - `measurement`: If true, the disturbace is acting on the output, this will cause the controller to have zeros at ω (roots of poly s² + 2ζωs + ω²). If false, the disturbance is acting on the input, this will cause the controller to have poles at ω (roots of poly s² + 2ζωs + ω²).
 """
-function add_resonant_disturbance(sys::AbstractStateSpace{Continuous}, ω, ζ, Ai::Integer; measurement=false)
+function add_resonant_disturbance(sys::AbstractStateSpace, ω, ζ, Ai::Integer; measurement=false)
     nx,nu,ny = sys.nx,sys.nu,sys.ny
     if measurement
         1 ≤ Ai ≤ sys.ny || throw(ArgumentError("Ai must be a valid output index"))
@@ -96,6 +96,9 @@ function add_resonant_disturbance(sys::AbstractStateSpace{Continuous}, ω, ζ, A
         Cd[Ai, 1] = 1
     end
     Ad = [-ζ -ω; ω -ζ]
+    if isdiscrete(sys)
+        Ad = exp(Ad * sys.Ts)
+    end
     measurement ? add_measurement_disturbance(sys, Ad, Cd) : add_disturbance(sys, Ad, Cd)
 end
 
@@ -160,6 +163,15 @@ function add_output_integrator(sys::AbstractStateSpace{<: Discrete}, ind=1; ϵ=0
     tf(M)*sys
 end
 
+function add_output_integrator(sys::AbstractStateSpace{Continuous}, ind=1; ϵ=0)
+    int = tf(1.0, [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
+end
+
 """
     add_input_integrator(sys::StateSpace, ui = 1, ϵ = 0)
 

test/test_augmentation.jl

@@ -36,6 +36,14 @@ Gd = add_resonant_disturbance(G, 1, 0, [1.0])
 allapproxin(a, b) = all(any(a .≈ b', dims=2))
 @test allapproxin(poles(Gd), [eigvals(exp([0 -1; 1 0]*0.1)); exp(-1*0.1)])
 
+# Discrete time and input index
+G = c2d(ss(tf(1.0, [1, 1])), 0.1)
+Gd = add_resonant_disturbance(G, 1, 0, 1)
+@test sminreal(Gd) == G
+@test Gd.nx == 3
+allapproxin(a, b) = all(any(a .≈ b', dims=2))
+@test allapproxin(poles(Gd), [eigvals(exp([0 -1; 1 0]*0.1)); exp(-1*0.1)])
+
 
 ##
 
@@ -56,6 +64,14 @@ Gd2 = [tf(1,1); tf(1, [1, -1], 1)]*G
 @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 sminreal(Gdc[1,1]) == Gc # Exact equivalence should hold here
+@test Gdc.nx == 4 # To guard agains changes in realization of tf as ss
+
 Gd = add_input_integrator(G)
 @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