debug: print warning about NaN values in ym when direct=false

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]

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.
      <https://en.wikipedia.org/wiki/Kalman_filter>, 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)

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

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