diff --git a/src/frd.jl b/src/frd.jl index d1d86e46..9014578b 100644 --- a/src/frd.jl +++ b/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 + """ diff --git a/src/plotting.jl b/src/plotting.jl index 417be4d9..841738e4 100644 --- a/src/plotting.jl +++ b/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) diff --git a/test/test_frd.jl b/test/test_frd.jl index f62c8d21..ec78b942 100644 --- a/test/test_frd.jl +++ b/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 diff --git a/test/test_subspace.jl b/test/test_subspace.jl index f0ab35bd..acbdba82 100644 --- a/test/test_subspace.jl +++ b/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 \ No newline at end of file +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