added: handle NaN values in measured output ym for all state estimators

Project.toml

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

docs/src/public/generic_func.md

@@ -21,22 +21,22 @@ evaloutput
 
 ## Change State x
 
-### Prepare State x
+### Init State x
 
 ```@docs
-preparestate!
+initstate!
 ```
 
-### Update State x
+### Prepare State x
 
 ```@docs
-updatestate!
+preparestate!
 ```
 
-### Init State x
+### Update State x
 
 ```@docs
-initstate!
+updatestate!
 ```
 
 ### Set State x

src/controller/execute.jl

@@ -68,7 +68,7 @@ function moveinput!(
     R̂y = Rhaty,
     R̂u = Rhatu
 )
-    if mpc.estim.direct && !mpc.estim.corrected[]
+    if mpc.estim.direct && !mpc.estim.prepared[]
         @warn "preparestate! should be called before moveinput! with current estimators"
     end
     validate_args(mpc, ry, d, lastu, D̂, R̂y, R̂u)

src/estimator/execute.jl

@@ -252,7 +252,7 @@ julia> ŷ = evaloutput(kf)
 ```
 """
 function evaloutput(estim::StateEstimator{NT}, d=estim.buffer.empty) where NT <: Real
-    if estim.direct && !estim.corrected[]
+    if estim.direct && !estim.prepared[]
         @warn "preparestate! should be called before evaloutput with current estimators"
     end
     validate_args(estim.model, d)
@@ -278,7 +278,9 @@ delayed/predictor (2.) formulation:
 
 1. If `estim.direct` is `true`, it removes the operating points with [`remove_op!`](@ref),
    calls [`correct_estimate!`](@ref), and returns the corrected state estimate 
-   ``\mathbf{x̂}_k(k)``.
+   ``\mathbf{x̂}_k(k)``. The correction step is skipped if `isnothing(ym)`, in case the 
+   measured outputs are temporarily unavailable. The [`MovingHorizonEstimator`](@ref) and
+   [`InternalModel`](@ref) also support partial correction with `NaN` values in `ym`.
 2. Else, it does nothing and returns the current best estimate ``\mathbf{x̂}_{k-1}(k)``.
 
 # Examples
@@ -297,12 +299,13 @@ julia> x̂ = preparestate!(estim1, [1])
 ```
 """
 function preparestate!(estim::StateEstimator, ym, d=estim.buffer.empty)
+    isnothing(ym) && (ym = estim.buffer.ym; ym .= NaN) # nothing: skip correction step
     if estim.direct
         validate_args(estim, ym, d)
         y0m, d0 = remove_op!(estim, ym, d)
         correct_estimate!(estim, y0m, d0)
-        estim.corrected[] = true
     end
+    estim.prepared[] = true
     x̂  = estim.buffer.x̂
     x̂ .= estim.x̂0 .+ estim.x̂op
     return x̂
@@ -317,9 +320,11 @@ This function should be called at the end of each discrete time step. It removes
 operating points with [`remove_op!`](@ref), calls [`update_estimate!`](@ref) and returns the
 state estimate for the next time step ``\mathbf{x̂}_k(k+1)``. The method [`preparestate!`](@ref)
 should be called prior to this one to correct the estimate when applicable (if
-`estim.direct == true`). Note that the [`MovingHorizonEstimator`](@ref) with the default
-`direct=true` option is not able to estimate ``\mathbf{x̂}_k(k+1)``, the returned value
-is therefore the current corrected state ``\mathbf{x̂}_k(k)``.
+`estim.direct == true`). If `isnothing(ym)`, only the prediction step is performed in 
+[`update_estimate!`](@ref), for when the measured outputs are temporarily unavailable. Note
+that the [`MovingHorizonEstimator`](@ref) with the default `direct=true` option is not able
+to estimate ``\mathbf{x̂}_k(k+1)``, the returned value is therefore the current corrected
+state ``\mathbf{x̂}_k(k)``.
 
 # Examples
 ```jldoctest
@@ -334,13 +339,14 @@ julia> x̂ = updatestate!(kf, u, ym) # x̂[2] is the integrator state (nint_ym a
 ```
 """
 function updatestate!(estim::StateEstimator, u, ym, d=estim.buffer.empty)
-    if estim.direct && !estim.corrected[]
+    isnothing(ym) && (ym = estim.buffer.ym; ym .= NaN) # nothing: skip correction step
+    if estim.direct && !estim.prepared[]
         error("preparestate! must be called before updatestate! with direct=true option")
     end
     validate_args(estim, ym, d, u)
     y0m, d0, u0 = remove_op!(estim, ym, d, u)
-    update_estimate!(estim, y0m, d0, u0)
-    estim.corrected[] = false
+    update_estimate!(estim, u0, y0m, d0)
+    estim.prepared[] = false
     x̂next  = estim.buffer.x̂
     x̂next .= estim.x̂0 .+ estim.x̂op
     return x̂next

src/estimator/internal_model.jl

@@ -26,7 +26,7 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
     Âs::Matrix{NT}
     B̂s::Matrix{NT}
     direct::Bool
-    corrected::Vector{Bool}
+    prepared::Vector{Bool}
     buffer::StateEstimatorBuffer{NT}
     function InternalModel{NT}(
         model::SM, i_ym, Asm, Bsm, Csm, Dsm
@@ -45,7 +45,7 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
         x̂s, x̂snext = zeros(NT, nxs), zeros(NT, nxs)
         ŷs = zeros(NT, ny)
         direct = true # InternalModel always uses direct transmission from ym
-        corrected = [false]
+        prepared = [false]
         buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk)
         return new{NT, SM}(
             model, 
@@ -54,7 +54,7 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
             As, Bs, Cs, Ds, 
             Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm,
             Âs, B̂s,
-            direct, corrected,
+            direct, prepared,
             buffer
         )
     end
@@ -98,8 +98,9 @@ InternalModel estimator with a sample time Ts = 0.5 s:
     supposes 1 integrator per measured outputs by default, assuming that the current stochastic
     estimate ``\mathbf{ŷ_s^m}(k) = \mathbf{y^m}(k) - \mathbf{ŷ_d^m}(k)`` is constant in the 
     future. This is the dynamic matrix control (DMC) strategy, which is simple but sometimes
-    too aggressive. Additional poles and zeros in `stoch_ym` can mitigate this. The following
-    block diagram summarizes the internal model structure.
+    too aggressive. Additional poles and zeros in `stoch_ym` can mitigate this. If there
+    is a `NaN` in ``\mathbf{y^m}(k)``, its associated stochastic output will be `0`. The
+    following block diagram summarizes the internal model structure.
 
     ![block diagram of the internal model structure](../assets/imc.svg)
 """
@@ -253,28 +254,35 @@ function setmodel_estimator!(estim::InternalModel, model, _ , _ , _ , _ , _ )
     return nothing
 end
 
-"""
+@doc raw"""
     correct_estimate!(estim::InternalModel, y0m, d0)
 
 Compute the current stochastic output estimation `ŷs` for [`InternalModel`](@ref).
+
+It evaluates ``\mathbf{ŷ_s^m}(k) = \mathbf{y^m}(k) - \mathbf{ŷ_d^m}(k)`` and 
+``\mathbf{ŷ_s^u = 0}`` for the measured and unmeasured outputs, respectively. If there
+is a `NaN` in `y0m`, its associated stochastic output will be `0`.
 """
 function correct_estimate!(estim::InternalModel, y0m, d0)
+    if !all(isfinite, y0m)
+        @warn "NaN values in the internal model measurements ym: assigning them ŷs=0"
+    end
     ŷ0d = estim.buffer.ŷ
     ĥ!(ŷ0d, estim, estim.model, estim.x̂d, d0)
     ŷs = estim.ŷs
-    for j in eachindex(ŷs) # broadcasting was allocating unexpectedly, so we use a loop
-        if j in estim.i_ym
-            i = estim.i_ym[j]
-            ŷs[j] = y0m[i] - ŷ0d[j]
+    for i in eachindex(ŷs) # broadcasting was allocating unexpectedly, so we use a loop
+        if i in estim.i_ym
+            y0m_i = y0m[estim.i_ym[i]]
+            ŷs[i] = isfinite(y0m_i) ? y0m_i - ŷ0d[i] : 0
         else
-            ŷs[j] = 0
+            ŷs[i] = 0
         end
     end
     return nothing
 end
 
 @doc raw"""
-    update_estimate!(estim::InternalModel, _ , d0, u0)
+    update_estimate!(estim::InternalModel,  u0, _ , d0)
 
 Update `estim.x̂0`/`x̂d`/`x̂s` with current inputs `u0`, measured outputs `y0m` and dist. `d0`.
 
@@ -288,7 +296,7 @@ The [`InternalModel`](@ref) updates the deterministic `x̂d` and stochastic `x̂
 This estimator does not augment the state vector, thus ``\mathbf{x̂ = x̂_d}``. See 
 [`init_internalmodel`](@ref) for details. 
 """
-function update_estimate!(estim::InternalModel, _ , d0, u0)
+function update_estimate!(estim::InternalModel, u0, _ , d0)
     model = estim.model
     x̂d, x̂s, ŷs = estim.x̂d, estim.x̂s, estim.ŷs
     # -------------- deterministic model ---------------------
@@ -345,7 +353,7 @@ end
 
 # Compute estimated output with current stochastic estimate `estim.ŷs` for `InternalModel`
 function evaloutput(estim::InternalModel, d)
-    if !estim.corrected[]
+    if !estim.prepared[]
         @warn "preparestate! should be called before evaloutput with InternalModel"
     end
     validate_args(estim.model, d)