diff --git a/Project.toml b/Project.toml index 9b0e0adbf..d9bab6a3f 100644 --- a/Project.toml +++ b/Project.toml @@ -1,6 +1,6 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" -version = "2.8.0" +version = "2.8.1" authors = ["Francis Gagnon"] [deps] diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 3a80dcf96..a23a01ac5 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -274,8 +274,8 @@ provided below. ``` """ function update_estimate!(estim::SteadyKalmanFilter, u0, y0m, d0) - if !estim.direct && all(isfinite, y0m) - correct_estimate_obsv!(estim, y0m, d0) + if !estim.direct + correct_estimate!(estim, y0m, d0) end return predict_estimate_obsv!(estim, u0, d0) end @@ -518,8 +518,8 @@ provided below, see [^2] for details. , Accessed 2024-08-08. """ function update_estimate!(estim::KalmanFilter, u0, y0m, d0) - if !estim.direct && all(isfinite, y0m) - correct_estimate_kf!(estim, y0m, d0, estim.Ĉm) + if !estim.direct + correct_estimate!(estim, y0m, d0) end return predict_estimate_kf!(estim, u0, d0, estim.Â) end @@ -871,7 +871,7 @@ step is skipped if `estim.direct == true` since it's already done by the user. ISBN9780470045343. """ function update_estimate!(estim::UnscentedKalmanFilter, u0, y0m, d0) - if !estim.direct && all(isfinite, y0m) + if !estim.direct correct_estimate!(estim, y0m, d0) end x̂0corr, X̂0corr, P̂corr = estim.x̂0, estim.X̂0, estim.cov.P̂ @@ -1200,7 +1200,7 @@ and prediction step equations are provided below. The correction step is skipped ``` """ function update_estimate!(estim::ExtendedKalmanFilter{NT}, u0, y0m, d0) where NT<:Real - if !estim.direct && all(isfinite, y0m) + if !estim.direct correct_estimate!(estim, y0m, d0) end cst_u0, cst_d0 = Constant(u0), Constant(d0) diff --git a/src/estimator/luenberger.jl b/src/estimator/luenberger.jl index 2eaa9d3a8..6f3c4db36 100644 --- a/src/estimator/luenberger.jl +++ b/src/estimator/luenberger.jl @@ -135,8 +135,8 @@ end Same than [`update_estimate!(::SteadyKalmanFilter)`](@ref) but using [`Luenberger`](@ref). """ function update_estimate!(estim::Luenberger, u0, y0m, d0) - if !estim.direct && all(isfinite, y0m) - correct_estimate_obsv!(estim, y0m, d0) + if !estim.direct + correct_estimate!(estim, y0m, d0) end return predict_estimate_obsv!(estim, u0, d0) end diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index ceb8565e2..9b8912ffa 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -119,6 +119,11 @@ end preparestate!(kalmanfilter4, [55, NaN]) ) @test all(kalmanfilter4.x̂0 .≈ 7) + kalmanfilter5 = SteadyKalmanFilter(linmodel, nint_ym=[1, 1], direct=false) + @test_logs( + (:warn, "NaN values in the Kalman filter measurements ym: skipping correction step"), + updatestate!(kalmanfilter5, [10, 50], [55, NaN]) + ) end @testitem "SKF set model" setup=[SetupMPCtests] begin @@ -252,6 +257,11 @@ end preparestate!(kalmanfilter4, [55, NaN]) ) @test all(kalmanfilter4.x̂0 .≈ 7) + kalmanfilter5 = KalmanFilter(linmodel, direct=false) + @test_logs( + (:warn, "NaN values in the Kalman filter measurements ym: skipping correction step"), + updatestate!(kalmanfilter5, [10,50], [55, NaN]) + ) end @testitem "KF set model" setup=[SetupMPCtests] begin @@ -376,6 +386,11 @@ end preparestate!(lo4, [55, NaN]) ) @test all(lo4.x̂0 .≈ 7) + lo5 = Luenberger(linmodel, nint_ym=[1, 1], direct=false) + @test_logs( + (:warn, "NaN values in the Luenberger measurements ym: skipping correction step"), + updatestate!(lo5, [10, 50], [55, NaN]) + ) end @testitem "Luenb. set model" setup=[SetupMPCtests] begin @@ -589,7 +604,7 @@ end @testitem "UKF estimator methods" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra - linmodel = LinModel(sys,Ts,i_u=[1,2]) + linmodel = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10, 50], yop=[50, 30]) function f!(xnext, x,u,_,model) mul!(xnext, model.A, x) mul!(xnext, model.Bu, u, 1, 1) @@ -651,6 +666,11 @@ end preparestate!(ukf4, [55, NaN]) ) @test all(ukf4.x̂0 .≈ 7) + ukf5 = UnscentedKalmanFilter(linmodel, direct=false) + @test_logs( + (:warn, "NaN values in the Kalman filter measurements ym: skipping correction step"), + updatestate!(ukf5, [10, 50], [55, NaN]) + ) end @testitem "UKF set model" setup=[SetupMPCtests] begin @@ -752,7 +772,7 @@ end using .SetupMPCtests, ControlSystemsBase, LinearAlgebra using DifferentiationInterface import FiniteDiff - linmodel = LinModel(sys,Ts,i_u=[1,2]) + linmodel = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10, 50], yop=[50,30]) function f!(xnext, x,u,_,model) mul!(xnext, model.A, x) mul!(xnext, model.Bu, u, 1, 1) @@ -819,6 +839,11 @@ end preparestate!(ekf5, [55, NaN]) ) @test all(ekf5.x̂0 .≈ 7) + ekf6 = ExtendedKalmanFilter(linmodel, direct=false) + @test_logs( + (:warn, "NaN values in the Kalman filter measurements ym: skipping correction step"), + updatestate!(ekf6, [10, 50], [55, NaN]) + ) end @testitem "EKF set model" setup=[SetupMPCtests] begin @@ -1072,12 +1097,17 @@ end @test x̂ ≈ [0, 0] atol=1e-3 @test isa(x̂, Vector{Float32}) - mhe4 = MovingHorizonEstimator(linmodel, He=2) + mhe4 = MovingHorizonEstimator(linmodel, He=2, direct=true) @test_logs( (:warn, "NaN values in the MHE measurements ym: ignoring them in the objective"), preparestate!(mhe4, [50, NaN], [5]) ) @test mhe4.x̂0 ≈ zeros(6) atol=1e-9 + mhe5 = MovingHorizonEstimator(linmodel, He=2, direct=false) + @test_logs( + (:warn, "NaN values in the MHE measurements ym: ignoring them in the objective"), + updatestate!(mhe5, [10, 50], [50, NaN], [5]) + ) end