diff --git a/docs/src/examples/closed_loop_id.md b/docs/src/examples/closed_loop_id.md index b1e414e2..d299cfd6 100644 --- a/docs/src/examples/closed_loop_id.md +++ b/docs/src/examples/closed_loop_id.md @@ -21,12 +21,13 @@ y_{k+1} &= (a + bL)y_k + b r_k ``` The very first experiment below will illustrate the problem when there is no excitation through $r$. -We start by defining a model of the true system, a function that simulates some data and adds colored output noise, as well as a function that estimates three different models and plots their frequency responses. We will consider three estimation methods +We start by defining a model of the true system, a function that simulates some data and adds colored output noise, as well as a function that estimates four different models and plots their frequency responses. We will consider four estimation methods 1. [`arx`](@ref), a prediction-error approach based on a least-squares estimate. 2. A subspace-based method [`subspaceid`](@ref), known to be biased in the presence of output feedback. -3. The prediction-error method (PEM) [`newpem`](@ref) +3. [`pbsid`](@ref), a predictor-based subspace method (PBSIDopt from Chiuso 2007) designed to handle closed-loop data. +4. The prediction-error method (PEM) [`newpem`](@ref) -The ARX and PEM methods are theoretically unbiased in the presence of output feedback, see [^Ljung], while the subspace-based method is not. (Note: the subspace-based method is used to form the initial guess for the iterative PEM algorithm) +The ARX, PBSID and PEM methods are theoretically unbiased in the presence of output feedback, see [^Ljung], while the standard subspace-based method is not. (Note: the subspace-based method is used to form the initial guess for the iterative PEM algorithm) ```@example closedloop using ControlSystemsBase, ControlSystemIdentification, Plots @@ -46,20 +47,20 @@ function generate_data(u; T) d end -function estimate_and_plot(d, nx=1; title, focus=:prediciton) +function estimate_and_plot(d, nx=1; title, focus=:prediction) Gh1 = arx(d, 1, 1) sys0 = subspaceid(d, nx; focus) - tf(sys0) + + sys_pbsid = pbsid(d, nx, weight=1) Gh2, _ = ControlSystemIdentification.newpem(d, nx; sys0, focus) - tf(Gh2) figb = bodeplot( - [G, Gh1, sys0.sys, Gh2.sys]; + [G, Gh1, sys0.sys, sys_pbsid.sys, Gh2.sys]; ticks = :default, title, - lab = ["True system" "ARX" "Subspace" "PEM"], + lab = ["True system" "ARX" "Subspace" "PBSID" "PEM"], plotphase = false, ) @@ -92,14 +93,14 @@ title = "-Lx + 5sin(t)" estimate_and_plot(generate_data(u, T=80), title=title*", T=80") ``` -In this case, all but the subspace-based method performs quite well +In this case, ARX and PEM perform well, PBSID performs somewhere in the middle, while the standard subspace method performs pooly ```@example closedloop estimate_and_plot(generate_data(u, T=8000), title=title*", T=8000") ``` -More data does not help the subspace method. +More data does not help the poorly perfoming methods. -With **a more complex excitation** (random white-spectrum noise), all methods perform well +With **a more complex excitation** (random white-spectrum noise), all methods perform well despite a small amount of data ```@example closedloop L = 0.5 # Feedback gain u = -L*x u = (x, t) -> -L * x .+ 5randn() @@ -107,10 +108,6 @@ title = "-Lx + 5randn()" estimate_and_plot(generate_data(u, T=80), title=title*", T=80") ``` -and even slightly better with more data. -```@example closedloop -estimate_and_plot(generate_data(u, T=8000), title=title*", T=8000") -``` If the **feedback is strong but the excitation is weak**, the results are rather poor for all methods, it's thus important to have enough energy in the excitation compared to the feedback path. ```@example closedloop @@ -120,7 +117,7 @@ title = "-Lx + 0.1randn()" estimate_and_plot(generate_data(u, T=80), title=title*", T=80") ``` -In this case, we can try to increase the model order of the PEM and subspace-based methods to see if they are able to learn the noise model (which has two poles) +In this case, we can try to increase the model order of the PEM, PBSID and subspace-based methods to see if they are able to learn the noise model (which has two poles) ```@example closedloop estimate_and_plot(generate_data(u, T=8000), 3, title=title*", T=8000") ``` diff --git a/src/ControlSystemIdentification.jl b/src/ControlSystemIdentification.jl index a13b0a9b..e96762e8 100644 --- a/src/ControlSystemIdentification.jl +++ b/src/ControlSystemIdentification.jl @@ -47,7 +47,7 @@ export iddata, export AbstractPredictionStateSpace, PredictionStateSpace, N4SIDStateSpace, pem, newpem, structured_pem, prediction_error, prediction_error_filter, predictiondata, predict, simulate, noise_model, estimate_x0 -export n4sid, subspaceid, era, okid, find_similarity_transform, schur_stab +export n4sid, subspaceid, era, okid, find_similarity_transform, schur_stab, pbsid export getARXregressor, getARregressor, find_na, @@ -84,6 +84,7 @@ include("pem.jl") include("arx.jl") include("subspace.jl") include("subspace2.jl") +include("pbsid.jl") include("spectrogram.jl") include("frequency_weights.jl") include("basis_functions.jl") diff --git a/src/pbsid.jl b/src/pbsid.jl new file mode 100644 index 00000000..14477187 --- /dev/null +++ b/src/pbsid.jl @@ -0,0 +1,290 @@ +""" + pbsid( + data::InputOutputData, + nx = :auto; + verbose = false, + f = nx === :auto ? min(length(data) ÷ 20, 50) : 2nx + 10, # future horizon + p = f, # past horizon (must be >= f) + λ = 0.0, # regularization parameter (Tikhonov) + weight = 0, # 0 = PBSIDopt, 1 = CVA-like + zeroD = false, # force D matrix to be zero + stable = true, + zeroD = false, + svd::F1 = svd!, + scaleU = true, + ) + +Estimate a state-space model using the PBSID-opt algorithm. + +# Arguments +- `data`: Identification data [`iddata`](@ref) +- `nx`: Model order (number of states). Use `:auto` for automatic selection. +- `verbose`: Print diagnostic information +- `f`: Future horizon for predictor construction +- `p`: Past horizon for VARX model (must be >= f) +- `λ`: Tikhonov regularization parameter for VARX estimation +- `weight`: Weighting scheme: 0 = PBSIDopt (default), 1 = CVA-like recursive weighting +- `zeroD`: If true, exclude direct feedthrough term D from VARX regression +- `stable`: Stabilize unstable A matrix using eigenvalue reflection +- `zeroD`: Force the D matrix to be zero in the final model +- `svd`: SVD function to use +- `scaleU`: Rescale inputs to have unit standard deviation + +# Returns +`N4SIDStateSpace` containing the identified system and covariance matrices. + +# References +- Chiuso, A. (2007). The role of vector autoregressive modeling in predictor-based subspace identification. +""" +function pbsid( + d::InputOutputData, + nx = :auto; + verbose = false, + f = nx === :auto ? min(length(d) ÷ 20, 50) : 2nx + 10, + p = f, + weight = 0, + stable = true, + zeroD = false, + svd::F1 = svd!, + scaleU = true, +) where {F1} + + nx !== :auto && f < nx && throw(ArgumentError("f must be at least nx")) + p >= f || throw(ArgumentError("p (past horizon) must be >= f (future horizon)")) + + y, u = copy(time2(output(d))), copy(time2(input(d))) + Ts = sampletime(d) + + if scaleU + CU = vec(std(u, dims=2)) + CU[CU .== 0] .= 1 + u ./= CU + end + + s, X, VARX = _pbsid_compute_x(u, y, f, p; weight, zeroD, svd) + S = s.S + + if nx === :auto + nx = sum(S .> sqrt(S[1] * S[end])) + verbose && @info "Choosing order $nx" + end + + verbose && @info "PBSIDopt: nx=$nx, f=$f, p=$p, weight=$weight" + + x = X[1:nx, :] + + A, B, C, D, K = _pbsid_mats(x, u, y, f, p; stable, verbose) + + + # Return as N4SIDStateSpace + # N4SIDStateSpace(sys, K, Q, R, S, P, x0, singular_values) + # We'll compute approximate covariances from residuals + N_state = size(x, 2) + u_aligned = u[:, p+1:p+N_state] + y_aligned = y[:, p+1:p+N_state] + + # Compute residuals for covariance estimation + e = y_aligned - C * x - D * u_aligned + x_pred = A * x[:, 1:end-1] + B * u_aligned[:, 1:end-1] + w = x[:, 2:end] - x_pred + + fve = 1 - sum(abs2, e) / sum(abs2, y_aligned .- mean(y_aligned, dims=2)) + + Ns = size(e, 2) + R = (e * e') / Ns + Q = (w * w') / (Ns - 1) + Scov = zeros(nx, size(y, 1)) # cross-covariance (approximate as zero) + + # Compute P from DARE if possible + P = try + R_reg = R + 1e-10 * I + Ptmp, _, _ = ared(A', C', Q, R_reg) + Ptmp + catch + Matrix{Float64}(I(nx)) # fallback to identity + end + + x0 = x[:, 1] # initial state estimate + + if scaleU + B = B ./ CU' + D = D ./ CU' + end + + sys = ss(A, B, C, D, Ts) + + return N4SIDStateSpace(sys, Q, R, Scov, K, P, x0, s, fve) +end + + + +function _pbsid_compute_x(u, y, f, p; weight=0, zeroD=false, svd) + # Get dimensions + r, N = size(u) # r inputs, N samples + l = size(y, 1) # l outputs + m = r + l # combined dimension + + # Check dimensions + @assert size(y, 2) == N "u and y must have the same number of samples" + @assert p >= f "p must be >= f" + + # Number of usable samples after forming past/future windows + j = N - p - f + 1 + + # Construct past data matrix Z (stacked [u; y] history for p steps) + # Z has dimension (p*m) × j + Z = zeros(p * m, j) + for i in 1:p + Z[(i-1)*m+1 : (i-1)*m+r, :] = u[:, p-i+1 : p-i+j] + Z[(i-1)*m+r+1 : i*m, :] = y[:, p-i+1 : p-i+j] + end + + # Construct future output matrix Y (stacked outputs for f steps) + # Y has dimension (f*l) × j + Y = zeros(f * l, j) + for i in 1:f + Y[(i-1)*l+1 : i*l, :] = y[:, p+i : p+i+j-1] + end + + # If zeroD, we need to construct future input matrix and regress it out + if zeroD + # Construct future input matrix Uf + Uf = zeros(f * r, j) + for i in 1:f + Uf[(i-1)*r+1 : i*r, :] = u[:, p+i : p+i+j-1] + end + Zext = [Z; Uf] + VARXext = Y / Zext + VARX = VARXext[:, 1:p*m] + else + VARX = Y / Z + end + + LK = zeros(f * l, p * m) + LK .= VARX + + # Apply CVA-like weighting + if weight == 1 + # CVA: weight by inverse square root of output covariance + Yf = Y + Wf = Yf * Yf' / j + Wf_inv_sqrt = inv(sqrt(Symmetric(Wf))) + LK = Wf_inv_sqrt * LK + end + + LKZ = LK * Z + s = svd(LKZ) + S = s.S + + X = Diagonal(S) * s.Vt + return s, X, VARX +end + +function _pbsid_mats(x, u, y, f, p; stable=false, verbose=false) + nx = size(x, 1) # state dimension + nu = size(u, 1) # number of inputs + ny = size(y, 1) # number of outputs + N = size(x, 2) # number of samples in state sequence + + # Align u, y with x by removing first p samples + # x starts at time p+1, so we need u and y from p+1 onwards + u_aligned = u[:, p+1:p+N] + y_aligned = y[:, p+1:p+N] + + # Estimate C and D from output equation: y = C*x + D*u + # [C D] = Y * pinv([X; U]) + XU = [x[:, 1:end-1]; u_aligned[:, 1:end-1]] + Y_out = y_aligned[:, 1:end-1] + + CD = Y_out / XU + C = CD[:, 1:nx] + D = CD[:, nx+1:end] + + # Compute innovation (residual) e = y - C*x - D*u + e = y_aligned - C * x - D * u_aligned + + # Estimate A, B, K from state equation: x(k+1) = A*x(k) + B*u(k) + K*e(k) + # [A B K] = X(:,2:end) * pinv([X(:,1:end-1); U(:,1:end-1); e(:,1:end-1)]) + XUe = [x[:, 1:end-1]; u_aligned[:, 1:end-1]; e[:, 1:end-1]] + X_next = x[:, 2:end] + + ABK = X_next / XUe + A = ABK[:, 1:nx] + B = ABK[:, nx+1:nx+nu] + K = ABK[:, nx+nu+1:end] + + # Force A to be stable if requested + if stable == 1 + A = stabilize_eigenvalues_method1(A) + elseif stable == 2 + A = stabilize_eigenvalues_method2(A) + end + + # Recompute Kalman gain K via discrete algebraic Riccati equation (DARE) + # For the innovation form, we solve the DARE to get the steady-state K + + # Estimate noise covariances from residuals + # Process noise covariance Q and measurement noise covariance R + # Innovation covariance S (cross-covariance) + + # Recompute residuals with potentially modified A + x_pred = A * x[:, 1:end-1] + B * u_aligned[:, 1:end-1] + process_residual = x[:, 2:end] - x_pred + + # Covariance matrices + Ns = size(e, 2) + R = (e * e') / Ns # measurement noise covariance + Q = (process_residual * process_residual') / (Ns - 1) # process noise covariance + S_cov = (process_residual * e[:, 1:end-1]') / (Ns - 1) # cross-covariance + + if any(abs.(eigvals(A-K*C)) .> 1.0) + verbose && @warn "Estimated predictor is unstable, recomputing K via DARE" + R_reg = R + 1e-10 * I + K = kalman(Discrete, A, C, Q, R_reg) + end + + return A, B, C, D, K +end + +""" + stabilize_eigenvalues_method1(A) + +Force matrix A to be stable by reflecting unstable eigenvalues inside unit circle. +Method 1: Simple eigenvalue reflection. +""" +function stabilize_eigenvalues_method1(A) + F = eigen(A) + λ = F.values + V = F.vectors + + # Reflect eigenvalues outside unit circle + λ_stable = map(λi -> abs(λi) > 1 ? λi / abs(λi)^2 : λi, λ) + + # Reconstruct A + A_stable = real(V * Diagonal(λ_stable) / V) + return A_stable +end + +""" + stabilize_eigenvalues_method2(A) + +Force matrix A to be stable using Schur decomposition. +Method 2: More numerically stable approach. +""" +function stabilize_eigenvalues_method2(A) + F = schur(A) + T = F.T + Z = F.Z + + # Modify diagonal of T (eigenvalues) to be inside unit circle + n = size(T, 1) + for i in 1:n + if abs(T[i, i]) > 1 + T[i, i] = T[i, i] / abs(T[i, i])^2 + end + end + + # Reconstruct A + A_stable = real(Z * T * Z') + return A_stable +end diff --git a/test/runtests.jl b/test/runtests.jl index 1512ed8b..a4035a77 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -139,6 +139,11 @@ end include("test_impulseest.jl") end + @testset "pbsid" begin + @info "Testing pbsid" + include("test_pbsid.jl") + end + @testset "input_signals" begin @info "Testing input_signals" include("test_input_signals.jl") diff --git a/test/test_pbsid.jl b/test/test_pbsid.jl new file mode 100644 index 00000000..76a0ead9 --- /dev/null +++ b/test/test_pbsid.jl @@ -0,0 +1,116 @@ +using ControlSystemIdentification, ControlSystemsBase +using Test, Random, LinearAlgebra, Statistics +using ControlSystemIdentification: pbsid + +@testset "pbsid" begin + @info "Testing pbsid" + + @testset "SISO identification" begin + Random.seed!(42) + a = 0.9 + b = 1.0 + G = tf(b, [1, -a], 1) + nx = 1 + + N = 500 + u = randn(1, N) + y, _, _ = lsim(G, u, 1:N) + yn = y + 0.01 * randn(size(y)) + + d = iddata(yn, u, 1) + res = pbsid(d, nx; f=10, p=10, verbose=false) + + @test res.sys.nx == nx + + # Check poles match + true_pole = a + est_pole = res.sys.A[1] + @test est_pole ≈ true_pole atol=0.05 + + # Check prediction performance + yp = predict(res, d) + vaf = 1 - sum(abs2, yn - yp) / sum(abs2, yn .- mean(yn)) + @test vaf > 0.99 + end + + @testset "MIMO identification" begin + Random.seed!(123) + + nx = 2 + ny = 2 + nu = 2 + G = ssrand(ny, nu, nx, Ts=1, proper=true) + + # Generate data with white noise input + N = 1000 + u = randn(nu, N) + y, _, _ = lsim(G, u, 1:N) + yn = y + 0.01 * randn(size(y)) + + d = iddata(yn, u, 1) + + # Identify with pbsid + res = pbsid(d, nx; f=10, p=10, verbose=false) + + @test res.sys.nx == nx + + # Check that the identified system is stable + @test all(abs.(eigvals(res.sys.A)) .<= 1.0 + 1e-6) + + # Check prediction performance + yp = predict(res, d) + total_vaf = 1 - sum(abs2, yn - yp) / sum(abs2, yn .- mean(yn, dims=2)) + @test total_vaf > 0.99 + end + + @testset "weight=0 vs weight=1" begin + Random.seed!(456) + + nx = 2 + G = ssrand(1, 1, nx, Ts=1, proper=true) + G = G / norm(G, Inf) # Normalize + + N = 500 + u = randn(1, N) + y, _, _ = lsim(G, u, 1:N) + yn = y + 0.01 * randn(size(y)) + + d = iddata(yn, u, 1) + + res0 = pbsid(d, nx; f=10, p=10, weight=0, verbose=false) + res1 = pbsid(d, nx; f=10, p=10, weight=1, verbose=false) + + # Both should give good prediction + yp0 = predict(res0, d) + yp1 = predict(res1, d) + + vaf0 = 1 - sum(abs2, yn - yp0) / sum(abs2, yn .- mean(yn)) + vaf1 = 1 - sum(abs2, yn - yp1) / sum(abs2, yn .- mean(yn)) + + @test vaf0 > 0.99 + @test vaf1 > 0.99 + end + + @testset "automatic order selection" begin + Random.seed!(789) + + nx = 3 + G = ssrand(1, 1, nx, Ts=1, proper=true) + G = G / norm(G, Inf) + + N = 500 + u = randn(1, N) + y, _, _ = lsim(G, u, 1:N) + yn = y + 0.001 * randn(size(y)) # Low noise + + d = iddata(yn, u, 1) + + res = pbsid(d, :auto; f=15, p=15, verbose=false) + + # Should select a positive order + @test res.sys.nx >= 1 + # Check that the model is stable + @test all(abs.(eigvals(res.sys.A)) .<= 1.0 + 1e-6) + end + +end