diff --git a/ext/ControlSystemIdentificationLSOptExt.jl b/ext/ControlSystemIdentificationLSOptExt.jl index ab01be3b..91456381 100644 --- a/ext/ControlSystemIdentificationLSOptExt.jl +++ b/ext/ControlSystemIdentificationLSOptExt.jl @@ -177,7 +177,13 @@ function _inner_pem( function Λ() resid = zeros(T * ny) J = ForwardDiff.jacobian(residuals!, resid, res.minimizer) - (T - length(p_guess)) * Symmetric(J' * J) + Σ = sum(abs2, reshape(resid, ny, T), dims=2)[:] ./ (T - length(p_guess)) + inds = range(1, step=ny, length=T) + for i = 1:ny + J[inds, :] ./= sqrt(Σ[i]) + inds = inds .+ 1 + end + Symmetric(J' * J) end ukf = get_ukf(res.minimizer) diff --git a/src/pem.jl b/src/pem.jl index 781c867d..c586e242 100644 --- a/src/pem.jl +++ b/src/pem.jl @@ -642,10 +642,10 @@ using ChainRules using ForwardDiffChainRules @ForwardDiff_frule LinearAlgebra.exp!(x1::AbstractMatrix{<:ForwardDiff.Dual}) true -function inner_constructor(p, constructor, Ts) +function inner_constructor(p, constructor, Ts, method) sys = constructor(p) if iscontinuous(sys) - return c2d(sys, Ts, :zoh), p.K, p.x0 + return c2d(sys, Ts, method), p.K, p.x0 end return sys, p.K, p.x0 end @@ -710,6 +710,7 @@ function structured_pem( # alphaguess = LineSearches.InitialStatic(alpha = 0.95), linesearch = LineSearches.BackTracking(), ), + method = :zoh, store_trace = true, show_trace = true, show_every = 50, @@ -742,7 +743,7 @@ function structured_pem( end p0ca = ComponentArray(p = p0, K = T.(K0), x0 = x0i) function predloss(p) - sysi, Ki, x0 = inner_constructor(p, constructor, d.Ts) + sysi, Ki, x0 = inner_constructor(p, constructor, d.Ts, method) syso = PredictionStateSpace(sysi, Ki, 0, 0) Pyh = ControlSystemsBase.observer_predictor(syso; h) simres = lsim(Pyh, pd; x0=Vector(x0)) @@ -752,7 +753,7 @@ function structured_pem( c1 + inner_regularizer(regularizer, p, syso, simres) end function simloss(p) - syssim, _, x0 = inner_constructor(p, constructor, d.Ts) + syssim, _, x0 = inner_constructor(p, constructor, d.Ts, method) simres = lsim(syssim, d; x0) y = simres.y y .= metric.(y .- d.y) @@ -767,7 +768,7 @@ function structured_pem( time_limit, x_abstol, f_abstol, g_tol, f_calls_limit, g_calls_limit), autodiff = :forward, ) - sys_opt, K_opt, x0_opt = inner_constructor(res.minimizer, constructor, d.Ts) + sys_opt, K_opt, x0_opt = inner_constructor(res.minimizer, constructor, d.Ts, method) sysp_opt = PredictionStateSpace( sys_opt, pred ? K_opt : zeros(T, nx, ny), @@ -776,5 +777,13 @@ function structured_pem( zeros(T, nx, ny), ) pred && !isstable(observer_predictor(sysp_opt)) && @warn("Estimated predictor dynamics A-KC is unstable") - (; sys=sysp_opt, x0=x0_opt, res) + function Λ() + cost = pred ? predloss : simloss + H = Symmetric(ForwardDiff.hessian(cost, res.minimizer)) + # NOTE: this scales all gradients by the same σ unlike in nonlinear_pem where we compute a unique σ for each output + iσ2 = (length(d) - length(res.minimizer))/cost(res.minimizer) + iσ2 * H + end + ax = ComponentArrays.getaxes(res.minimizer) + (; sys=sysp_opt, x0=x0_opt, res, Λ, constructor=p->inner_constructor(ComponentArray(p, ax), constructor, d.Ts, method)) end diff --git a/test/test_nonlinear_pem.jl b/test/test_nonlinear_pem.jl index 6e889228..73fc7c32 100644 --- a/test/test_nonlinear_pem.jl +++ b/test/test_nonlinear_pem.jl @@ -81,13 +81,17 @@ eopt = norm(x0 - model.x0) / norm(x0) @test eopt < e0 +y_err = 2sqrt.(diag(Σ[1:4, 1:4])) if isinteractive() - scatter(p_opt, yerror=2sqrt.(diag(Σ[1:4, 1:4])), lab="Estimate") + scatter(p_opt, yerror=y_err, lab="Estimate") scatter!(p_true, lab="True") scatter!(p0, lab="Initial guess") end +z_score = abs.((p_opt - p_true) ./ y_err) +@test all(z_score .< 3) # All parameters should be within 3 standard deviations + ysim = simulate(model, U) ypred = predict(model, d, model.x0) diff --git a/test/test_pem.jl b/test/test_pem.jl index 7ab6a6af..27729d69 100644 --- a/test/test_pem.jl +++ b/test/test_pem.jl @@ -1,6 +1,8 @@ using ControlSystemIdentification, Optim, ControlSystemsBase using ControlSystemsBase.DemoSystems: resonant -using Random, Test +import LowLevelParticleFilters: SimpleMvNormal +using MonteCarloMeasurements +using Random, Test, LinearAlgebra Random.seed!(1) T = 1000 sys = c2d(resonant(ω0 = 0.1) * tf(1, [0.1, 1]), 1)# generate_system(nx, nu, ny) @@ -339,7 +341,7 @@ end P_true = constructor(p_true) u = sign.(sin.((1 / 3 * t)')) .+ sign.(sin.((1 / 2 * t)')) - res_true = lsim(P_true, u, t) + res_true = lsim(c2d(P_true, Ts, :tustin), u, t) d = iddata(res_true) nx = 2 @@ -347,7 +349,7 @@ end sum(abs2, p.K) end - res = ControlSystemIdentification.structured_pem(d, nx; focus=:prediction, p0, constructor, regularizer, show_trace = false, show_every = 1, iterations=300) + res = ControlSystemIdentification.structured_pem(d, nx; focus=:prediction, p0, constructor, regularizer, show_trace = false, show_every = 1, iterations=300, method=:tustin) p_est = res.res.minimizer.p @test norm(p_est - p_true) < 1e-6 @@ -355,9 +357,27 @@ end # p_est = res.res.minimizer.p # @test norm(p_est - p_true)/norm(p_true) < 1e-2 # Severe convergence problems here, but it does eventually converge using NelderMead The test is deactivated since it takes a long time - res = ControlSystemIdentification.structured_pem(d, nx; focus=:simulation, p0, constructor, regularizer, show_trace = false, show_every = 1, iterations=300) + + res = ControlSystemIdentification.structured_pem(d, nx; focus=:simulation, p0, constructor, regularizer, show_trace = false, show_every = 1, iterations=300, method=:tustin) p_est = res.res.minimizer.p @test norm(p_est - p_true) < 1e-6 + + d2 = deepcopy(d) + d2.y .+= randn.() + + res = ControlSystemIdentification.structured_pem(d2, nx; focus=:simulation, p0, constructor, regularizer, show_trace = false, show_every = 1, iterations=300, method=:tustin) + + + H = res.Λ() + Σ = inv(H) + @test tr(Σ) < 1e4 + dp = SimpleMvNormal(res.res.minimizer, Σ) + pars = Particles(rand!(Xoshiro(), dp, zeros(length(dp), 100))') + sysu, Ku, x0u = res.constructor(pars) + + # w = exp10.(LinRange(-3, 2, 200)) + # bodeplot(sysu, w, plotphase=false) + # bodeplot!(res.sys, w, plotphase=false, legend=false) end