added: handle NaN values in measured outputs ym for MovingHorizonEstimator

Project.toml

@@ -1,6 +1,6 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
-version = "2.6.0"
+version = "2.7.0"
 authors = ["Francis Gagnon"]
 
 [deps]

src/estimator/mhe/construct.jl

@@ -374,8 +374,10 @@ MovingHorizonEstimator estimator with a sample time Ts = 10.0 s:
     with ``p=1`` is particularly useful for the MHE since it moves its expensive
     computations after the MPC optimization. That is, [`preparestate!`](@ref) will solve the
     optimization by default, but it can be postponed to [`updatestate!`](@ref) with
-    `direct=false`.
-
+    `direct=false`. If a `NaN` value appears in the ``\mathbf{y^m}(k-j)`` vectors it will
+    be ignored in the objective function. An error will be thrown if it appears in
+    ``\mathbf{u}`` or ``\mathbf{d}`` vectors since they are arguments of the dynamics.
+
     The Extended Help of [`SteadyKalmanFilter`](@ref) details the tuning of the covariances
     and the augmentation with `nint_ym` and `nint_u` arguments. The default augmentation
     scheme is identical, that is `nint_u=0` and `nint_ym` computed by [`default_nint`](@ref).

src/estimator/mhe/execute.jl

@@ -331,11 +331,18 @@ Add data to the observation windows of the moving horizon estimator and clamp `e
 If ``k ≥ H_e``, the observation windows are moving in time and `estim.Nk` is clamped to
 `estim.He`. It returns `true` if the observation windows are moving, `false` otherwise.
 If no `u0` argument is provided, the manipulated input of the last time step is added to its
-window (the correct value if `estim.direct`).
+window (the correct value if `estim.direct`). 
 """
 function add_data_windows!(estim::MovingHorizonEstimator, y0m, d0, u0=estim.lastu0)
     model = estim.model
     nx̂, nym, nd, nu, nŵ = estim.nx̂, estim.nym, model.nd, model.nu, estim.nx̂
+    # --- check for NaN values in the arguments ---
+    any(isnan, u0) && throw(ArgumentError("NaN values in the MHE manipulated input u"))
+    if any(isnan, y0m)
+        @warn "NaN values in the MHE measurements ym: ignoring them in the objective"
+    end
+    any(isnan, d0) && throw(ArgumentError("NaN values in the MHE measured disturbance d"))
+    # --- data windows for the predictions ---
     yopm = @views model.yop[estim.i_ym]
     Nk = estim.Nk[]
     p = estim.direct ? 0 : 1 # u0 argument is u0(k-1) if estim.direct, else u0(k)
@@ -344,7 +351,6 @@ function add_data_windows!(estim::MovingHorizonEstimator, y0m, d0, u0=estim.last
     estim.Nk .+= 1
     Nk = estim.Nk[]
     ismoving = (Nk > estim.He)
-    # --- data windows for the predictions ---
     # see MovingHorzionEstimator extended help for the exact time steps in each data window
     if ismoving
         estim.Y0m[1:end-nym]        .= @views estim.Y0m[nym+1:end]
@@ -378,6 +384,7 @@ function add_data_windows!(estim::MovingHorizonEstimator, y0m, d0, u0=estim.last
         estim.Ŵ[(1 + nŵ*(Nk-1)):(nŵ*Nk)]                .= ŵ
         estim.X̂0_old[(1 + nx̂*(Nk-1)):(nx̂*Nk)]           .= x̂0_old
     end
+    # --- update the arrival state estimated at k-Nk ---
     estim.x̂0arr_old .= @views estim.X̂0_old[1:nx̂]
     return ismoving
 end
@@ -446,6 +453,12 @@ function initpred!(estim::MovingHorizonEstimator, model::LinModel)
     mul!(F, G, U0, 1, 1)
     (model.nd > 0) && mul!(F, J, D0, 1, 1)
     fx̄ .= estim.x̂0arr_old
+    if any(isnan, F) # ignore NaN values in V̂ for the objective function:
+        i_nan = findall(isnan, F)
+        Ẽ, F = copy(Ẽ), copy(F)
+        Ẽ[i_nan, :]  .= 0
+        F[i_nan]     .= 0
+    end
     # --- update H̃, q̃ and p vectors for quadratic optimization ---
     ẼZ̃ = [ẽx̄; Ẽ]
     FZ̃ = [fx̄; F]
@@ -796,6 +809,9 @@ function obj_nonlinprog(estim::MovingHorizonEstimator, ::SimModel, x̄, V̂, Ŵ
         nŴ, nYm = Nk*estim.nx̂, Nk*estim.nym
         Ŵ, V̂ = Ŵ[1:nŴ], V̂[1:nYm]
     end
+    if any(isnan, V̂) # ignore NaN values in V̂ for the objective function:
+        V̂ = [isnan(v) ? 0 : v for v in V̂]
+    end
     Jε = estim.nε > 0 ? estim.C*Z̃[begin]^2 : 0
     return dot(x̄, invP̄, x̄) + dot(Ŵ, invQ̂_Nk, Ŵ) + dot(V̂, invR̂_Nk, V̂) + Jε
 end

test/2_test_state_estim.jl

@@ -1029,6 +1029,14 @@ end
     x̂ = updatestate!(mhe3, [0], [0])
     @test x̂ ≈ [0, 0] atol=1e-3
     @test isa(x̂, Vector{Float32})
+
+    mhe4 = MovingHorizonEstimator(linmodel, He=2)
+    @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
+
 end
 
 @testitem "MHE estimation and getinfo (NonLinModel)" setup=[SetupMPCtests] begin
@@ -1138,6 +1146,13 @@ end
     @test_nowarn ModelPredictiveControl.info2debugstr(info)
     @test_throws ErrorException setstate!(mhe1, [1,2,3,4,5,6], diagm(.1:.1:.6))
 
+    mhe7 = MovingHorizonEstimator(nonlinmodel, He=2)
+    @test_logs(
+        (:warn, "NaN values in the MHE measurements ym: ignoring them in the objective"),
+        preparestate!(mhe7, [50, NaN], [5])
+    )
+    @test mhe7.x̂0 ≈ zeros(6) atol=1e-9
+
 end
 
 @testitem "MHE estimation with unfilled window" setup=[SetupMPCtests] begin