Merge pull request #93 from JuliaControl/low_meas

add options to disturbance augmentation

Author: baggepinnen

src/model_augmentation.jl

@@ -23,6 +23,13 @@ end
 
 """
     add_measurement_disturbance(sys::StateSpace{Continuous}, Ad::Matrix, Cd::Matrix)
+
+Create the system
+```
+Ae = [A 0; 0 Ad]
+Be = [B; 0]
+Ce = [C Cd]
+```
 """
 function add_measurement_disturbance(sys::AbstractStateSpace, Ad::AbstractMatrix, Cd::AbstractMatrix)
     A,B,C,D = ControlSystemsBase.ssdata(sys)
@@ -52,23 +59,28 @@ end
 
 """
     add_low_frequency_disturbance(sys::StateSpace; ϵ = 0, measurement = false)
+    add_low_frequency_disturbance(sys::StateSpace, Cd; ϵ = 0, measurement = false)
 
 Augment `sys` with a low-frequency (integrating if `ϵ=0`) disturbance model.
 If an integrating input disturbance is used together with an observer, the controller will have integral action.
 
+- `Cd`: If adding an input disturbance. this matrix indicates how the disturbance states affect the states of `sys`, and defaults to `sys.B`. If `measurement=true`, this matrix indicates how the disturbance states affect the outputs of `sys`, and defaults to `I(sys.ny)`.
+
 # Arguments:
 - `ϵ`: Move the integrator pole `ϵ` into the stable region.
 - `measurement`: If true, the disturbance is a measurement disturbance, otherwise it's an input diturbance. 
 """
-function add_low_frequency_disturbance(sys::AbstractStateSpace; ϵ=0, measurement=false)
+function add_low_frequency_disturbance(sys::AbstractStateSpace, Cd::Union{Nothing, AbstractMatrix} = nothing; ϵ=0, measurement=false)
     nx,nu,ny = sys.nx,sys.nu,sys.ny
-    Ad = -ϵ*I(nu)
-    isdiscrete(sys) && (Ad += I)
     if measurement
-        Cd = I(nu)
+        Ad = -ϵ*I(ny)
+        isdiscrete(sys) && (Ad += I)
+        Cd === nothing && (Cd = I(ny))
         add_measurement_disturbance(sys, Ad, Cd)
     else
-        Cd = sys.B
+        Ad = -ϵ*I(nu)
+        isdiscrete(sys) && (Ad += I)
+        Cd === nothing && (Cd = sys.B)
         add_disturbance(sys, Ad, Cd)
     end
 end

test/test_augmentation.jl

@@ -15,6 +15,42 @@ Gd.C[end] == 1
 @test rank(ctrb(Gd)) == 3
 @test any(isapprox(0, atol=eps()), poles(Gd))
 
+G = ssrand(4,2,3, proper=true)
+Gd = add_low_frequency_disturbance(G)
+@test Gd.nx == G.nx + G.nu
+@test rank(obsv(Gd)) == Gd.nx
+@test rank(ctrb(Gd)) == G.nx
+@test any(isapprox(0, atol=eps()), poles(Gd))
+@test Gd.A[end-1:end, end-1:end] == 0I
+
+G = ssrand(2,4,3, proper=true)
+Gd = add_low_frequency_disturbance(G, measurement=true)
+@test Gd.nx == G.nx + G.ny
+@test rank(obsv(Gd)) == G.nx + G.ny
+@test rank(ctrb(Gd)) == G.nx
+@test any(isapprox(0, atol=eps()), poles(Gd))
+@test Gd.C[:, end-1:end] == I
+@test Gd.A[end-1:end, end-1:end] == 0I
+
+# Discrete time
+G = ssrand(4,2,3, proper=true, Ts=0.1)
+Gd = add_low_frequency_disturbance(G)
+@test Gd.nx == G.nx + G.nu
+@test rank(obsv(Gd)) == Gd.nx
+@test rank(ctrb(Gd)) == G.nx
+@test any(isapprox(1, atol=eps()), poles(Gd))
+@test Gd.A[end-1:end, end-1:end] == I
+
+G = ssrand(2,4,3, proper=true, Ts=0.1)
+Gd = add_low_frequency_disturbance(G, measurement=true)
+@test Gd.nx == G.nx + G.ny
+@test rank(obsv(Gd)) == G.nx + G.ny
+@test rank(ctrb(Gd)) == G.nx
+@test any(isapprox(1, atol=eps()), poles(Gd))
+@test Gd.C[:, end-1:end] == I
+@test Gd.A[end-1:end, end-1:end] == I
+
+
 G = ssrand(1,1,3, proper=true)
 Gd = add_resonant_disturbance(G, 1, 0, 3)
 @test Gd.nx == 5