diff --git a/.github/workflows/CI.yml b/.github/workflows/CI.yml index 10c7f8f62..420205703 100644 --- a/.github/workflows/CI.yml +++ b/.github/workflows/CI.yml @@ -17,7 +17,7 @@ jobs: permissions: # needed for julia-actions/cache delete old caches that it has created actions: write contents: read - continue-on-error: ${{ matrix.version == 'nightly' }} + continue-on-error: ${{ matrix.version == 'nightly' || matrix.version == 'pre' }} strategy: fail-fast: false matrix: diff --git a/src/model/linearization.jl b/src/model/linearization.jl index fe0ca333b..54958a772 100644 --- a/src/model/linearization.jl +++ b/src/model/linearization.jl @@ -155,7 +155,8 @@ julia> linearize!(linmodel, model, x=[20.0], u=[0.0]); linmodel.A ``` """ function linearize!( - linmodel::LinModel{NT}, model::SimModel; x=model.x0+model.xop, u=model.uop, d=model.dop + linmodel::LinModel{NT}, model::SimModel; + x=(model.buffer.x.=model.x0.+model.xop), u=model.uop, d=model.dop ) where NT<:Real nonlinmodel = model buffer = nonlinmodel.buffer diff --git a/test/1_test_sim_model.jl b/test/1_test_sim_model.jl index 18a769d14..be50f1457 100644 --- a/test/1_test_sim_model.jl +++ b/test/1_test_sim_model.jl @@ -100,11 +100,14 @@ end @testitem "LinModel sim methods" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase linmodel1 = setop!(LinModel(Gss), uop=[10,50], yop=[50,30]) - @test updatestate!(linmodel1, [10, 50]) ≈ zeros(2) - @test updatestate!(linmodel1, [10, 50], Float64[]) ≈ zeros(2) + u, d = [10, 50], Float64[] + @test updatestate!(linmodel1, u) ≈ zeros(2) + @test updatestate!(linmodel1, u, d) ≈ zeros(2) + @test_skip @allocations(updatestate!(linmodel1, u)) == 0 @test linmodel1.x0 ≈ zeros(2) @test evaloutput(linmodel1) ≈ linmodel1() ≈ [50,30] @test evaloutput(linmodel1, Float64[]) ≈ linmodel1(Float64[]) ≈ [50,30] + @test_skip @allocations(evaloutput(linmodel1)) == 0 x = initstate!(linmodel1, [10, 60]) @test evaloutput(linmodel1) ≈ [50 + 19.0, 30 + 7.4] @test preparestate!(linmodel1) ≈ x # new method @@ -155,9 +158,9 @@ end using DifferentiationInterface import FiniteDiff linmodel1 = LinModel(sys,Ts,i_u=[1,2]) - f1(x,u,_,model) = model.A*x + model.Bu*u - h1(x,_,model) = model.C*x - nonlinmodel1 = NonLinModel(f1,h1,Ts,2,2,2,solver=nothing,p=linmodel1) + f1!(x,u,_,model) = model.A*x + model.Bu*u + h1!(x,_,model) = model.C*x + nonlinmodel1 = NonLinModel(f1!,h1!,Ts,2,2,2,solver=nothing,p=linmodel1) @test nonlinmodel1.nx == 2 @test nonlinmodel1.nu == 2 @test nonlinmodel1.nd == 0 @@ -265,15 +268,21 @@ end @testitem "NonLinModel sim methods" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel1 = LinModel(sys,Ts,i_u=[1,2]) - f1(x,u,_,model) = model.A*x + model.Bu*u - h1(x,_,model) = model.C*x - nonlinmodel = NonLinModel(f1,h1,Ts,2,2,2,p=linmodel1,solver=nothing) + function f1!(xnext, x, u, _ , model) + mul!(xnext, model.A, x) + mul!(xnext, model.Bu, u, 1, 1) + end + h1!(y, x , _ , model) = mul!(y, model.C, x) + nonlinmodel = NonLinModel(f1!,h1!,Ts,2,2,2,p=linmodel1,solver=nothing) - @test updatestate!(nonlinmodel, zeros(2,)) ≈ zeros(2) - @test updatestate!(nonlinmodel, zeros(2,), Float64[]) ≈ zeros(2) + u, d = zeros(2), Float64[] + @test updatestate!(nonlinmodel, u) ≈ zeros(2) + @test updatestate!(nonlinmodel, u, d) ≈ zeros(2) + @test_skip @allocations(updatestate!(nonlinmodel, u)) == 0 @test nonlinmodel.x0 ≈ zeros(2) @test evaloutput(nonlinmodel) ≈ nonlinmodel() ≈ zeros(2) - @test evaloutput(nonlinmodel, Float64[]) ≈ nonlinmodel(Float64[]) ≈ zeros(2) + @test evaloutput(nonlinmodel, d) ≈ nonlinmodel(Float64[]) ≈ zeros(2) + @test_skip @allocations(evaloutput(nonlinmodel)) == 0 x = initstate!(nonlinmodel, [0, 10]) # do nothing for NonLinModel @test evaloutput(nonlinmodel) ≈ [0, 0] @@ -287,9 +296,9 @@ end using DifferentiationInterface import ForwardDiff, FiniteDiff Ts = 1.0 - f1(x,u,d,_) = x.^5 .+ u.^4 .+ d.^3 - h1(x,d,_) = x.^2 .+ d - nonlinmodel1 = NonLinModel(f1,h1,Ts,1,1,1,1,solver=nothing) + f1!(x,u,d,_) = x.^5 .+ u.^4 .+ d.^3 + h1!(x,d,_) = x.^2 .+ d + nonlinmodel1 = NonLinModel(f1!,h1!,Ts,1,1,1,1,solver=nothing) x, u, d = [2.0], [3.0], [4.0] linmodel1 = linearize(nonlinmodel1; x, u, d) @test linmodel1.A ≈ 5*x.^4 @@ -302,9 +311,9 @@ end @test linmodel1.Bu ≈ linmodel1b.Bu @test linmodel1.Bd ≈ linmodel1b.Bd @test linmodel1.C ≈ linmodel1b.C - @test linmodel1.Dd ≈ linmodel1b.Dd + @test linmodel1.Dd ≈ linmodel1b.Dd - nonlinmodel2 = NonLinModel(f1,h1,Ts,1,1,1,1,solver=nothing, jacobian=AutoFiniteDiff()) + nonlinmodel2 = NonLinModel(f1!,h1!,Ts,1,1,1,1,solver=nothing, jacobian=AutoFiniteDiff()) linmodel2 = linearize(nonlinmodel2; x, u, d) @test linmodel2.A ≈ 5*x.^4 atol=1e-3 @test linmodel2.Bu ≈ 4*u.^3 atol=1e-3 @@ -334,7 +343,7 @@ end @test linmodel3.Bd ≈ Bd @test linmodel3.C ≈ C @test linmodel3.Dd ≈ Dd - + # test `linearize` at a non-equilibrium point: Ynl, Yl = let nonlinmodel3=nonlinmodel3 N = 5 @@ -348,7 +357,7 @@ end yl = linmodel3(d) Ynl[i] = ynl[1] Yl[i] = yl[1] - linmodel3 = linearize(nonlinmodel3; u, d) + linearize!(linmodel3, nonlinmodel3; u, d) updatestate!(nonlinmodel3, u, d) updatestate!(linmodel3, u, d) end @@ -356,10 +365,24 @@ end end @test all(isapprox.(Ynl, Yl, atol=1e-6)) + # test nd==0 also works with AutoFiniteDiff (does not support empty matrices): f2!(xnext, x, u, _, _) = (xnext .= x .+ u) h2!(y, x, _, _) = (y .= x) nonlinmodel4 = NonLinModel(f2!,h2!,Ts,1,1,1,0,solver=nothing,jacobian=AutoFiniteDiff()) @test_nowarn linearize(nonlinmodel4, x=[1], u=[2]) + + function f3!(xnext, x, u, d, _) + xnext .= x.*u .+ x.*d + end + function h3!(y, x, d, _) + y .= x.*d + end + nonlinmodel4 = NonLinModel(f3!, h3!, Ts, 1, 1, 1, 1, solver=nothing) + linmodel4 = linearize(nonlinmodel4; x, u, d) + # return nothing (see this issue : https://github.com/JuliaLang/julia/issues/51112): + linearize2!(linmodel, model) = (linearize!(linmodel, model); nothing) + linearize2!(linmodel4, nonlinmodel4) + @test_skip @allocations(linearize2!(linmodel4, nonlinmodel4)) == 0 end @testitem "NonLinModel real time simulations" setup=[SetupMPCtests] begin diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index eef093e91..88f5f5462 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -65,14 +65,18 @@ end using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel1 = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30]) skalmanfilter1 = SteadyKalmanFilter(linmodel1, nint_ym=[1, 1]) - preparestate!(skalmanfilter1, [50, 30]) - @test updatestate!(skalmanfilter1, [10, 50], [50, 30]) ≈ zeros(4) - preparestate!(skalmanfilter1, [50, 30]) - @test updatestate!(skalmanfilter1, [10, 50], [50, 30], Float64[]) ≈ zeros(4) + u, y, d = [10, 50], [50, 30], Float64[] + preparestate!(skalmanfilter1, y) + @test updatestate!(skalmanfilter1, u, y) ≈ zeros(4) + preparestate!(skalmanfilter1, y) + @test updatestate!(skalmanfilter1, u, y, d) ≈ zeros(4) @test skalmanfilter1.x̂0 ≈ zeros(4) - preparestate!(skalmanfilter1, [50, 30]) + @test_skip @allocations(preparestate!(skalmanfilter1, y)) == 0 + @test_skip @allocations(updatestate!(skalmanfilter1, u, y)) == 0 + preparestate!(skalmanfilter1, y) @test evaloutput(skalmanfilter1) ≈ skalmanfilter1() ≈ [50, 30] - @test evaloutput(skalmanfilter1, Float64[]) ≈ skalmanfilter1(Float64[]) ≈ [50, 30] + @test evaloutput(skalmanfilter1, d) ≈ skalmanfilter1(d) ≈ [50, 30] + @test_skip @allocations(evaloutput(skalmanfilter1, d)) == 0 @test initstate!(skalmanfilter1, [10, 50], [50, 30+1]) ≈ [zeros(3); [1]] linmodel2 = LinModel(append(tf(1, [1, 0]), tf(2, [10, 1])), 1.0) skalmanfilter2 = SteadyKalmanFilter(linmodel2, nint_u=[1, 1], direct=false) @@ -194,14 +198,18 @@ end using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel1 = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30]) kalmanfilter1 = KalmanFilter(linmodel1) - preparestate!(kalmanfilter1, [50, 30]) - @test updatestate!(kalmanfilter1, [10, 50], [50, 30]) ≈ zeros(4) - preparestate!(kalmanfilter1, [50, 30]) - @test updatestate!(kalmanfilter1, [10, 50], [50, 30], Float64[]) ≈ zeros(4) + u, y, d = [10, 50], [50, 30], Float64[] + preparestate!(kalmanfilter1, y) + @test updatestate!(kalmanfilter1, u, y) ≈ zeros(4) + preparestate!(kalmanfilter1, y) + @test updatestate!(kalmanfilter1, u, y, d) ≈ zeros(4) @test kalmanfilter1.x̂0 ≈ zeros(4) - preparestate!(kalmanfilter1, [50, 30]) + @test_skip @allocations(preparestate!(kalmanfilter1, y)) == 0 + @test_skip @allocations(updatestate!(kalmanfilter1, u, y)) == 0 + preparestate!(kalmanfilter1, y) @test evaloutput(kalmanfilter1) ≈ kalmanfilter1() ≈ [50, 30] - @test evaloutput(kalmanfilter1, Float64[]) ≈ kalmanfilter1(Float64[]) ≈ [50, 30] + @test evaloutput(kalmanfilter1, d) ≈ kalmanfilter1(d) ≈ [50, 30] + @test_skip @allocations(evaloutput(kalmanfilter1, d)) == 0 @test initstate!(kalmanfilter1, [10, 50], [50, 30+1]) ≈ [zeros(3); [1]] setstate!(kalmanfilter1, [1,2,3,4]) @test kalmanfilter1.x̂0 ≈ [1,2,3,4] @@ -311,14 +319,18 @@ end using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel1 = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30]) lo1 = Luenberger(linmodel1, nint_ym=[1, 1]) - preparestate!(lo1, [50, 30]) - @test updatestate!(lo1, [10, 50], [50, 30]) ≈ zeros(4) - preparestate!(lo1, [50, 30]) - @test updatestate!(lo1, [10, 50], [50, 30], Float64[]) ≈ zeros(4) + u, y, d = [10, 50], [50, 30], Float64[] + preparestate!(lo1, y) + @test updatestate!(lo1, u, y) ≈ zeros(4) + preparestate!(lo1, y) + @test updatestate!(lo1, u, y, d) ≈ zeros(4) @test lo1.x̂0 ≈ zeros(4) - preparestate!(lo1, [50, 30]) + @test_skip @allocations(preparestate!(lo1, y)) == 0 + @test_skip @allocations(updatestate!(lo1, u, y)) == 0 + preparestate!(lo1, y) @test evaloutput(lo1) ≈ lo1() ≈ [50, 30] - @test evaloutput(lo1, Float64[]) ≈ lo1(Float64[]) ≈ [50, 30] + @test evaloutput(lo1, d) ≈ lo1(d) ≈ [50, 30] + @test_skip @allocations(evaloutput(lo1, d)) == 0 @test initstate!(lo1, [10, 50], [50, 30+1]) ≈ [zeros(3); [1]] setstate!(lo1, [1,2,3,4]) @test lo1.x̂0 ≈ [1,2,3,4] @@ -436,14 +448,18 @@ end using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel1 = setop!(LinModel(sys,Ts,i_u=[1,2]) , uop=[10,50], yop=[50,30]) internalmodel1 = InternalModel(linmodel1) - preparestate!(internalmodel1, [50, 30] .+ 1) - @test updatestate!(internalmodel1, [10, 50], [50, 30] .+ 1) ≈ zeros(2) - preparestate!(internalmodel1, [50, 30] .+ 1) - @test updatestate!(internalmodel1, [10, 50], [50, 30] .+ 1, Float64[]) ≈ zeros(2) + u, y, d = [10, 50], [50, 30] .+ 1, Float64[] + preparestate!(internalmodel1, y) + @test updatestate!(internalmodel1, u, y) ≈ zeros(2) + preparestate!(internalmodel1, y) + @test updatestate!(internalmodel1, u, y, d) ≈ zeros(2) @test internalmodel1.x̂d ≈ internalmodel1.x̂0 ≈ zeros(2) @test internalmodel1.x̂s ≈ ones(2) - preparestate!(internalmodel1, [51, 31]) - @test evaloutput(internalmodel1, Float64[]) ≈ [51,31] + @test_skip @allocations(preparestate!(internalmodel1, y)) == 0 + @test_skip @allocations(updatestate!(internalmodel1, u, y)) == 0 + preparestate!(internalmodel1, y) + @test evaloutput(internalmodel1, d) ≈ [51,31] + @test_skip @allocations(evaloutput(internalmodel1, d)) == 0 @test initstate!(internalmodel1, [10, 50], [50, 30]) ≈ zeros(2) @test internalmodel1.x̂s ≈ zeros(2) setstate!(internalmodel1, [1,2]) @@ -557,19 +573,30 @@ end @testitem "UnscentedKalmanFilter estimator methods" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel1 = LinModel(sys,Ts,i_u=[1,2]) - f(x,u,_,model) = model.A*x + model.Bu*u - h(x,_,model) = model.C*x - nonlinmodel = NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing, p=linmodel1) + function f!(xnext, x,u,_,model) + mul!(xnext, model.A, x) + mul!(xnext, model.Bu, u, 1, 1) + return nothing + end + function h!(y, x,_,model) + mul!(y, model.C, x) + return nothing + end + nonlinmodel = NonLinModel(f!, h!, Ts, 2, 2, 2, solver=nothing, p=linmodel1) nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30]) ukf1 = UnscentedKalmanFilter(nonlinmodel) - preparestate!(ukf1, [50, 30]) - @test updatestate!(ukf1, [10, 50], [50, 30]) ≈ zeros(4) atol=1e-9 - preparestate!(ukf1, [50, 30]) - @test updatestate!(ukf1, [10, 50], [50, 30], Float64[]) ≈ zeros(4) atol=1e-9 + u, y, d = [10, 50], [50, 30], Float64[] + preparestate!(ukf1, y) + @test updatestate!(ukf1, u, y) ≈ zeros(4) atol=1e-9 + preparestate!(ukf1, y) + @test updatestate!(ukf1, u, y, d) ≈ zeros(4) atol=1e-9 @test ukf1.x̂0 ≈ zeros(4) atol=1e-9 - preparestate!(ukf1, [50, 30]) + @test_skip @allocations(preparestate!(ukf1, y)) == 0 + @test_skip @allocations(updatestate!(ukf1, u, y)) == 0 + preparestate!(ukf1, y) @test evaloutput(ukf1) ≈ ukf1() ≈ [50, 30] - @test evaloutput(ukf1, Float64[]) ≈ ukf1(Float64[]) ≈ [50, 30] + @test evaloutput(ukf1, d) ≈ ukf1(d) ≈ [50, 30] + @test_skip @allocations(evaloutput(ukf1, d)) == 0 @test initstate!(ukf1, [10, 50], [50, 30+1]) ≈ zeros(4) atol=1e-9 setstate!(ukf1, [1,2,3,4]) @test ukf1.x̂0 ≈ [1,2,3,4] @@ -705,19 +732,30 @@ end using DifferentiationInterface import FiniteDiff linmodel1 = LinModel(sys,Ts,i_u=[1,2]) - f(x,u,_,model) = model.A*x + model.Bu*u - h(x,_,model) = model.C*x - nonlinmodel = NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing, p=linmodel1) + function f!(xnext, x,u,_,model) + mul!(xnext, model.A, x) + mul!(xnext, model.Bu, u, 1, 1) + return nothing + end + function h!(y, x,_,model) + mul!(y, model.C, x) + return nothing + end + nonlinmodel = NonLinModel(f!, h!, Ts, 2, 2, 2, solver=nothing, p=linmodel1) nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30]) ekf1 = ExtendedKalmanFilter(nonlinmodel) - preparestate!(ekf1, [50, 30]) - @test updatestate!(ekf1, [10, 50], [50, 30]) ≈ zeros(4) atol=1e-9 - preparestate!(ekf1, [50, 30]) - @test updatestate!(ekf1, [10, 50], [50, 30], Float64[]) ≈ zeros(4) atol=1e-9 + u, y, d = [10, 50], [50, 30], Float64[] + preparestate!(ekf1, y) + @test updatestate!(ekf1, u, y) ≈ zeros(4) atol=1e-9 + preparestate!(ekf1, y) + @test updatestate!(ekf1, u, y, d) ≈ zeros(4) atol=1e-9 @test ekf1.x̂0 ≈ zeros(4) atol=1e-9 - preparestate!(ekf1, [50, 30]) + @test_skip @allocations(preparestate!(ekf1, y)) == 0 + @test_skip @allocations(updatestate!(ekf1, u, y)) == 0 + preparestate!(ekf1, y) @test evaloutput(ekf1) ≈ ekf1() ≈ [50, 30] - @test evaloutput(ekf1, Float64[]) ≈ ekf1(Float64[]) ≈ [50, 30] + @test evaloutput(ekf1, d) ≈ ekf1(d) ≈ [50, 30] + @test_skip @allocations(evaloutput(ekf1, d)) == 0 @test initstate!(ekf1, [10, 50], [50, 30+1]) ≈ zeros(4); setstate!(ekf1, [1,2,3,4]) @test ekf1.x̂0 ≈ [1,2,3,4] diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index ba7729aa2..42a3f221d 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -425,12 +425,13 @@ end @testitem "ExplicitMPC moves and getinfo" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra mpc1 = ExplicitMPC(LinModel(tf(5, [2, 1]), 3), Nwt=[0], Hp=1000, Hc=1) - r = [5] - preparestate!(mpc1, [0]) + r, y = [5], [0] + preparestate!(mpc1, y) u = moveinput!(mpc1, r) @test u ≈ [1] atol=1e-2 u = mpc1(r) @test u ≈ [1] atol=1e-2 + @test_skip @allocations(moveinput!(mpc1, r)) == 0 info = getinfo(mpc1) @test info[:u] ≈ u @test info[:Ŷ][end] ≈ r[1] atol=1e-2