diff --git a/Project.toml b/Project.toml index 51ae33a4..1144511d 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" authors = ["Francis Gagnon"] -version = "1.5.2" +version = "1.5.3" [deps] ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e" diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 867a237b..5acc59b1 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -125,7 +125,7 @@ julia> estim = SteadyKalmanFilter(LinModel(tf(3, [10, 1]), 0.5), nint_ym=[2], di julia> u = [1]; y = [3 - 0.1]; x̂ = round.(initstate!(estim, u, y), digits=3) 3-element Vector{Float64}: - 5.0 + 10.0 0.0 -0.1 diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 465d029a..6f1ceede 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -1210,7 +1210,7 @@ function correct_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter}, x̂0, P̂ = estim.x̂0, estim.P̂ # in-place operations to reduce allocations: P̂_Ĉmᵀ = K̂ - mul!(P̂_Ĉmᵀ, P̂.data, Ĉm') # the ".data" weirdly removes a type instability in mul! + mul!(P̂_Ĉmᵀ, P̂, Ĉm') M̂ = estim.buffer.R̂ mul!(M̂, Ĉm, P̂_Ĉmᵀ) M̂ .+= R̂ @@ -1252,7 +1252,7 @@ function predict_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter}, # in-place operations to reduce allocations: f̂!(x̂0next, û0, k0, estim, estim.model, x̂0corr, u0, d0) P̂corr_Âᵀ = estim.buffer.P̂ - mul!(P̂corr_Âᵀ, P̂corr.data, Â') # the ".data" weirdly removes a type instability in mul! + mul!(P̂corr_Âᵀ, P̂corr, Â') Â_P̂corr_Âᵀ = estim.buffer.Q̂ mul!(Â_P̂corr_Âᵀ, Â, P̂corr_Âᵀ) P̂next = estim.buffer.P̂ diff --git a/src/model/linmodel.jl b/src/model/linmodel.jl index aadb50dd..ab779713 100644 --- a/src/model/linmodel.jl +++ b/src/model/linmodel.jl @@ -82,9 +82,9 @@ with the state ``\mathbf{x}`` and output ``\mathbf{y}`` vectors. The ``\mathbf{z comprises the manipulated inputs ``\mathbf{u}`` and measured disturbances ``\mathbf{d}``, in any order. `i_u` provides the indices of ``\mathbf{z}`` that are manipulated, and `i_d`, the measured disturbances. The constructor automatically discretizes continuous systems, -resamples discrete ones if `Ts ≠ sys.Ts`, computes a new realization if not minimal, and -separates the ``\mathbf{z}`` terms in two parts (details in Extended Help). The rest of the -documentation assumes discrete dynamics since all systems end up in this form. +resamples discrete ones if `Ts ≠ sys.Ts`, computes a new balancing and minimal state-space +realization, and separates the ``\mathbf{z}`` terms in two parts (details in Extended Help). +The rest of the documentation assumes discrete models since all systems end up in this form. See also [`ss`](@extref ControlSystemsBase.ss) @@ -119,10 +119,11 @@ LinModel with a sample time Ts = 0.1 s and: `sys` is discrete and the provided argument `Ts ≠ sys.Ts`, the system is resampled by using the aforementioned discretization methods. - Note that the constructor transforms the system to its minimal realization using [`minreal`](@extref ControlSystemsBase.minreal) - for controllability/observability. As a consequence, the final state-space - representation may be different from the one provided in `sys`. It is also converted - into a more practical form (``\mathbf{D_u=0}`` because of the zero-order hold): + Note that the constructor transforms the system to its minimal and balancing realization + using [`minreal`](@extref ControlSystemsBase.minreal) for controllability/observability. + As a consequence, the final state-space representation will be presumably different from + the one provided in `sys`. It is also converted into a more practical form + (``\mathbf{D_u=0}`` because of the zero-order hold): ```math \begin{aligned} \mathbf{x}(k+1) &= \mathbf{A x}(k) + \mathbf{B_u u}(k) + \mathbf{B_d d}(k) \\ @@ -152,8 +153,8 @@ function LinModel( sysu = sminreal(sys[:,i_u]) # remove states associated to measured disturbances d sysd = sminreal(sys[:,i_d]) # remove states associated to manipulates inputs u if !iszero(sysu.D) - error("LinModel only supports strictly proper systems (state matrix D must be 0 "* - "for columns associated to manipulated inputs u)") + error("LinModel only supports strictly proper systems (state-space matrix D must "* + "be 0 for columns associated to manipulated inputs u)") end if iscontinuous(sys) isnothing(Ts) && error("Sample time Ts must be specified if sys is continuous") @@ -175,7 +176,7 @@ function LinModel( end end # minreal to merge common poles if possible and ensure controllability/observability: - sys_dis = minreal([sysu_dis sysd_dis]) # same realization if already minimal + sys_dis = minreal([sysu_dis sysd_dis]) nu = length(i_u) A = sys_dis.A Bu = sys_dis.B[:,1:nu] @@ -207,7 +208,7 @@ LinModel with a sample time Ts = 0.5 s and: ``` """ function LinModel(sys::TransferFunction, Ts::Union{Real,Nothing} = nothing; kwargs...) - sys_min = minreal(ss(sys)) # remove useless states with pole-zero cancellation + sys_min = ss(sys) # minreal is automatically called during conversion return LinModel(sys_min, Ts; kwargs...) end @@ -220,7 +221,7 @@ Discretize with zero-order hold when `sys` is a continuous system with delays. The delays must be multiples of the sample time `Ts`. """ function LinModel(sys::DelayLtiSystem, Ts::Real; kwargs...) - sys_dis = minreal(c2d(sys, Ts, :zoh)) # c2d only supports :zoh for DelayLtiSystem + sys_dis = c2d(sys, Ts, :zoh) # c2d only supports :zoh for DelayLtiSystem return LinModel(sys_dis, Ts; kwargs...) end diff --git a/test/0_test_module.jl b/test/0_test_module.jl index 134060ac..05ebb019 100644 --- a/test/0_test_module.jl +++ b/test/0_test_module.jl @@ -3,8 +3,9 @@ Ts = 400.0 sys = [ tf(1.90,[1800.0,1]) tf(1.90,[1800.0,1]) tf(1.90,[1800.0,1]); tf(-0.74,[800.0,1]) tf(0.74,[800.0,1]) tf(-0.74,[800.0,1]) ] - sys_ss = minreal(ss(sys)) - Gss = c2d(sys_ss[:,1:2], Ts, :zoh) - Gss2 = c2d(sys_ss[:,1:2], 0.5Ts, :zoh) + sys_ss = ss(sys) + sys_ss_u = sminreal(sys_ss[:,1:2]) + Gss = minreal(c2d(sys_ss_u, Ts, :zoh)) + Gss2 = minreal(c2d(sys_ss_u, 0.5Ts, :zoh)) export Ts, sys, sys_ss, Gss, Gss2 end \ No newline at end of file diff --git a/test/1_test_sim_model.jl b/test/1_test_sim_model.jl index be50f145..d054a3a0 100644 --- a/test/1_test_sim_model.jl +++ b/test/1_test_sim_model.jl @@ -37,9 +37,9 @@ @test linmodel5.nu == 2 @test linmodel5.nd == 1 @test linmodel5.ny == 2 - sysu_ss = sminreal(c2d(minreal(ss(sys))[:,1:2], Ts, :zoh)) - sysd_ss = sminreal(c2d(minreal(ss(sys))[:,3], Ts, :tustin)) - sys_ss = [sysu_ss sysd_ss] + sysu_ss = c2d(sminreal(ss(sys)[:,1:2]), Ts, :zoh) + sysd_ss = c2d(sminreal(ss(sys)[:,3]), Ts, :tustin) + sys_ss = minreal([sysu_ss sysd_ss]) @test linmodel5.A ≈ sys_ss.A @test linmodel5.Bu ≈ sys_ss.B[:,1:2] @test linmodel5.Bd ≈ sys_ss.B[:,3] @@ -51,14 +51,19 @@ linmodel6 = LinModel([delay(Ts) delay(Ts)]*sys,Ts,i_d=[3]) @test linmodel6.nx == 3 - @test sum(eigvals(linmodel6.A) .≈ 0) == 1 - - linmodel7 = LinModel( - ss(diagm( .1: .1: .3), I(3), diagm( .4: .1: .6), 0, 1.0), - i_u=[1, 2], - i_d=[3]) - @test linmodel7.A ≈ diagm( .1: .1: .3) - @test linmodel7.C ≈ diagm( .4: .1: .6) + @test sum(isapprox.(eigvals(linmodel6.A), 0, atol=1e-15)) == 1 + + A = diagm( .1: .1: .3) + Bu = [I(2); zeros(1,2)] + C = diagm( .4: .1: .6) + Bd = [zeros(2,1); I(1)] + Dd = 0; + linmodel7 = LinModel(A, Bu, C, Bd, Dd, 1.0) + @test linmodel7.A ≈ A + @test linmodel7.Bu ≈ Bu + @test linmodel7.Bd ≈ Bd + @test linmodel7.C ≈ C + @test linmodel7.Dd ≈ zeros(3,1) linmodel8 = LinModel(Gss.A, Gss.B, Gss.C, zeros(Float32, 2, 0), zeros(Float32, 2, 0), Ts) @test isa(linmodel8, LinModel{Float64}) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 88f5f546..9b5b3e0e 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1334,8 +1334,8 @@ end @testitem "MovingHorizonEstimator v.s. Kalman filters" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel1 = setop!(LinModel(sys,Ts,i_d=[3]), uop=[10,50], yop=[50,30], dop=[20]) - mhe = MovingHorizonEstimator(linmodel1, He=3, nint_ym=0, direct=false) kf = KalmanFilter(linmodel1, nint_ym=0, direct=false) + mhe = MovingHorizonEstimator(linmodel1, He=3, nint_ym=0, direct=false) X̂_mhe = zeros(4, 6) X̂_kf = zeros(4, 6) for i in 1:6 @@ -1347,13 +1347,17 @@ end updatestate!(mhe, [11, 50], y, [25]) updatestate!(kf, [11, 50], y, [25]) end - @test X̂_mhe ≈ X̂_kf atol=1e-3 rtol=1e-3 - mhe = MovingHorizonEstimator(linmodel1, He=3, nint_ym=0, direct=true) + @test X̂_mhe ≈ X̂_kf atol=1e-6 rtol=1e-6 kf = KalmanFilter(linmodel1, nint_ym=0, direct=true) + # recuperate P̂(-1|-1) exact value using the Kalman filter: + preparestate!(kf, [50, 30], [20]) + σP̂ = sqrt.(diag(kf.P̂)) + mhe = MovingHorizonEstimator(linmodel1, He=3, nint_ym=0, direct=true, σP_0=σP̂) + updatestate!(kf, [10, 50], [50, 30], [20]) X̂_mhe = zeros(4, 6) X̂_kf = zeros(4, 6) for i in 1:6 - y = [50,31] + randn(2) + y = [50,31] #+ randn(2) x̂_mhe = preparestate!(mhe, y, [25]) x̂_kf = preparestate!(kf, y, [25]) X̂_mhe[:,i] = x̂_mhe @@ -1361,14 +1365,15 @@ end updatestate!(mhe, [11, 50], y, [25]) updatestate!(kf, [11, 50], y, [25]) end - @test X̂_mhe ≈ X̂_kf atol=1e-3 rtol=1e-3 + @test X̂_mhe ≈ X̂_kf atol=1e-6 rtol=1e-6 + f = (x,u,d,model) -> model.A*x + model.Bu*u + model.Bd*d h = (x,d,model) -> model.C*x + model.Dd*d nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, p=linmodel1, solver=nothing) nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30], dop=[20]) - mhe = MovingHorizonEstimator(nonlinmodel, He=5, nint_ym=0, direct=false) ukf = UnscentedKalmanFilter(nonlinmodel, nint_ym=0, direct=false) ekf = ExtendedKalmanFilter(nonlinmodel, nint_ym=0, direct=false) + mhe = MovingHorizonEstimator(nonlinmodel, He=5, nint_ym=0, direct=false) X̂_mhe = zeros(4, 6) X̂_ukf = zeros(4, 6) X̂_ekf = zeros(4, 6) @@ -1384,11 +1389,18 @@ end updatestate!(ukf, [11, 50], y, [25]) updatestate!(ekf, [11, 50], y, [25]) end - @test X̂_mhe ≈ X̂_ukf atol=1e-3 rtol=1e-3 - @test X̂_mhe ≈ X̂_ekf atol=1e-3 rtol=1e-3 - mhe = MovingHorizonEstimator(nonlinmodel, He=5, nint_ym=0, direct=true) + @test X̂_mhe ≈ X̂_ukf atol=1e-6 rtol=1e-6 + @test X̂_mhe ≈ X̂_ekf atol=1e-6 rtol=1e-6 + ukf = UnscentedKalmanFilter(nonlinmodel, nint_ym=0, direct=true) ekf = ExtendedKalmanFilter(nonlinmodel, nint_ym=0, direct=true) + # recuperate P̂(-1|-1) exact value using the Unscented Kalman filter: + preparestate!(ukf, [50, 30], [20]) + preparestate!(ekf, [50, 30], [20]) + σP̂ = sqrt.(diag(ukf.P̂)) + mhe = MovingHorizonEstimator(nonlinmodel, He=5, nint_ym=0, direct=true, σP_0=σP̂) + updatestate!(ukf, [10, 50], [50, 30], [20]) + updatestate!(ekf, [10, 50], [50, 30], [20]) X̂_mhe = zeros(4, 6) X̂_ukf = zeros(4, 6) X̂_ekf = zeros(4, 6) @@ -1404,8 +1416,8 @@ end updatestate!(ukf, [11, 50], y, [25]) updatestate!(ekf, [11, 50], y, [25]) end - @test X̂_mhe ≈ X̂_ukf atol=1e-3 rtol=1e-3 - @test X̂_mhe ≈ X̂_ekf atol=1e-3 rtol=1e-3 + @test X̂_mhe ≈ X̂_ukf atol=1e-6 rtol=1e-6 + @test X̂_mhe ≈ X̂_ekf atol=1e-6 rtol=1e-6 end @testitem "MovingHorizonEstimator LinModel v.s. NonLinModel" setup=[SetupMPCtests] begin diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 42a3f221..3e5e27f1 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -705,13 +705,13 @@ end @test_nowarn geq_end(5.0, 4.0, 3.0, 2.0) nmpc6 = NonLinMPC(linmodel3, Hp=10) preparestate!(nmpc6, [0]) - @test moveinput!(nmpc6, [0]) ≈ [0.0] + @test moveinput!(nmpc6, [0]) ≈ [0.0] atol=5e-2 nonlinmodel2 = NonLinModel{Float32}(f, h, 3000.0, 1, 2, 1, 1, solver=nothing, p=linmodel2) nmpc7 = NonLinMPC(nonlinmodel2, Hp=10) y = similar(nonlinmodel2.yop) nonlinmodel2.solver_h!(y, Float32[0,0], Float32[0], nonlinmodel2.p) preparestate!(nmpc7, [0], [0]) - @test moveinput!(nmpc7, [0], [0]) ≈ [0.0] + @test moveinput!(nmpc7, [0], [0]) ≈ [0.0] atol=5e-2 nmpc8 = NonLinMPC(nonlinmodel, Nwt=[0], Hp=100, Hc=1, transcription=MultipleShooting()) preparestate!(nmpc8, [0], [0]) u = moveinput!(nmpc8, [10], [0])