diff --git a/docs/make.jl b/docs/make.jl index b980e422..a2de65bc 100644 --- a/docs/make.jl +++ b/docs/make.jl @@ -48,6 +48,7 @@ makedocs( "Hair dryer" => "examples/hair_dryer.md", "VARX model" => "examples/varx.md", "Nonlinear belt drive" => "examples/hammerstein_wiener.md", + "LPV identification" => "examples/lpv.md", "Fit parameters of ModelingToolkit model" => "examples/modelingtoolkit.md", ], "API" => "api.md", diff --git a/docs/src/examples/lpv.md b/docs/src/examples/lpv.md new file mode 100644 index 00000000..b8bfa420 --- /dev/null +++ b/docs/src/examples/lpv.md @@ -0,0 +1,241 @@ +# Linear Parameter-Varying (LPV) identification + +!!! warning "Experimental" + LPV identification in this package is considered experimental and may + change in the future without respecting semantic versioning. + +This example shows how to identify a Linear Parameter-Varying (LPV) +state-space model using [`lpv_pem`](@ref). The model has matrices that depend +on a *known, measured* scalar scheduling variable ``\lambda(t)``: + +```math +\begin{aligned} +A(\lambda) &= \sum_{k} \theta^A_k\, \varphi_k(\lambda),\\ +B(\lambda) &= \sum_{k} \theta^B_k\, \varphi_k(\lambda),\\ +C(\lambda) &= \sum_{k} \theta^C_k\, \varphi_k(\lambda),\\ +D(\lambda) &= \sum_{k} \theta^D_k\, \varphi_k(\lambda), +\end{aligned} +``` + +where ``\{\varphi_k\}`` is a user-supplied basis in ``\lambda``. ``\lambda`` may +or may not coincide with a state variable of the system; the only requirement is that it +is measured at every time step. There is a single shared state-space realization +whose entries vary smoothly with ``\lambda``. + +## Generating data + +We simulate a known affine-in-``\lambda`` system, +``A(\lambda) = A_0 + \lambda A_1``, ``B(\lambda) = B_0 + \lambda B_1``, +driven by white-noise input. The scheduling variable is a slow sinusoid. + +```@example lpv +using ControlSystemIdentification, ControlSystemsBase, Plots, Random, LinearAlgebra, Statistics +Random.seed!(0) + +Ts = 0.05 +T = 2000 + +A0 = [0.85 0.10; -0.05 0.80] +A1 = [0.05 0.00; 0.10 -0.05] +B0 = [0.10; 0.20;;] +B1 = [0.00; 0.05;;] +C = [1.0 0.0] + +A(λ) = A0 .+ λ .* A1 +B(λ) = B0 .+ λ .* B1 + +λ = 0.5 .* sin.(0.01 .* (1:T)) +u = randn(1, T) + +y = let + x = zeros(2) + y = zeros(1, T) + for t in 1:T + y[:, t] = C * x + x = A(λ[t]) * x + B(λ[t]) * u[:, t] + end + y +end +y .+= 0.01 .* randn(size(y)) +d = iddata(y, u, Ts) + +plot( + plot(timevec(d), λ; xlabel="t [s]", ylabel="λ(t)", lab="scheduling"), + plot(d); + layout = (2, 1), size=(700, 500), +) +``` + +## Fitting an LPV model + +We choose an affine basis in ``\lambda``, ``\varphi_1(\lambda) = 1``, +``\varphi_2(\lambda) = \lambda``, and estimate the parameters with [`lpv_pem`](@ref). +`lpv_pem` warm-starts from a sliding-window subspace fit (see +[`lpv_warmstart`](@ref)) when no initial guess is given. + +```@example lpv +basis = [λ -> 1.0, λ -> λ] +res = lpv_pem(d, λ, 2; basis, show_trace = false, iterations = 200) +sys_lpv, x0h, _ = res +``` + +The returned `sys_lpv` is an [`LPVStateSpace`](@ref). It is callable: passing a +scheduling value freezes the matrices and returns a plain `StateSpace`. + +```@example lpv +sys_lpv(0.0) +``` + +## Validation + +We compare simulation performance against ground truth on the full trajectory and +against a single LTI fit by [`subspaceid`](@ref). As with `predplot` for the +LPV case, passing the schedule `λ` as the third positional argument to +[`simplot`](@ref) is enough for it to route through the LPV one-step +predictor. Each trace is annotated with its NRMSE fit percentage. + +```@example lpv +sys_lti = subspaceid(d, 2) + +simplot(sys_lpv, d, λ; sysname = "LPV") +simplot!(sys_lti, d; sysname = "LTI", ploty = false) +``` + +On varying-``\lambda`` data the LPV model is strictly better than any single +LTI fit, because the LTI model cannot match the changing dynamics. + +## A practical example: semi-active suspension + +A common real-world use of LPV identification is **semi-active vibration +isolation**: a mass-spring-damper where the damping coefficient is set by a +controllable voltage. Magneto-rheological shock absorbers on a vehicle +suspension and active engine mounts are typical instances. The damper voltage +is sampled at every time step, so it is a known scheduling variable that +enters the system matrix directly. + +The continuous-time 1-DOF model is + +```math +m\ddot{x} + c(\lambda)\dot{x} + k x = u,\qquad +c(\lambda) = c_0 + c_1\lambda,\qquad y = x, +``` + +with state ``[x,\ \dot{x}]^\top``. In state-space form the only λ-dependence +sits in the ``(2,2)`` entry of ``A``, which makes this a textbook +affine-in-λ LPV system. With the parameters used below the damping ratio sweeps +from ``\zeta \approx 0.05`` (sharp resonance) to ``\zeta \approx 0.3`` +(well-damped) as ``\lambda`` is varied, easily distinguishable in the data. + +### Simulating a damper-sweep experiment + +We simulate a 30 s experiment where the operator slowly sweeps the damper +voltage while exciting the mass with broadband noise. The damper voltage is +*measured*, and is used as the scheduling variable for identification. + +```@example lpv2 +using ControlSystemIdentification, ControlSystemsBase, Plots, Random, LinearAlgebra, Statistics +Random.seed!(0) + +# Physical parameters +m, k = 1.0, 100.0 # mass [kg], stiffness [N/m] +c0, c1 = 1.0, 5.0 # baseline + per-volt damping + +Ts = 0.01 +T_total = 30.0 +N = Int(T_total / Ts) +t = range(0, step = Ts, length = N) + +# Damper voltage trajectory (the scheduling variable) +λ = 0.5 .+ 0.4 .* sin.(2π .* 0.05 .* t) # roughly [0.1, 0.9] + +# Continuous-time model as a function of λ +Ac(λ) = [0.0 1.0; -k/m -(c0 + c1*λ)/m] +Bc = [0.0; 1/m;;] +Cc = [1.0 0.0] +Dc = zeros(1, 1) + +u = reshape(randn(N), 1, N) # broadband excitation force + +# Simulate by re-discretizing the LTV model at every sample (ZOH on u) +y_clean = let + x = zeros(2) + y_clean = zeros(1, N) + for n in 1:N + y_clean[:, n] = Cc * x + sys_n = c2d(ss(Ac(λ[n]), Bc, Cc, Dc), Ts) + x = sys_n.A * x + sys_n.B * u[:, n] + end + y_clean +end +y = y_clean .+ 1e-3 .* randn(size(y_clean)) +d = iddata(y, u, Ts) + +plot( + plot(t, λ; ylabel = "λ(t) [V]", lab = "damper voltage"), + plot(t, vec(y); ylabel = "y(t) [m]", lab = "measured position"); + layout = (2, 1), xlabel = "t [s]", size = (700, 480), +) +``` + +### Identifying the LPV model + +With an affine basis ``\{1,\lambda\}`` we recover the operating-point +dependence directly. The warm-start (subspace ID + modal-form +coefficient regression) is what makes this work despite never having a +constant-``\lambda`` segment of data: + +```@example lpv2 +basis_susp = [λ -> 1.0, λ -> λ] +res2 = lpv_pem(d, λ, 2; basis = basis_susp, + K0 = 1e-6 .* ones(2, 1), + show_trace = false, iterations = 300) +sys_susp, x0_susp, _ = res2 +``` + +### Validation: LPV vs single LTI fit + +A single LTI fit has to compromise between the under- and over-damped regimes +the experiment sweeps through, while the LPV model does not. We use +[`simplot`](@ref) to overlay simulation against measurement and annotate each +trace with its NRMSE fit percentage. Passing the schedule `λ` as the third +positional argument to `simplot` is enough for the recipe to route it through +the LPV simulator. + +```@example lpv2 +sys_lti2 = subspaceid(d, 2) + +simplot(sys_susp, d, λ; sysname = "LPV") +simplot!(sys_lti2, d; sysname = "LTI", ploty=false) +``` + +### Frequency response at frozen operating points + +Because [`LPVStateSpace`](@ref) is callable, freezing the identified model at a +particular ``\lambda`` returns an ordinary `StateSpace`. This is exactly the +artifact a controller designer would feed into a gain-scheduled controller +synthesis on top of the identified plant. + +```@example lpv2 +truth(λ) = c2d(ss(Ac(λ), Bc, Cc, Dc), Ts) +w = exp10.(range(-0.5, log10(π / Ts); length = 300)) + +p = bodeplot(truth(0.1), w; plotphase = false, lab = "truth λ=0.1") +bodeplot!(p, sys_susp(0.1), w; plotphase = false, lab = "LPV λ=0.1", ls = :dash) +bodeplot!(p, truth(0.9), w; plotphase = false, lab = "truth λ=0.9") +bodeplot!(p, sys_susp(0.9), w; plotphase = false, lab = "LPV λ=0.9", ls = :dash) +p +``` + +The resonance peak collapses by more than 15 dB between ``\lambda=0.1`` and +``\lambda=0.9``, and the LPV fit tracks it at both endpoints. + +## Notes + +- The basis can be any user-supplied set of functions of ``\lambda``: polynomial, + radial-basis-function, piecewise-linear, etc. For mild dependence (as in the + suspension example) an affine basis suffices; stronger nonlinearity in + ``\lambda`` is well-served by a polynomial or RBF basis. +- Multi-dimensional scheduling is not yet documented; the basis API technically + accepts vector ``\lambda`` if the user writes a multivariate basis function, + but this path has not been thoroughly tested. +- The Kalman gain ``K`` is currently held constant in ``\lambda``. diff --git a/src/ControlSystemIdentification.jl b/src/ControlSystemIdentification.jl index a13b0a9b..12468dd6 100644 --- a/src/ControlSystemIdentification.jl +++ b/src/ControlSystemIdentification.jl @@ -47,6 +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 LPVStateSpace, lpv_pem, lpv_warmstart export n4sid, subspaceid, era, okid, find_similarity_transform, schur_stab export getARXregressor, getARregressor, @@ -87,6 +88,7 @@ include("subspace2.jl") include("spectrogram.jl") include("frequency_weights.jl") include("basis_functions.jl") +include("lpv.jl") include("plotting.jl") include("input_signals.jl") diff --git a/src/lpv.jl b/src/lpv.jl new file mode 100644 index 00000000..482e860d --- /dev/null +++ b/src/lpv.jl @@ -0,0 +1,570 @@ +## Linear Parameter-Varying (LPV) state-space identification. +# +# The user supplies a scheduling-variable trajectory λ(t) and a basis {φ_k} in λ. +# The estimated model has matrices +# +# A(λ) = Σ_k θ^A_k φ_k(λ), similarly for B(λ), C(λ), D(λ), +# +# i.e. a single shared state-space realization whose entries depend smoothly on λ. +# Estimation uses the same prediction-error scaffolding (Optim + ForwardDiff) as +# `structured_pem`, but with a hand-rolled LTV loop because the predictor is time-varying. + +# ---------------------------------------------------------------------------- +# Basis helpers +# ---------------------------------------------------------------------------- + +_normalize_basis(basis::Function, λ_probe) = + (basis, length(basis(λ_probe))) + +function _normalize_basis(basis::AbstractVector, λ_probe) + isempty(basis) && throw(ArgumentError("basis must be non-empty")) + fns = Tuple(basis) + fn = let fns = fns + λ -> [f(λ) for f in fns] + end + (fn, length(basis)) +end + +# Hand-rolled because the inner BFGS loop is differentiated through with ForwardDiff, +# and tensor-array operations (tullio/einsum/NNlib) drag in heavier dependencies. +function _contract!(out, M::AbstractArray{<:Any,3}, φ) + out .= 0 + @inbounds @views for k in eachindex(φ) + out .= out .+ M[:, :, k] .* φ[k] + end + out +end + +function _contract(M::AbstractArray{T,3}, φ) where T + out = zeros(T, size(M, 1), size(M, 2)) + _contract!(out, M, φ) +end + +# ---------------------------------------------------------------------------- +# LPVStateSpace +# ---------------------------------------------------------------------------- + +""" + LPVStateSpace{Tθ,Tb,TK,TT} + +State-space model with matrices `A,B,C,D` parametrized as a basis expansion in a +scalar scheduling variable `λ`: + + A(λ) = Σ_k θ.A[:,:,k] * φ_k(λ) + B(λ) = Σ_k θ.B[:,:,k] * φ_k(λ) + C(λ) = Σ_k θ.C[:,:,k] * φ_k(λ) + D(λ) = Σ_k θ.D[:,:,k] * φ_k(λ) + +Fields: +- `basis::Tb`: callable `λ -> Vector` returning the `nb` basis values at `λ` +- `nb::Int`: number of basis functions +- `θ::Tθ`: `ComponentArray` with 3-D fields `A,B,C,D` (last axis is the basis dimension) +- `K::TK`: constant Kalman gain (the λ-varying case is left for future work) +- `Ts`: sample time +- `nx,nu,ny`: state, input, and output dimensions + +Call `sys(λ)` to obtain a frozen `StateSpace` at a particular operating point. +See also [`lpv_pem`](@ref), [`lpv_warmstart`](@ref). + +!!! warning "Experimental" + LPV identification in this package is considered experimental and may + change in the future without respecting semantic versioning. In particular, + the field layout of [`LPVStateSpace`](@ref), the basis API, and the + handling of the Kalman gain are subject to revision. +""" +struct LPVStateSpace{Tθ,Tb,TK,TT} + basis::Tb + nb::Int + θ::Tθ + K::TK + Ts::TT + nx::Int + nu::Int + ny::Int +end + +function Base.show(io::IO, s::LPVStateSpace) + print(io, "LPVStateSpace with nx=$(s.nx), nu=$(s.nu), ny=$(s.ny), nb=$(s.nb), Ts=$(s.Ts)") +end + +function (s::LPVStateSpace)(λ) + φ = s.basis(λ) + A = _contract(s.θ.A, φ) + B = _contract(s.θ.B, φ) + C = _contract(s.θ.C, φ) + D = _contract(s.θ.D, φ) + ss(A, B, C, D, s.Ts) +end + +ControlSystemsBase.ninputs(s::LPVStateSpace) = s.nu +ControlSystemsBase.noutputs(s::LPVStateSpace) = s.ny +ControlSystemsBase.nstates(s::LPVStateSpace) = s.nx + +# ---------------------------------------------------------------------------- +# LTV simulate/predict +# ---------------------------------------------------------------------------- + +""" + simulate(sys::LPVStateSpace, u, λ; x0 = zeros(sys.nx)) + +Simulate the LPV system along the scheduling trajectory `λ`. `u` and `λ` must have +the same length along the time dimension. +""" +function simulate(sys::LPVStateSpace, u::AbstractMatrix, λ::AbstractVector; x0 = zeros(sys.nx)) + size(u, 2) == length(λ) || throw(ArgumentError("size(u, 2) must equal length(λ)")) + T = promote_type(eltype(sys.θ), eltype(u), eltype(x0)) + N = size(u, 2) + y = zeros(T, sys.ny, N) + x = T.(copy(x0)) + @inbounds for t in 1:N + φ = sys.basis(λ[t]) + At = _contract(sys.θ.A, φ) + Bt = _contract(sys.θ.B, φ) + Ct = _contract(sys.θ.C, φ) + Dt = _contract(sys.θ.D, φ) + ut = @view u[:, t] + y[:, t] = Ct * x + Dt * ut + x = At * x + Bt * ut + end + y +end + +simulate(sys::LPVStateSpace, d::AbstractIdData, λ::AbstractVector; x0 = zeros(sys.nx)) = + simulate(sys, time2(input(d)), λ; x0) + +""" + predict(sys::LPVStateSpace, d, λ; x0 = zeros(sys.nx), h = 1) + +One-step-ahead prediction of the LPV system along the scheduling trajectory `λ`, +using the Kalman gain stored in `sys.K`. The innovation update reuses the +constant gain at every `t` — λ-varying `K` is not yet supported. +""" +function predict(sys::LPVStateSpace, d::AbstractIdData, λ::AbstractVector; + x0 = zeros(sys.nx), h::Int = 1) + h == 1 || throw(ArgumentError("h > 1 not supported for LPVStateSpace yet")) + length(d) == length(λ) || throw(ArgumentError("length(d) must equal length(λ)")) + y = time2(output(d)) + u = time2(input(d)) + T = promote_type(eltype(sys.θ), eltype(y), eltype(u), eltype(x0)) + N = size(y, 2) + yh = zeros(T, sys.ny, N) + x = T.(copy(x0)) + @inbounds for t in 1:N + φ = sys.basis(λ[t]) + At = _contract(sys.θ.A, φ) + Bt = _contract(sys.θ.B, φ) + Ct = _contract(sys.θ.C, φ) + Dt = _contract(sys.θ.D, φ) + ut = @view u[:, t] + yt = @view y[:, t] + ŷ = Ct * x + Dt * ut + yh[:, t] = ŷ + e = yt - ŷ + x = At * x + Bt * ut + sys.K * e + end + yh +end + +# ---------------------------------------------------------------------------- +# Warm-start: sliding-window subspace ID + modal-form coefficient regression +# ---------------------------------------------------------------------------- + +""" + lpv_warmstart(d, λ, nx; basis, window = nothing, stride = nothing) -> ComponentArray + lpv_warmstart(ds, λs, nx; basis, window = nothing, stride = nothing) -> ComponentArray + +Produce an initial guess `θ⁰` for [`lpv_pem`](@ref) by + +1. sliding a window across the dataset and running [`subspaceid`](@ref) on each; +2. bringing every local model to a common modal form via `modal_form`; +3. regressing the entries of `A, B, C, D` against `basis(λ̄_i)` by ordinary least + squares, where `λ̄_i` is the mean of `λ` in window `i`. + +If a vector of datasets `ds` together with the corresponding vector of +scheduling trajectories `λs` is provided, the same procedure is applied to each +dataset independently and the resulting local models are pooled before the +final regression. All datasets must share the same sample time, number of +inputs, and number of outputs. + +`basis` follows the same convention as in [`lpv_pem`](@ref) (a `Function` returning +a vector, or a `Vector` of functions). `window` defaults to `max(20nx, N÷10)` and +`stride` defaults to `window ÷ 4`. + +The modal-form alignment is a heuristic — it works well when the dominant poles +move smoothly with `λ` and do not change order or coalesce, but can mis-align +entries when poles cross. In those cases pass a hand-crafted `p0` to `lpv_pem` +instead. + +Returns a `ComponentArray` with fields `A, B, C, D`, each a 3-D array whose +trailing dimension is the basis index. + +!!! warning "Experimental" + LPV identification in this package is considered experimental and may + change in the future without respecting semantic versioning. +""" +lpv_warmstart(d::AbstractIdData, λ::AbstractVector, nx::Int; kwargs...) = + lpv_warmstart([d], [λ], nx; kwargs...) + +function lpv_warmstart(ds::AbstractVector{<:AbstractIdData}, + λs::AbstractVector, + nx::Int; + basis, + window::Union{Nothing,Int} = nothing, + stride::Union{Nothing,Int} = nothing) + length(ds) == length(λs) || throw(ArgumentError("Number of datasets must equal number of λ trajectories")) + isempty(ds) && throw(ArgumentError("Must provide at least one dataset")) + allequal(d.Ts for d in ds) || throw(ArgumentError("All datasets must share the same sample time")) + ny, nu = ds[1].ny, ds[1].nu + all(d.ny == ny for d in ds) || throw(ArgumentError("All datasets must have the same number of outputs")) + all(d.nu == nu for d in ds) || throw(ArgumentError("All datasets must have the same number of inputs")) + basis_fn, nb = _normalize_basis(basis, first(λs[1])) + + locals = StateSpace[] + λ_means = Float64[] + for (d, λ) in zip(ds, λs) + length(λ) == length(d) || throw(ArgumentError("each λ must have the same length as its dataset")) + N = length(d) + w = window === nothing ? max(20nx, N ÷ 10) : window + w = min(w, N) + st = stride === nothing ? max(1, w ÷ 4) : stride + starts = 1:st:(N - w + 1) + isempty(starts) && (starts = 1:1) + for s in starts + d_i = d[s:(s + w - 1)] + λ_i = @view λ[s:(s + w - 1)] + try + sys_i = subspaceid(d_i, nx; verbose = false) + sysm, _, _ = modal_form(sys_i.sys) + push!(locals, sysm) + push!(λ_means, mean(λ_i)) + catch err + @warn "Local fit failed for window starting at $s; skipping" exception = (err, catch_backtrace()) + end + end + end + + M = length(locals) + M >= 1 || error("lpv_warmstart: no window produced a successful local fit") + if M < nb + @warn "lpv_warmstart: fewer valid windows ($M) than basis functions ($nb); the regression is underdetermined" + end + + Φ = zeros(M, nb) + for i in 1:M + Φ[i, :] = basis_fn(λ_means[i]) + end + + ComponentArray( + A = _ols_field(Φ, locals, s -> s.A), + B = _ols_field(Φ, locals, s -> s.B), + C = _ols_field(Φ, locals, s -> s.C), + D = _ols_field(Φ, locals, s -> s.D), + ) +end + +function _ols_field(Φ, locals, get) + M = length(locals) + nb = size(Φ, 2) + nr, nc = size(get(locals[1])) + arr = zeros(nr, nc, nb) + for r in 1:nr, c in 1:nc + b = [get(locals[i])[r, c] for i in 1:M] + arr[r, c, :] = Φ \ b + end + arr +end + +# ---------------------------------------------------------------------------- +# lpv_pem +# ---------------------------------------------------------------------------- + +function _lpv_dataset_predloss(p, x0_col, basis_fn, λ, y, u, metric, + ::Val{zeroD}, ::Val{predflag}) where {zeroD, predflag} + N = size(y, 2) + x = copy(x0_col) + L = zero(promote_type(eltype(p), eltype(λ))) + nx = length(x) + nu = size(u, 1) + ny = size(y, 1) + At = zeros(eltype(p), nx, nx) + Bt = zeros(eltype(p), nx, nu) + Ct = zeros(eltype(p), ny, nx) + Dt = zeros(eltype(p), ny, nu) + ŷ = zeros(eltype(p), ny) + e = zeros(eltype(p), ny) + xtmp = zeros(eltype(p), nx) + @inbounds for t in 1:N + φ = basis_fn(λ[t]) + _contract!(At, p.A, φ) + _contract!(Bt, p.B, φ) + _contract!(Ct, p.C, φ) + ut = @view u[:, t] + yt = @view y[:, t] + # ŷ ← Ct*x [+ Dt*ut] + mul!(ŷ, Ct, x) + if !zeroD + _contract!(Dt, p.D, φ) + mul!(ŷ, Dt, ut, 1, 1) + end + e .= yt .- ŷ + L += sum(metric, e) + # x ← At*x + Bt*ut [+ K*e]; bounce through xtmp to avoid mul! output/input aliasing on x. + mul!(xtmp, At, x) + x, xtmp = xtmp, x + mul!(x, Bt, ut, 1, 1) + if predflag + mul!(x, p.K, e, 1, 1) + end + end + L +end + +# Single-dataset loss: x0 is a Vector (column) in the ComponentArray. +function _lpv_loss_factory(zeroD_v::Val, predflag_v::Val, basis_fn, λ, y, u, metric, regularizer) + let basis_fn = basis_fn, λ = λ, y = y, u = u, metric = metric, regularizer = regularizer + function loss(p) + L = _lpv_dataset_predloss(p, p.x0, basis_fn, λ, y, u, metric, zeroD_v, predflag_v) + L + regularizer(p) + end + return loss + end +end + +# Multi-dataset loss: x0 is a Matrix of shape (nx, M); column i is the initial state for dataset i. +function _lpv_multi_loss_factory(zeroD_v::Val, predflag_v::Val, basis_fn, λs, ys, us, metric, regularizer) + let basis_fn = basis_fn, λs = λs, ys = ys, us = us, metric = metric, regularizer = regularizer + function loss(p) + L = zero(eltype(p)) + @inbounds for i in eachindex(λs) + L += _lpv_dataset_predloss(p, view(p.x0, :, i), basis_fn, + λs[i], ys[i], us[i], metric, + zeroD_v, predflag_v) + end + L + regularizer(p) + end + return loss + end +end + +""" + sys, x0, res = lpv_pem( d, λ, nx; basis, ...) + sys, x0, res = lpv_pem(ds, λs, nx; basis, ...) + +Linear Parameter-Varying (LPV) state-space identification using PEM. + +The model has matrices that vary as a basis expansion in a measured scalar +scheduling variable `λ(t)`: + + A(λ) = Σ_k θ^A_k φ_k(λ), ..., D(λ) = Σ_k θ^D_k φ_k(λ). + +A constant Kalman gain `K` is also estimated (when `focus = :prediction`). +Estimation minimizes one-step prediction error over the full dataset; the +predictor is time-varying along `λ(t)`. + +If a vector of datasets `ds` and a corresponding vector of scheduling +trajectories `λs` is passed, the same parameters `θ` and `K` are fit to all +datasets jointly. Each dataset gets its own initial state, returned as a +matrix `x0::Matrix{Float64}` of shape `(nx, length(ds))`. All datasets must +share sample time, number of inputs, and number of outputs. + +# Arguments +- `d`, `ds`: [`iddata`](@ref) (or a vector thereof). +- `λ::AbstractVector`, `λs`: scheduling trajectory (or a vector of them); + each must satisfy `length(λ) == length(d)`. +- `nx`: model order. + +# Keyword arguments +- `basis`: either a `Vector` of functions `λ -> ::Real` or a single `Function` + `λ -> ::AbstractVector`. The number of basis functions is inferred. +- `focus`: `:prediction` (default) or `:simulation`. In `:simulation` mode `K` is + forced to zero and the parameter `K` is dropped. +- `zeroD`: if `true`, the `D(λ)` term is omitted entirely. +- `p0`: optional initial guess as a `ComponentArray` with fields `A,B,C[,D]` of + shape `(nx,nx,nb),(nx,nu,nb),(ny,nx,nb),(ny,nu,nb)`. If `nothing`, a warm + start is computed via [`lpv_warmstart`](@ref). +- `K0`: optional initial Kalman gain `(nx,ny)`. +- `x0`: optional initial state. For the multi-dataset method, this is a + matrix of shape `(nx, length(ds))` with one column per experiment. +- The remaining keyword arguments are forwarded to `Optim.Options` and follow + the same conventions as [`structured_pem`](@ref) and [`newpem`](@ref). + +# Returns +A named tuple `(; sys, x0, res)` where `sys::LPVStateSpace`, `x0` is the +optimized initial state (a `Vector` for the single-dataset method, a `Matrix` +for the multi-dataset method), and `res` is the `Optim` result. + +# Example +```julia +A0 = [0.9 0.1; 0 0.8]; A1 = [0.0 0.0; 0.05 0.0] +B0 = [0.1; 0.2;;]; B1 = [0.0; 0.05;;] +C = [1.0 0.0]; Ts = 0.05 +T = 2000 +λ = sin.(0.01 .* (1:T)) +u = randn(1, T) +A(λ) = A0 .+ λ .* A1 +B(λ) = B0 .+ λ .* B1 + +x = zeros(2); y = zeros(1, T) +for t in 1:T + y[:, t] = C * x + x = A(λ[t]) * x + B(λ[t]) * u[:, t] +end +y .+= 0.01 .* randn(size(y)) +d = iddata(y, u, Ts) + +basis = [_ -> 1.0, λ -> λ] +sys, x0h, res = lpv_pem(d, λ, 2; basis) +``` + +See also [`lpv_warmstart`](@ref), [`structured_pem`](@ref), [`newpem`](@ref). + +!!! warning "Experimental" + LPV identification in this package is considered experimental and may + change in the future without respecting semantic versioning. The basis + API, the handling of the Kalman gain `K`, and the return type + [`LPVStateSpace`](@ref) in particular are subject to revision; the + `h > 1` prediction horizon, multi-dimensional scheduling variables, and + λ-varying `K` are also not yet supported. +""" +function lpv_pem(d::AbstractIdData, λ::AbstractVector, nx::Int; + x0 = nothing, kwargs...) + x0_mat = x0 === nothing ? nothing : reshape(collect(x0), :, 1) + out = lpv_pem([d], [λ], nx; x0 = x0_mat, kwargs...) + (; sys = out.sys, x0 = vec(out.x0), res = out.res) +end + +function lpv_pem(ds::AbstractVector{<:AbstractIdData}, + λs::AbstractVector, + nx::Int; + basis, + focus::Symbol = :prediction, + zeroD::Bool = false, + p0 = nothing, + K0 = nothing, + x0 = nothing, + h::Int = 1, + metric::F = abs2, + regularizer::RE = p -> 0, + optimizer = BFGS(linesearch = LineSearches.BackTracking()), + store_trace = true, + show_trace = true, + show_every = 50, + iterations = 10000, + allow_f_increases = false, + time_limit = 100, + x_tol = 0, + x_abstol = x_tol, + f_abstol = 1e-16, + g_tol = 1e-12, + f_calls_limit = 0, + g_calls_limit = 0, + ) where {F,RE} + h == 1 || throw(ArgumentError("h > 1 not supported for lpv_pem yet")) + focus ∈ (:prediction, :simulation) || throw(ArgumentError("focus must be :prediction or :simulation")) + length(ds) == length(λs) || throw(ArgumentError("Number of datasets must equal number of λ trajectories")) + isempty(ds) && throw(ArgumentError("Must provide at least one dataset")) + allequal(d.Ts for d in ds) || throw(ArgumentError("All datasets must share the same sample time")) + ny, nu = ds[1].ny, ds[1].nu + all(d.ny == ny for d in ds) || throw(ArgumentError("All datasets must have the same number of outputs")) + all(d.nu == nu for d in ds) || throw(ArgumentError("All datasets must have the same number of inputs")) + for i in 1:length(ds) + length(λs[i]) == length(ds[i]) || throw(ArgumentError("length(λs[$i]) must equal length(ds[$i])")) + end + + M = length(ds) + Ts = ds[1].Ts + basis_fn, nb = _normalize_basis(basis, first(λs[1])) + + if p0 === nothing + show_trace && @info "lpv_pem: computing warm-start with sliding-window subspaceid" + p0 = lpv_warmstart(ds, λs, nx; basis = basis_fn) + end + + K0_ = K0 === nothing ? + (focus === :prediction ? 1e-6 .* randn(nx, ny) : zeros(nx, ny)) : + copy(K0) + size(K0_) == (nx, ny) || throw(DimensionMismatch("K0 must have size ($nx, $ny)")) + + x0_mat = if x0 === nothing + m = zeros(nx, M) + for (i, (d, λ)) in enumerate(zip(ds, λs)) + sys_mean = let + φ = basis_fn(mean(λ)) + A_mean = _contract(p0.A, φ) + B_mean = _contract(p0.B, φ) + C_mean = _contract(p0.C, φ) + D_mean = zeroD ? zeros(ny, nu) : _contract(p0.D, φ) + ss(A_mean, B_mean, C_mean, D_mean, Ts) + end + m[:, i] = try + estimate_x0(sys_mean, d, min(length(d), 10nx)) + catch + zeros(nx) + end + end + m + else + size(x0) == (nx, M) || throw(DimensionMismatch("x0 must have size ($nx, $M)")) + collect(x0) + end + + if zeroD + p_init = ComponentArray( + A = collect(p0.A), + B = collect(p0.B), + C = collect(p0.C), + K = K0_, + x0 = x0_mat, + ) + else + D0 = hasproperty(p0, :D) ? collect(p0.D) : zeros(ny, nu, nb) + p_init = ComponentArray( + A = collect(p0.A), + B = collect(p0.B), + C = collect(p0.C), + D = D0, + K = K0_, + x0 = x0_mat, + ) + end + + ys = [time2(output(d)) for d in ds] + us = [time2(input(d)) for d in ds] + + loss = _lpv_multi_loss_factory( + Val(zeroD), + Val(focus === :prediction), + basis_fn, λs, ys, us, metric, regularizer, + ) + + res = Optim.optimize( + loss, + p_init, + optimizer, + Optim.Options(; + store_trace, show_trace, show_every, iterations, allow_f_increases, + time_limit, x_abstol, f_abstol, g_tol, f_calls_limit, g_calls_limit); + autodiff = AutoForwardDiff(), + ) + p_opt = res.minimizer + + θ_opt = if zeroD + ComponentArray( + A = collect(p_opt.A), + B = collect(p_opt.B), + C = collect(p_opt.C), + D = zeros(ny, nu, nb), + ) + else + ComponentArray( + A = collect(p_opt.A), + B = collect(p_opt.B), + C = collect(p_opt.C), + D = collect(p_opt.D), + ) + end + + K_opt = focus === :prediction ? collect(p_opt.K) : zeros(nx, ny) + sys = LPVStateSpace(basis_fn, nb, θ_opt, K_opt, Ts, nx, nu, ny) + (; sys, x0 = collect(p_opt.x0), res) +end diff --git a/test/runtests.jl b/test/runtests.jl index 1512ed8b..81d4bb0c 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -94,6 +94,11 @@ end include("test_pem.jl") end + @testset "lpv" begin + @info "Testing lpv" + include("test_lpv.jl") + end + @testset "nonlinear_pem" begin @info "Testing nonlinear_pem" include("test_nonlinear_pem.jl") diff --git a/test/test_lpv.jl b/test/test_lpv.jl new file mode 100644 index 00000000..08a7d62d --- /dev/null +++ b/test/test_lpv.jl @@ -0,0 +1,172 @@ +using ControlSystemIdentification, ControlSystemsBase +using Test, Random, LinearAlgebra, Statistics +using ComponentArrays + +# Simulate from a known affine-in-λ LPV system. Returns (d, λ, sys_true_factory). +function simulate_affine_lpv(; T = 2000, Ts = 0.05, σy = 0.01, seed = 1) + Random.seed!(seed) + A0 = [0.85 0.10; -0.05 0.80] + A1 = [0.05 0.00; 0.10 -0.05] + B0 = reshape([0.10, 0.20], 2, 1) + B1 = reshape([0.00, 0.05], 2, 1) + C = [1.0 0.0] + A(λ) = A0 .+ λ .* A1 + B(λ) = B0 .+ λ .* B1 + λ_traj = 0.5 .* sin.(0.01 .* (1:T)) + u = randn(1, T) + x = zeros(2) + y = zeros(1, T) + for t in 1:T + y[:, t] = C * x + x = A(λ_traj[t]) * x + B(λ_traj[t]) * u[:, t] + end + y_meas = y .+ σy .* randn(size(y)) + d = iddata(y_meas, u, Ts) + return d, λ_traj, (; A0, A1, B0, B1, C, Ts) +end + +@testset "LPVStateSpace basic" begin + # Construct manually and check frozen evaluation + nx, nu, ny, nb = 2, 1, 1, 2 + A_arr = randn(nx, nx, nb) + B_arr = randn(nx, nu, nb) + C_arr = randn(ny, nx, nb) + D_arr = zeros(ny, nu, nb) + θ = ComponentArray(A = A_arr, B = B_arr, C = C_arr, D = D_arr) + K = zeros(nx, ny) + basis = λ -> [1.0, λ] + sys = LPVStateSpace(basis, nb, θ, K, 0.1, nx, nu, ny) + s0 = sys(0.0) + @test s0.A ≈ A_arr[:, :, 1] + @test s0.B ≈ B_arr[:, :, 1] + @test s0.C ≈ C_arr[:, :, 1] + s1 = sys(1.0) + @test s1.A ≈ A_arr[:, :, 1] .+ A_arr[:, :, 2] +end + +@testset "LPV simulate/predict shapes" begin + nx, nu, ny, nb = 2, 1, 1, 2 + θ = ComponentArray( + A = cat([0.9 0.1; 0.0 0.8], zeros(2, 2); dims = 3), + B = cat(reshape([0.1, 0.2], 2, 1), zeros(2, 1); dims = 3), + C = cat([1.0 0.0], zeros(1, 2); dims = 3), + D = zeros(1, 1, 2), + ) + sys = LPVStateSpace(λ -> [1.0, λ], nb, θ, zeros(nx, ny), 0.05, nx, nu, ny) + T = 100 + u = randn(1, T) + λ = 0.1 .* sin.(1:T) + y = ControlSystemIdentification.simulate(sys, u, λ) + @test size(y) == (ny, T) + d = iddata(y, u, sys.Ts) + yh = ControlSystemIdentification.predict(sys, d, λ) + @test size(yh) == (ny, T) +end + +@testset "LPV warmstart returns the right shapes" begin + d, λ, _ = simulate_affine_lpv(T = 1500) + θ0 = lpv_warmstart(d, λ, 2; basis = [_ -> 1.0, λ -> λ]) + @test size(θ0.A) == (2, 2, 2) + @test size(θ0.B) == (2, 1, 2) + @test size(θ0.C) == (1, 2, 2) + @test size(θ0.D) == (1, 1, 2) + @test all(isfinite, θ0.A) + @test all(isfinite, θ0.B) + @test all(isfinite, θ0.C) +end + +@testset "LPV PEM recovery beats LTI on varying-λ data" begin + d, λ, _ = simulate_affine_lpv(T = 2000, σy = 0.005) + basis = [_ -> 1.0, λ -> λ] + + # Single-LTI baseline + sys_lti = subspaceid(d, 2) + yh_lti = predict(sys_lti, d) + e_lti = mean(abs2, d.y .- yh_lti) + + # LPV fit. K0 is pinned for determinism across Julia/Random versions. + res = lpv_pem(d, λ, 2; basis, + K0 = 1e-6 .* ones(2, 1), + show_trace = false, store_trace = false, + iterations = 200, time_limit = 60) + sys_lpv, x0h, _ = res + + yh_lpv = ControlSystemIdentification.predict(sys_lpv, d, λ; x0 = x0h) + e_lpv = mean(abs2, d.y .- yh_lpv) + + @test e_lpv < e_lti # LPV should be strictly better on truly varying-λ data + @test e_lpv < 0.05 # Coarse absolute bound — adjust if seed/tolerances change +end + +@testset "LPV PEM with zeroD" begin + d, λ, _ = simulate_affine_lpv(T = 1500, σy = 0.005) + basis = [_ -> 1.0, λ -> λ] + res = lpv_pem(d, λ, 2; basis, zeroD = true, + K0 = 1e-6 .* ones(2, 1), + show_trace = false, store_trace = false, + iterations = 100, time_limit = 60) + sys_lpv, _, _ = res + @test all(iszero, sys_lpv.θ.D) + # Predict should still work + yh = ControlSystemIdentification.predict(sys_lpv, d, λ) + @test size(yh) == (1, length(d)) +end + +@testset "LPV PEM multi-dataset" begin + # Two experiments from the same affine-in-λ system but with different + # scheduling trajectories and inputs. The joint fit should explain both. + d1, λ1, _ = simulate_affine_lpv(T = 1200, σy = 0.005, seed = 1) + d2, λ2, _ = simulate_affine_lpv(T = 1200, σy = 0.005, seed = 7) + basis = [_ -> 1.0, λ -> λ] + + res = lpv_pem([d1, d2], [λ1, λ2], 2; basis, + K0 = 1e-6 .* ones(2, 1), + show_trace = false, store_trace = false, + iterations = 200, time_limit = 90) + sys_joint, x0_mat, _ = res + @test size(x0_mat) == (2, 2) + @test all(isfinite, x0_mat) + + e1 = mean(abs2, d1.y .- ControlSystemIdentification.predict(sys_joint, d1, λ1; x0 = x0_mat[:, 1])) + e2 = mean(abs2, d2.y .- ControlSystemIdentification.predict(sys_joint, d2, λ2; x0 = x0_mat[:, 2])) + @test e1 < 0.05 + @test e2 < 0.05 + + # Joint fit must beat a single-dataset fit on the OTHER experiment, since + # the single fit overfits its own dataset's input/noise realization. + res1 = lpv_pem(d1, λ1, 2; basis, + K0 = 1e-6 .* ones(2, 1), + show_trace = false, store_trace = false, + iterations = 200, time_limit = 60) + sys_only1, x0_only1, _ = res1 + # Held-out prediction using sys_only1 starts from zero state, which is fair + # since it never saw d2. + e2_only1 = mean(abs2, d2.y .- ControlSystemIdentification.predict(sys_only1, d2, λ2)) + @test e1 < e2_only1 +end + +@testset "LPV PEM basis-of-length-1 ≈ LTI" begin + # When the basis has a single constant function, the LPV model is just LTI; + # the result should match a plain LTI fit to within a moderate tolerance. + Random.seed!(2) + Ts = 0.05 + A = [0.85 0.10; -0.05 0.80] + B = reshape([0.10, 0.20], 2, 1) + C = [1.0 0.0] + sys_true = ss(A, B, C, 0, Ts) + T = 1500 + u = randn(1, T) + y, _, _ = lsim(sys_true, u) + y .+= 0.005 .* randn(size(y)) + d = iddata(y, u, Ts) + λ = zeros(T) + + res = lpv_pem(d, λ, 2; basis = [_ -> 1.0], + K0 = 1e-6 .* ones(2, 1), + show_trace = false, store_trace = false, + iterations = 200, time_limit = 60) + sys_lpv, x0h, _ = res + yh = ControlSystemIdentification.predict(sys_lpv, d, λ; x0 = x0h) + e = mean(abs2, d.y .- yh) + @test e < 0.01 +end