diff --git a/src/model_augmentation.jl b/src/model_augmentation.jl
index 3ed115f4..d65bd3f2 100644
--- a/src/model_augmentation.jl
+++ b/src/model_augmentation.jl
@@ -119,13 +119,17 @@ end
 
 - `Bd`: The disturbance input matrix.
 """
-function add_resonant_disturbance(sys::AbstractStateSpace, ω, ζ, Bd::AbstractArray)
+function add_resonant_disturbance(sys::AbstractStateSpace, ω, ζ, Bd::AbstractArray; measurement=false)
     Ad = [-ζ -float(ω); ω -ζ]
     if isdiscrete(sys)
         Ad .*= sys.Ts
         Ad = exp(Ad)
     end
-    add_disturbance(sys, Ad, [Bd zeros(sys.nx)])
+    if measurement
+        add_measurement_disturbance(sys, Ad, Bd)
+    else
+        add_disturbance(sys, Ad, [Bd zeros(sys.nx)])
+    end
 end
 
 """
diff --git a/test/test_augmentation.jl b/test/test_augmentation.jl
index 148a43c2..49153ad3 100644
--- a/test/test_augmentation.jl
+++ b/test/test_augmentation.jl
@@ -80,6 +80,15 @@ Gd = add_resonant_disturbance(G, 1, 0, 1)
 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, input matrix, and measurement disturbance
+G = c2d(ss(tf(1.0, [1, 1])), 0.1)
+Gd = add_resonant_disturbance(G, 1, 0, [1.0 0.0], measurement=true)
+@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)])
+@test size(Gd.C, 2) == 3  # C matrix extended with disturbance states
+
 
 ##