added: low-level API correctstate! and predictstate!

docs/src/internals/state_estim.md

@@ -57,13 +57,7 @@ ModelPredictiveControl.remove_op!
 ModelPredictiveControl.init_estimate!
 ```
 
-## Correct Estimate
-
-```@docs
-ModelPredictiveControl.correct_estimate!
-```
-
-## Update Estimate
+## Correct and Predict Estimate
 
 !!! info
     All these methods assume that the `u0`, `y0m` and `d0` arguments are deviation vectors
@@ -74,5 +68,6 @@ ModelPredictiveControl.correct_estimate!
     ``\mathbf{x̂_0}``, respectively.
 
 ```@docs
-ModelPredictiveControl.update_estimate!
+ModelPredictiveControl.correct_estimate!
+ModelPredictiveControl.predict_estimate!
 ```

docs/src/public/generic_func.md

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

src/ModelPredictiveControl.jl

@@ -44,7 +44,10 @@ import FastGaussQuadrature
 export SimModel, LinModel, NonLinModel
 export DiffSolver, RungeKutta, ForwardEuler
 export setop!, setname!
-export setstate!, setmodel!, preparestate!, updatestate!, evaloutput, linearize, linearize!
+export linearize, linearize!
+export evaloutput
+export setstate!, preparestate!, updatestate!, correctstate!, predictstate!
+export setmodel!
 export savetime!, periodsleep
 export StateEstimator, InternalModel, Luenberger
 export SteadyKalmanFilter, KalmanFilter, UnscentedKalmanFilter, ExtendedKalmanFilter

src/controller/execute.jl

@@ -553,6 +553,15 @@ function preparestate!(mpc::PredictiveController, ym, d=mpc.estim.buffer.empty)
     return preparestate!(mpc.estim, ym, d)
 end
 
+"""
+    correctstate!(mpc::PredictiveController, ym, d=[]) -> x̂
+
+Call [`correctstate!`](@ref) on `mpc.estim` [`StateEstimator`](@ref).
+"""
+function correctstate!(mpc::PredictiveController, ym, d=mpc.estim.buffer.empty) 
+    return correctstate!(mpc.estim, ym, d)
+end
+
 @doc raw"""
     getinput!(mpc::PredictiveController, Z̃) -> u
 
@@ -584,6 +593,16 @@ function updatestate!(mpc::PredictiveController, u, ym, d=mpc.estim.buffer.empty
 end
 updatestate!(::PredictiveController, _ ) = throw(ArgumentError("missing measured outputs ym"))
 
+
+"""
+    predictstate!(mpc::PredictiveController, u, d=[]) -> x̂next
+
+Call [`predictstate!`](@ref) on `mpc.estim` [`StateEstimator`](@ref).
+"""
+function predictstate!(mpc::PredictiveController, u, d=mpc.estim.buffer.empty)
+    return predictstate!(mpc.estim, u, d)
+end
+
 """
     savetime!(mpc::PredictiveController) -> t
 

src/estimator/execute.jl

@@ -1,15 +1,13 @@
 """
-    remove_op!(estim::StateEstimator, ym, d, u=nothing) -> y0m, d0, u0
+    remove_op!(estim::StateEstimator, ym=nothing, d=nothing, u=nothing) -> y0m, d0, u0
 
 Remove operating pts on measured outputs `ym`, disturbances `d` and inputs `u` (if provided).
 """
-function remove_op!(estim::StateEstimator, ym, d, u=nothing)
+function remove_op!(estim::StateEstimator, ym=nothing, d=nothing, u=nothing)
     y0m, u0, d0 = estim.buffer.ym, estim.buffer.u, estim.buffer.d
-    y0m .= @views ym .- estim.model.yop[estim.i_ym]
-    d0  .= d  .- estim.model.dop
-    if !isnothing(u)
-        u0 .= u .- estim.model.uop
-    end
+    !isnothing(ym) && (y0m .= @views ym .- estim.model.yop[estim.i_ym])
+    !isnothing(d)  && (d0  .= d  .- estim.model.dop)
+    !isnothing(u)  && (u0  .= u .- estim.model.uop)
     return y0m, d0, u0
 end
 
@@ -301,25 +299,47 @@ function preparestate!(estim::StateEstimator, ym, d=estim.buffer.empty)
         validate_args(estim, ym, d)
         y0m, d0 = remove_op!(estim, ym, d)
         correct_estimate!(estim, y0m, d0)
-        estim.corrected[] = true
+        estim.corrected[] = true 
     end
     x̂  = estim.buffer.x̂
     x̂ .= estim.x̂0 .+ estim.x̂op
     return x̂
 end
 
+@doc raw"""
+    correctstate!(estim::StateEstimator, ym, d=[]) -> x̂
+
+Perform the correction step of `estim` with measured outputs `ym` and disturbances `d`.
+
+It removes the operating points with [`remove_op!`](@ref), calls [`correct_estimate!`](@ref),
+and returns the corrected state estimate ``\mathbf{x̂}_k(k)``. This is a lower-level function
+than [`preparestate!`](@ref). Be sure to call it before [`moveinput!`](@ref) if
+`estim.direct == true`, or after otherwise. This method is not supported by
+[`MovingHorizonEstimator`](@ref), use [`preparestate!`](@ref) instead.
+"""
+function correctstate!(estim::StateEstimator, ym, d=estim.buffer.empty)
+    validate_args(estim, ym, d)
+    y0m, d0 = remove_op!(estim, ym, d)
+    correct_estimate!(estim, y0m, d0)
+    estim.corrected[] = true 
+    x̂  = estim.buffer.x̂
+    x̂ .= estim.x̂0 .+ estim.x̂op
+    return x̂
+end
+
 @doc raw"""
     updatestate!(estim::StateEstimator, u, ym, d=[]) -> x̂next
 
 Update `estim.x̂0` estimate with current inputs `u`, measured outputs `ym` and dist. `d`. 
 
 This function should be called at the end of each discrete time step. It removes the 
-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)``.
+operating points with [`remove_op!`](@ref), calls [`correct_estimate!`](@ref) if 
+`estim.direct == false`, calls [`predict_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 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)``.
 
 # Examples
 ```jldoctest
@@ -339,14 +359,40 @@ function updatestate!(estim::StateEstimator, u, ym, d=estim.buffer.empty)
     end
     validate_args(estim, ym, d, u)
     y0m, d0, u0 = remove_op!(estim, ym, d, u)
-    update_estimate!(estim, y0m, d0, u0)
+    update_estimate!(estim, u0, y0m, d0)
     estim.corrected[] = false
     x̂next  = estim.buffer.x̂
     x̂next .= estim.x̂0 .+ estim.x̂op
     return x̂next
 end
 updatestate!(::StateEstimator, _ ) = throw(ArgumentError("missing measured outputs ym"))
 
+@doc raw"""
+    predictstate!(estim::StateEstimator, u, d=[]) -> x̂next
+
+Preform the prediction step of `estim` with current inputs `u` and disturbances `d`.
+
+It removes the operating points with [`remove_op!`](@ref), calls [`predict_estimate!`](@ref),
+and returns the state estimate for the next time step ``\mathbf{x̂}_k(k+1)``. This is a 
+lower-level function than [`updatestate!`](@ref). Be sure to call [`correctstate!`](@ref) 
+before this function if a new measurement is available, but the correction step can be
+skipped otherwise. This method is not supported by [`MovingHorizonEstimator`](@ref),
+use [`updatestate!`](@ref).
+"""
+function predictstate!(estim::StateEstimator, u, d=estim.buffer.empty)
+    validate_args(estim, estim.buffer.empty, d, u)
+    _ , d0, u0 = remove_op!(estim, nothing, d, u)
+    predict_estimate!(estim, u0, d0)
+    x̂next  = estim.buffer.x̂
+    x̂next .= estim.x̂0 .+ estim.x̂op
+    return x̂next
+end
+
+"Call [`correct_estimate!`](@ref) if `!direct` and [`predict_estimate!`](@ref) methods."
+function update_estimate!(estim::StateEstimator, u0, y0m, d0)
+    estim.direct || correct_estimate!(estim, y0m, d0)
+    return predict_estimate!(estim, u0, d0)
+end
 
 """
     savetime!(estim::StateEstimator) -> t

src/estimator/internal_model.jl

@@ -253,10 +253,13 @@ 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.
 """
 function correct_estimate!(estim::InternalModel, y0m, d0)
     ŷ0d = estim.buffer.ŷ
@@ -274,9 +277,9 @@ function correct_estimate!(estim::InternalModel, y0m, d0)
 end
 
 @doc raw"""
-    update_estimate!(estim::InternalModel, _ , d0, u0)
+    predict_estimate!(estim::InternalModel, u0, d0)
 
-Update `estim.x̂0`/`x̂d`/`x̂s` with current inputs `u0`, measured outputs `y0m` and dist. `d0`.
+Update `estim.x̂0`/`x̂d`/`x̂s` with current inputs `u0` and measured disturbances `d0`.
 
 The [`InternalModel`](@ref) updates the deterministic `x̂d` and stochastic `x̂s` estimates with:
 ```math
@@ -288,7 +291,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 predict_estimate!(estim::InternalModel, u0, d0)
     model = estim.model
     x̂d, x̂s, ŷs = estim.x̂d, estim.x̂s, estim.ŷs
     # -------------- deterministic model ---------------------