add support for multiple coherence (#194)

* add support for multiple coherence

* tweak tol

Author: baggepinnen

src/frd.jl

@@ -260,7 +260,9 @@ from which it is obvious that ``0 ≤ κ² ≤ 1`` and that κ² is close to 1 i
 """
 function coherence(d::AbstractIdData; n = length(d) ÷ 10, noverlap = n ÷ 2, window = hamming, method=:welch, σ = 0.05)
     noutputs(d) == 1 || throw(ArgumentError("coherence only supports a single output. Index the data object like `d[i,j]` to obtain the `i`:th output and the `j`:th input."))
-    ninputs(d) == 1 || throw(ArgumentError("coherence only supports a single input. Index the data object like `d[i,j]` to obtain the `i`:th output and the `j`:th input."))
+    if ninputs(d) > 1
+        return multiple_coherence(d; n, noverlap, window, σ)
+    end
     y, u, h = vec(output(d)), vec(input(d)), sampletime(d)
     if method === :welch
         Syy, Suu, Syu = wcfft(y, u, n = n, noverlap = noverlap, window = window)
@@ -291,6 +293,80 @@ function wcfft(y, u; n = length(y) ÷ 10, noverlap = n ÷ 2, window = hamming)
     Syy, Suu, Syu
 end
 
+function multiple_coherence(d::AbstractIdData; n = length(d) ÷ 10, noverlap = n ÷ 2, window = hamming, σ = 0.05)
+    noutputs(d) == 1 || throw(ArgumentError("multiple_coherence only supports a single output."))
+    ninputs(d) > 1   || throw(ArgumentError("multiple_coherence requires multiple inputs. Use `coherence` for single input."))
+    
+    y = vec(output(d))
+    u = time1(input(d))
+    h = sampletime(d)
+
+    Syy, Suu, Syu = wcfft_mimo(y, u, n = n, noverlap = noverlap, window = window)
+
+    n_freqs = length(Syy)
+    γ2 = zeros(n_freqs)
+    
+    for i in 1:n_freqs
+        s_xy = conj(Syu[i]) 
+        num = real(dot(s_xy, Suu[i] \ s_xy))
+        γ2[i] = num / Syy[i]
+    end
+
+    γ2 = clamp.(γ2, 0.0, 1.0)
+
+    Sch = FRD(freqvec(h, γ2), γ2)
+    return Sch
+end
+
+function wcfft_mimo(y::AbstractVector, u::AbstractMatrix; n = length(y) ÷ 10, noverlap = n ÷ 2, window = hamming)
+    n_samples, n_inputs = size(u)
+    n_samples == length(y) || throw(DimensionMismatch("Input and output lengths differ"))
+
+    win, norm2 = DSP.Periodograms.compute_window(window, n)
+    
+    # Dimensions for FFT
+    nfft = nextfastfft(n)
+    n_freqs = nfft ÷ 2 + 1
+    
+    # Initialize Accumulators
+    Syy = zeros(Float64, n_freqs)
+    Suu = [zeros(ComplexF64, n_inputs, n_inputs) for _ in 1:n_freqs]
+    Syu = [zeros(ComplexF64, n_inputs) for _ in 1:n_freqs] # Vector of vectors
+
+    # Calculate window stride
+    step = n - noverlap
+    
+    # Segment Loop (Manual splitting to handle Matrix u safely)
+    for i in 1:step:(n_samples - n + 1)
+        # Extract segments and apply window
+        y_seg = y[i:i+n-1] .* win
+        u_seg = u[i:i+n-1, :] .* win # Broadcast window across columns
+
+        # Compute FFTs
+        Y_f = rfft(y_seg)
+        U_f = rfft(u_seg) # FFT along time dimension (dim 1)
+        
+        # Accumulate Spectra per frequency bin
+        for k in 1:n_freqs
+            Y_k = Y_f[k]       # Scalar
+            U_k = U_f[k, :]    # Vector (1 x Inputs)
+
+            # Auto-spectrum Output (Scalar)
+            Syy[k] += abs2(Y_k)
+
+            # Auto-spectral Matrix Input (Matrix: Inputs x Inputs)
+            # Outer product: U * U'
+            Suu[k] .+= U_k * U_k' 
+
+            # Cross-spectral Vector (Vector: Inputs)
+            # We compute S_yx = Y * U' (Output * conj(Input))
+            Syu[k] .+= Y_k .* conj.(U_k)
+        end
+    end
+
+    return Syy, Suu, Syu
+end
+
 
 
 """

src/plotting.jl

@@ -268,7 +268,6 @@ coherenceplot
         ArgumentError("Call like this: coherenceplot(iddata; hz=false)")
     d = p.args[1]
     d isa AbstractIdData || throw(ae)
-    ninputs(d) == 1 || throw(ArgumentError("coherenceplot only supports a single input. Index the data object like `d[i,j]` to obtain the `i`:th output and the `j`:th input."))
     if length(p.args) >= 2
         kwargs = p.args[2]
     else
@@ -282,7 +281,7 @@ coherenceplot
     title --> "Coherence"
     label --> false
     for i = 1:d.ny
-        frd = coherence(d[i,1]; kwargs...)
+        frd = coherence(d[i,:]; kwargs...)
         @series begin
             inds = findall(x -> x == 0, frd.w)
             useinds = setdiff(1:length(frd.w), inds)

test/test_frd.jl

@@ -58,6 +58,31 @@ plot!(G2, subplot = 1, lab = "G Est W", alpha = 0.3, title = "Process model")
 plot!(√N2, subplot = 2, lab = "N Est W", alpha = 0.3, title = "Noise model")
 
 
+
+@testset "multiple coherence" begin
+    @info "Testing multiple coherence"
+    nu = 2
+    nx = 2
+    ny = 1
+    N = 10000
+    u = randn(nu, N)
+    sys = ssrand(ny, nu, nx, Ts=1, proper=true)
+    sys.B .= I(nu)
+    y, t, x = lsim(sys, u)
+    y .+= sin.(1 .* t')
+    d = iddata(y, u, 1)
+    c = coherence(d)
+    plot(c, yscale=:identity)
+    coherenceplot(d)
+
+    @test mean(c.r) > 0.99
+    using ControlSystemIdentification: rad
+    @test mean(c[0.999rad:1.001rad].r) < 0.2 # low coherence at disturbance frequency
+end
+
+
+
+
 for op in (+, -, *, /)
     @test op(G, G) isa FRD
 end

test/test_subspace.jl

@@ -401,4 +401,33 @@ using Statistics
 
     @test meanmodel ≈ era(ds, 2, 50, 50, round(Int, 10/Ts), p=1)
     @test meanmodel ≈ era(ds, 2; m=50, n=50, l=round(Int, 10/Ts), p=1)
-end
+end
+
+## What happens when there is a constant input offset?
+@testset "constant bias input" begin
+    @info "Testing constant bias input"
+nu = 1
+nx = 2
+ny = 1
+off = 10
+N = 1000
+u = [randn(nu, N); ones(1, N)*off]
+sys = ssrand(ny, nu, nx, Ts=1, proper=true)
+sys = ss(sys.A, sys.B .* [1 1], sys.C, 0, sys.Ts)
+y, t, x = lsim(sys, u)
+d = iddata(y, u[1:1, :], 1)
+sys_id = subspaceid(d, nx+1)
+
+# @test hinfnorm(sys[1,1] - sys_id.sys)[1] < 1e-10
+@test observability(sys_id, atol=1e-6).isobservable
+@test !controllability(sys_id, atol=1e-6).iscontrollable
+
+# bodeplot([sys[1,1], sys_id.sys], exp10.(LinRange(-3, log10(pi), 100)), plotphase=false)
+
+#=
+With a constant input offset, subspace id can compensate exactly as long as the state dimension is increased by one.
+Modal form of the estimated system gets one pole in 1, and the B and C matrix entries corresponding to this state dimension are zero.
+
+The disturbance state is observable but not controllable
+=#
+end