From 5981e7650eca5f3725cfa933884b8778e334e409 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 19 Apr 2025 14:42:20 -0400 Subject: [PATCH 1/3] =?UTF-8?q?added:=20`setstate!`=20now=20allows=20cov?= =?UTF-8?q?=20estimate=20`P=CC=82`=20modification?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/controller/execute.jl | 7 +++---- src/estimator/execute.jl | 18 +++++++++++++++--- src/estimator/internal_model.jl | 6 ++++++ src/estimator/luenberger.jl | 6 ++++++ src/estimator/mhe/execute.jl | 6 ++++++ 5 files changed, 36 insertions(+), 7 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 17e8ff62..a3925111 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -488,12 +488,11 @@ Call `periodsleep(mpc.estim.model)`. periodsleep(mpc::PredictiveController, busywait=false) = periodsleep(mpc.estim.model, busywait) """ - setstate!(mpc::PredictiveController, x̂) -> mpc + setstate!(mpc::PredictiveController, x̂, P̂=nothing) -> mpc -Set `mpc.estim.x̂0` to `x̂ - estim.x̂op` from the argument `x̂`. +Call [`setstate!`](@ref) on `mpc.estim` [`StateEstimator`](@ref). """ -setstate!(mpc::PredictiveController, x̂) = (setstate!(mpc.estim, x̂); return mpc) - +setstate!(mpc::PredictiveController, x̂, P̂=nothing) = (setstate!(mpc.estim, x̂, P̂); return mpc) @doc raw""" setmodel!(mpc::PredictiveController, model=mpc.estim.model; ) -> mpc diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 867a237b..f107a470 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -330,16 +330,28 @@ function validate_args(estim::StateEstimator, ym, d, u=nothing) end """ - setstate!(estim::StateEstimator, x̂) -> estim + setstate!(estim::StateEstimator, x̂, P̂=nothing) -> estim -Set `estim.x̂0` to `x̂ - estim.x̂op` from the argument `x̂`. +Set `estim.x̂0` to `x̂ - estim.x̂op` from the argument `x̂`, and `estim.P̂` to `P̂` if applicable. + +The covariance error estimate `P̂` can be set only if `estim` computes it. """ -function setstate!(estim::StateEstimator, x̂) +function setstate!(estim::StateEstimator, x̂, P̂=nothing) size(x̂) == (estim.nx̂,) || error("x̂ size must be $((estim.nx̂,))") estim.x̂0 .= x̂ .- estim.x̂op + setstate_cov!(estim, P̂) return estim end +"Set the covariance error estimate `estim.P̂` to `P̂`." +function setstate_cov!(estim::StateEstimator, P̂) + if !isnothing(P̂) + size(P̂) == (estim.nx̂, estim.nx̂) || error("P̂ size must be $((estim.nx̂, estim.nx̂))") + estim.P̂ .= to_hermitian(P̂) + end + return nothing +end + @doc raw""" setmodel!(estim::StateEstimator, model=estim.model; ) -> estim diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index 18475652..6619e48d 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -226,6 +226,12 @@ function init_internalmodel(As, Bs, Cs, Ds) return Âs, B̂s end +"Throw an error if P̂ != nothing." +function setstate_cov!(estim::InternalModel, P̂) + P̂ == nothing || error("InternalModel does not compute an estimation covariance matrix P̂.") + return nothing +end + "Update similar values for [`InternalModel`](@ref) estimator." function setmodel_estimator!(estim::InternalModel, model, _ , _ , _ , _ , _ ) Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = matrices_internalmodel(model) diff --git a/src/estimator/luenberger.jl b/src/estimator/luenberger.jl index 430f9b2a..93f9d749 100644 --- a/src/estimator/luenberger.jl +++ b/src/estimator/luenberger.jl @@ -136,6 +136,12 @@ function update_estimate!(estim::Luenberger, y0m, d0, u0) return predict_estimate_obsv!(estim, y0m, d0, u0) end +"Throw an error if P̂ != nothing." +function setstate_cov!(estim::Luenberger, P̂) + P̂ == nothing || error("Luenberger does not compute an estimation covariance matrix P̂.") + return nothing +end + "Throw an error if `setmodel!` is called on `Luenberger` observer w/o the default values." function setmodel_estimator!(estim::Luenberger, model, args...) if estim.model !== model diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 8a441c6a..38041df0 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -650,6 +650,12 @@ end "No nonlinear constraints if `model` is a [`LinModel`](@ref), return `g` unchanged." con_nonlinprog!(g, ::MovingHorizonEstimator, ::LinModel, _ , _ , _ ) = g +"Throw an error if P̂ != nothing." +function setstate_cov!(estim::MovingHorizonEstimator, P̂) + P̂ == nothing || error("MovingHorizonEstimator does not compute an estimation covariance matrix P̂.") + return nothing +end + "Update the augmented model, prediction matrices, constrains and data windows for MHE." function setmodel_estimator!( estim::MovingHorizonEstimator, model, uop_old, yop_old, dop_old, Q̂, R̂ From e11147f480ab14f92fc570f3b8612130472db064 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 19 Apr 2025 16:52:40 -0400 Subject: [PATCH 2/3] =?UTF-8?q?test:=20new=20tests=20with=20`P=CC=82`=20mo?= =?UTF-8?q?dification=20in=20`setstate!`?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/estimator/execute.jl | 3 ++- test/2_test_state_estim.jl | 13 ++++++++++--- test/3_test_predictive_control.jl | 7 ++++--- 3 files changed, 16 insertions(+), 7 deletions(-) diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 10dc4424..23408995 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -334,7 +334,8 @@ end Set `estim.x̂0` to `x̂ - estim.x̂op` from the argument `x̂`, and `estim.P̂` to `P̂` if applicable. -The covariance error estimate `P̂` can be set only if `estim` computes it. +The covariance error estimate `P̂` can be set only if `estim` is a [`StateEstimator`](@ref) +that computes it. """ function setstate!(estim::StateEstimator, x̂, P̂=nothing) size(x̂) == (estim.nx̂,) || error("x̂ size must be $((estim.nx̂,))") diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 9b5b3e0e..c1cb8e76 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -115,6 +115,7 @@ end @test x̂ ≈ [0, 0] @test isa(x̂, Vector{Float32}) @test_throws ArgumentError updatestate!(skalmanfilter1, [10, 50]) + @test_throws ErrorException setstate!(skalmanfilter1, [1,2,3,4], diagm(.1:.1:.4)) end @testitem "SteadyKalmanFilter set model" setup=[SetupMPCtests] begin @@ -211,8 +212,9 @@ end @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]) + setstate!(kalmanfilter1, [1,2,3,4], diagm(.1:.1:.4)) @test kalmanfilter1.x̂0 ≈ [1,2,3,4] + @test kalmanfilter1.P̂ ≈ diagm(.1:.1:.4) for i in 1:40 preparestate!(kalmanfilter1, [50, 30]) updatestate!(kalmanfilter1, [11, 52], [50, 30]) @@ -363,6 +365,7 @@ end x̂ = updatestate!(lo3, [0], [0]) @test x̂ ≈ [0, 0] @test isa(x̂, Vector{Float32}) + @test_throws ErrorException setstate!(lo1, [1,2,3,4], diagm(.1:.1:.4)) end @testitem "Luenberger set model" setup=[SetupMPCtests] begin @@ -484,6 +487,7 @@ end x̂ = updatestate!(internalmodel3, [0], [0]) @test x̂ ≈ [0] @test isa(x̂, Vector{Float32}) + @test_throws ErrorException setstate!(internalmodel1, [1,2,3,4], diagm(.1:.1:.4)) end @testitem "InternalModel set model" setup=[SetupMPCtests] begin @@ -598,8 +602,9 @@ end @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]) + setstate!(ukf1, [1,2,3,4], diagm(.1:.1:.4)) @test ukf1.x̂0 ≈ [1,2,3,4] + @test ukf1.P̂ ≈ diagm(.1:.1:.4) for i in 1:40 preparestate!(ukf1, [50, 30]) updatestate!(ukf1, [11, 52], [50, 30]) @@ -757,8 +762,9 @@ end @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]) + setstate!(ekf1, [1,2,3,4], diagm(.1:.1:.4)) @test ekf1.x̂0 ≈ [1,2,3,4] + @test ekf1.P̂ ≈ diagm(.1:.1:.4) for i in 1:40 preparestate!(ekf1, [50, 30]) updatestate!(ekf1, [11, 52], [50, 30]) @@ -1055,6 +1061,7 @@ end x̂ = preparestate!(mhe6, [50, 30], [5]) @test x̂ ≈ zeros(6) atol=1e-9 @test_nowarn ModelPredictiveControl.info2debugstr(info) + @test_throws ErrorException setstate!(mhe1, [1,2,3,4,5,6], diagm(.1:.1:.6)) end @testitem "MovingHorizonEstimator fallbacks for arrival covariance estimation" setup=[SetupMPCtests] begin diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 3e5e27f1..65209bec 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -163,11 +163,12 @@ end @testitem "LinMPC other methods" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel1 = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30]) - mpc1 = LinMPC(linmodel1) + mpc1 = LinMPC(KalmanFilter(linmodel1)) @test initstate!(mpc1, [10, 50], [50, 30+1]) ≈ [zeros(3); [1]] - setstate!(mpc1, [1,2,3,4]) + setstate!(mpc1, [1,2,3,4], diagm(.1:.1:.4)) @test mpc1.estim.x̂0 ≈ [1,2,3,4] - setstate!(mpc1, [0,0,0,0]) + @test mpc1.estim.P̂ ≈ diagm(.1:.1:.4) + setstate!(mpc1, [0,0,0,0], mpc1.estim.P̂_0) preparestate!(mpc1, [50, 30]) updatestate!(mpc1, mpc1.estim.model.uop, [50, 30]) @test mpc1.estim.x̂0 ≈ [0,0,0,0] From 1367a55f8167b3b8821f75567c23ffebabb34a12 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 19 Apr 2025 16:57:45 -0400 Subject: [PATCH 3/3] changed: update codecov to v5 in CI --- .github/workflows/CI.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/CI.yml b/.github/workflows/CI.yml index 1a0c374e..09aba0bd 100644 --- a/.github/workflows/CI.yml +++ b/.github/workflows/CI.yml @@ -43,7 +43,7 @@ jobs: - uses: julia-actions/julia-buildpkg@v1 - uses: julia-actions/julia-runtest@v1 - uses: julia-actions/julia-processcoverage@v1 - - uses: codecov/codecov-action@v4 + - uses: codecov/codecov-action@v5 with: token: ${{ secrets.CODECOV_TOKEN }} fail_ci_if_error: false \ No newline at end of file