add experimental covariance estimate to structured_pem

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)

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

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)
 

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,25 +341,43 @@ 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
     function regularizer(p, _)
         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
 
     # res = ControlSystemIdentification.structured_pem(d, nx; focus=:prediction, p0=0.98p_true, constructor, regularizer, show_trace = true, show_every = 1, iterations=300000, h=6, optimizer=NelderMead())
     # 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