doc: clarify updatestate! and preparestate! doc

JuliaControl · Aug 5, 2024 · dc44d50 · dc44d50
1 parent 14fa165
commit dc44d50
Show file tree

Hide file tree

Showing 6 changed files with 92 additions and 57 deletions.
diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl
@@ -198,17 +198,37 @@ end
 
 Prepare `estim.x̂0` estimate with measured outputs `ym` and dist. `d` for current time step.
 
-This method does nothing if `estim.direct==false` (for delayed estimators). Otherwise, it
-removes the operating points with [`remove_op!`](@ref) and call [`prepare_estimate!`](@ref).
+This function should be called at the beginning of each discrete time step. Its behavior
+depends if `estim` is a [`StateEstimator`] in the current/filter (1) or delayed/predictor (2) 
+form:
+
+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)``.
+2. Else, it does nothing and returns the current best estimate ``\mathbf{x̂}_{k-1}(k)``.
+
+# Examples
+```jldoctest
+julia> estim1 = SteadyKalmanFilter(LinModel(ss(0.1, 0.5, 1, 0, 4)), nint_ym=0, direct=false);
+
+julia> preparestate!(estim1, [1])
+1-element Vector{Float64}:
+ 0.0
+
+julia> estim2 = SteadyKalmanFilter(LinModel(ss(0.1, 0.5, 1, 0, 4)), nint_ym=0, direct=true);
+
+julia> round.(preparestate!(estim2, [1]), digits=3)
+1-element Vector{Float64}:
+ 0.01
 """
 function preparestate!(estim::StateEstimator, ym, d=estim.model.dop)
     if estim.direct
         validate_args(estim, ym, d)
         y0m, d0 = remove_op!(estim, ym, d)
-        prepare_estimate!(estim, y0m, d0) # compute x̂0corr
+        correct_estimate!(estim, y0m, d0) # compute x̂0corr
     end
-    x̂   = copy(estim.x̂0)
-    x̂ .+= estim.x̂op
+    x̂  = estim.buffer.x̂
+    x̂ .= estim.x̂0 .+ estim.x̂op
     return x̂
 end
 
@@ -217,13 +237,18 @@ end
 
 Update `estim.x̂0` estimate with current inputs `u`, measured outputs `ym` and dist. `d`. 
 
-The method removes the operating points with [`remove_op!`](@ref) and call 
-[`update_estimate!`](@ref).
+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 (`estim.direct ==
+true`). 
 
 # Examples
 ```jldoctest
 julia> kf = SteadyKalmanFilter(LinModel(ss(0.1, 0.5, 1, 0, 4.0)));
 
+julia> preparestate!(kf, [0]);
+
 julia> x̂ = updatestate!(kf, [1], [0]) # x̂[2] is the integrator state (nint_ym argument)
 2-element Vector{Float64}:
  0.5
@@ -233,9 +258,9 @@ julia> x̂ = updatestate!(kf, [1], [0]) # x̂[2] is the integrator state (nint_y
 function updatestate!(estim::StateEstimator, u, ym, d=estim.buffer.empty)
     validate_args(estim, ym, d, u)
     y0m, d0, u0 = remove_op!(estim, ym, d, u)
-    x̂0next   = update_estimate!(estim, y0m, d0, u0)
-    x̂next   = x̂0next
-    x̂next .+= estim.x̂op
+    update_estimate!(estim, y0m, d0, u0)
+    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"))

diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl
@@ -238,7 +238,7 @@ end
 
 Save the current measured output `y0m` and disturbance `d0` for the stochastic predictions.
 """
-function prepare_estimate!(estim::InternalModel, y0m, d0)
+function correct_estimate!(estim::InternalModel, y0m, d0)
     estim.y0m .= y0m
     estim.d0  .= d0
     return nothing
@@ -259,21 +259,23 @@ 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{NT, SM}, y0m, d0, u0) where {NT<:Real, SM}
+function update_estimate!(estim::InternalModel, y0m, d0, u0)
     model = estim.model
     x̂d, x̂s = estim.x̂d, estim.x̂s
     # -------------- deterministic model ---------------------
-    ŷ0d, x̂dnext = Vector{NT}(undef, model.ny), Vector{NT}(undef, model.nx)
+    ŷ0d, ŷ0m, x̂dnext = estim.buffer.ŷ, estim.buffer.ym, estim.buffer.x̂
     h!(ŷ0d, model, x̂d, d0)
+    ŷ0m .= @views ŷ0d[estim.i_ym]
     f!(x̂dnext, model, x̂d, u0, d0) 
     x̂d .= x̂dnext # this also updates estim.x̂0 (they are the same object)
     # --------------- stochastic model -----------------------
-    x̂snext = Vector{NT}(undef, estim.nxs)
-    ŷs = zeros(NT, model.ny)
-    ŷs[estim.i_ym] = y0m - ŷ0d[estim.i_ym]   # ŷs=0 for unmeasured outputs
-    mul!(x̂snext, estim.Âs, x̂s)
+    ŷs  = estim.buffer.ŷ
+    ŷs .= 0     # ŷs=0 for unmeasured outputs
+    ŷs[estim.i_ym] .= y0m .- ŷ0m  
+    x̂snext = estim.Âs*x̂s # TODO: remove this allocation with a new buffer?
     mul!(x̂snext, estim.B̂s, ŷs, 1, 1)
     x̂s .= x̂snext
+    # --------------- operating points ---------------------
     x̂0next    = x̂dnext
     x̂0next  .+= estim.f̂op .- estim.x̂op
     estim.x̂0 .= x̂0next
@@ -301,12 +303,15 @@ This estimator does not augment the state vector, thus ``\mathbf{x̂ = x̂_d}``.
 function init_estimate!(estim::InternalModel, model::LinModel{NT}, y0m, d0, u0) where NT<:Real
     x̂d, x̂s = estim.x̂d, estim.x̂s
     # also updates estim.x̂0 (they are the same object):
+    # TODO: use estim.buffer.x̂ to reduce the allocation:
     x̂d .= (I - model.A)\(model.Bu*u0 + model.Bd*d0 + model.fop - model.xop)
-    ŷd0 = Vector{NT}(undef, model.ny)
+    ŷd0, ŷ0m = estim.buffer.ŷ, estim.buffer.ym
     h!(ŷd0, model, x̂d, d0)
-    ŷs = zeros(NT, model.ny)
-    ŷs[estim.i_ym] = y0m - ŷd0[estim.i_ym]  # ŷs=0 for unmeasured outputs
-    x̂s .= (I-estim.Âs)\estim.B̂s*ŷs
+    ŷ0m .= @views ŷd0[estim.i_ym]
+    ŷs  = estim.buffer.ŷ
+    ŷs .= 0     # ŷs=0 for unmeasured outputs
+    ŷs[estim.i_ym] .= y0m - ŷ0m
+    x̂s .= (I-estim.Âs)\estim.B̂s*ŷs # TODO: remove this allocation with a new buffer?
     return nothing
 end
 
@@ -321,8 +326,8 @@ estimate its outputs ``\mathbf{ŷ}(k)``, since the strategy imposes that
 store the current measured outputs and disturbances inside `estim` object, it should be thus
 called before `evalŷ`.
 """
-function evalŷ(estim::InternalModel{NT}, _ ) where NT<:Real
-    ŷ = Vector{NT}(undef, estim.model.ny)
+function evalŷ(estim::InternalModel, _ )
+    ŷ = estim.buffer.ŷ
     h!(ŷ, estim.model, estim.x̂d, estim.d0) 
     ŷ .+= estim.model.yop
     ŷ[estim.i_ym] = @views estim.y0m .+ estim.model.yop[estim.i_ym] 

diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl
@@ -192,21 +192,21 @@ end
 
 Prepare `estim.x̂0` with measured outputs `y0m` and disturbances `d0` for current time step.
 """
-function prepare_estimate!(estim::SteadyKalmanFilter, y0m, d0)
-    return prepare_estimate_obsv!(estim, y0m, d0)
+function correct_estimate!(estim::SteadyKalmanFilter, y0m, d0)
+    return correct_estimate_obsv!(estim, y0m, d0)
 end
 
 "Allow code reuse for `SteadyKalmanFilter` and `Luenberger` (observers with constant gain)."
-function prepare_estimate_obsv!(estim::StateEstimator, y0m, d0)
-    x̂0, K̂ = estim.x̂0, estim.K̂
-    ŷ0m = similar(y0m)
+function correct_estimate_obsv!(estim::StateEstimator, y0m, d0)
+    K̂ = estim.K̂
     Ĉm, D̂dm = @views estim.Ĉ[estim.i_ym, :], estim.D̂d[estim.i_ym, :]
+    ŷ0m = @views estim.buffer.ŷ[estim.i_ym]
     # in-place operations to reduce allocations:
-    mul!(ŷ0m, Ĉm, x̂0) 
+    mul!(ŷ0m, Ĉm, estim.x̂0) 
     mul!(ŷ0m, D̂dm, d0, 1, 1)
     v̂  = ŷ0m
     v̂ .= y0m .- ŷ0m
-    x̂0corr = x̂0
+    x̂0corr = estim.x̂0
     mul!(x̂0corr, K̂, v̂, 1, 1)
     return nothing
 end
@@ -229,11 +229,11 @@ end
 "Allow code reuse for `SteadyKalmanFilter` and `Luenberger` (observers with constant gain)."
 function update_estimate_obsv!(estim::StateEstimator, y0m, d0, u0)
     if !estim.direct
-        prepare_estimate_obsv!(estim, y0m, d0)
+        correct_estimate_obsv!(estim, y0m, d0)
     end
     x̂0corr = estim.x̂0
     Â, B̂u, B̂d = estim.Â, estim.B̂u, estim.B̂d
-    x̂0next = similar(estim.x̂0)
+    x̂0next = estim.buffer.x̂
     # in-place operations to reduce allocations:
     mul!(x̂0next, Â, x̂0corr)
     mul!(x̂0next, B̂u, u0, 1, 1)
@@ -382,9 +382,14 @@ function KalmanFilter(
     return KalmanFilter{NT, SM}(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct)
 end
 
-function prepare_estimate!(estim::KalmanFilter, y0m, d0)
+"""
+    correct_estimate!(estim::KalmanFilter, y0m, d0)
+
+
+"""
+function correct_estimate!(estim::KalmanFilter, y0m, d0)
     Ĉm = @views estim.Ĉ[estim.i_ym, :]
-    return prepare_estimate_kf!(estim, y0m, d0, Ĉm)
+    return correct_estimate_kf!(estim, y0m, d0, Ĉm)
 end
 
 
@@ -872,15 +877,13 @@ function ExtendedKalmanFilter(
     return ExtendedKalmanFilter{NT, SM}(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct)
 end
 
-function prepare_estimate!(estim::ExtendedKalmanFilter{NT}, y0m, d0) where NT<:Real
-    model = estim.model
-    nx̂, ny = estim.nx̂, model.ny
-    x̂0 = estim.x̂0
-    ŷ0 = Vector{NT}(undef, ny)
+function correct_estimate!(estim::ExtendedKalmanFilter, y0m, d0)
+    model, x̂0 = estim.model, estim.x̂0
+    ŷ0 = estim.buffer.ŷ
     ĥAD! = (ŷ0, x̂0) -> ĥ!(ŷ0, estim, model, x̂0, d0)
     ForwardDiff.jacobian!(estim.Ĥ, ĥAD!, ŷ0, x̂0)
     Ĥm = @views estim.Ĥ[estim.i_ym, :]
-    return prepare_estimate_kf!(estim, y0m, d0, Ĥm)
+    return correct_estimate_kf!(estim, y0m, d0, Ĥm)
 end
 
 
@@ -915,17 +918,17 @@ automatically computes the Jacobians:
 The matrix ``\mathbf{Ĥ^m}`` is the rows of ``\mathbf{Ĥ}`` that are measured outputs.
 """
 function update_estimate!(estim::ExtendedKalmanFilter{NT}, y0m, d0, u0) where NT<:Real
-    model = estim.model
-    nx̂, nu, ny = estim.nx̂, model.nu, model.ny
-    x̂0 = estim.x̂0
+    model, x̂0 = estim.model, estim.x̂0
+    nx̂, nu = estim.nx̂, model.nu
     # concatenate x̂0next and û0 vectors to allows û0 vector with dual numbers for AD:
+    # TODO: remove this allocation using estim.buffer
     x̂0nextû = Vector{NT}(undef, nx̂ + nu)
     f̂AD! = (x̂0nextû, x̂0) -> @views f̂!(
         x̂0nextû[1:nx̂], x̂0nextû[nx̂+1:end], estim, model, x̂0, u0, d0
     )
     ForwardDiff.jacobian!(estim.F̂_û, f̂AD!, x̂0nextû, x̂0)  
     if !estim.direct
-        ŷ0 = Vector{NT}(undef, ny)
+        ŷ0 = estim.buffer.ŷ
         ĥAD! = (ŷ0, x̂0) -> ĥ!(ŷ0, estim, model, x̂0, d0)
         ForwardDiff.jacobian!(estim.Ĥ, ĥAD!, ŷ0, x̂0)
     end
@@ -961,24 +964,22 @@ function validate_kfcov(nym, nx̂, Q̂, R̂, P̂_0=nothing)
 end
 
 
-function prepare_estimate_kf!(
-    estim::StateEstimator{NT}, y0m, d0, Ĉm, Â=estim.Â
-) where NT<:Real
+function correct_estimate_kf!(estim::StateEstimator, y0m, d0, Ĉm, Â=estim.Â)
     R̂, M̂, K̂ = estim.R̂, estim.M̂, estim.K̂
     x̂0, P̂ = estim.x̂0, estim.P̂
-    ŷ0 =  Vector{NT}(undef, estim.model.ny)
     mul!(M̂, P̂.data, Ĉm') # the ".data" weirdly removes a type instability in mul!
     rdiv!(M̂, cholesky!(Hermitian(Ĉm * P̂ * Ĉm' .+ R̂, :L)))
     if estim.direct
         K̂ .= M̂
     else
         mul!(K̂, Â, M̂)
     end
+    ŷ0 = estim.buffer.ŷ
     ĥ!(ŷ0, estim, estim.model, x̂0, d0)
     ŷ0m = @views ŷ0[estim.i_ym]
     v̂  = ŷ0m
     v̂ .= y0m .- ŷ0m
-    x̂0corr = x̂0
+    x̂0corr = estim.x̂0
     mul!(x̂0corr, K̂, v̂, 1, 1)
     return nothing
 end
@@ -995,14 +996,14 @@ substitutes the augmented model matrices with its Jacobians (`Â = F̂` and `C
 The implementation uses in-place operations and explicit factorization to reduce
 allocations. See e.g. [`KalmanFilter`](@ref) docstring for the equations.
 """
-function update_estimate_kf!(estim::StateEstimator{NT}, y0m, d0, u0, Ĉm, Â) where NT<:Real
+function update_estimate_kf!(estim::StateEstimator, y0m, d0, u0, Ĉm, Â)
     if !estim.direct
-        prepare_estimate_kf!(estim, y0m, d0, Ĉm, Â) # compute x̂0corr
+        correct_estimate_kf!(estim, y0m, d0, Ĉm, Â)
     end
     x̂0corr = estim.x̂0
     P̂, Q̂, M̂ = estim.P̂, estim.Q̂, estim.M̂
     nx̂, nu = estim.nx̂, estim.model.nu
-    x̂0next, û0 = Vector{NT}(undef, nx̂), Vector{NT}(undef, nu)
+    x̂0next, û0 = estim.buffer.x̂, estim.buffer.û
     f̂!(x̂0next, û0, estim, estim.model, x̂0corr, u0, d0)
     P̂next = Hermitian(Â * (P̂ .- M̂ * Ĉm * P̂) * Â' .+ Q̂, :L)
     x̂0next  .+= estim.f̂op .- estim.x̂op

diff --git a/src/estimator/luenberger.jl b/src/estimator/luenberger.jl
@@ -111,10 +111,10 @@ end
 """
     prepare_estimate_obsv!(estim::Luenberger, y0m, d0, _ )
 
-Same than [`prepare_estimate!(::SteadyKalmanFilter)`](@ref) but using [`Luenberger`](@ref).
+Same than [`correct_estimate!(::SteadyKalmanFilter)`](@ref) but using [`Luenberger`](@ref).
 """
-function prepare_estimate!(estim::Luenberger, y0m, d0)
-    return prepare_estimate_obsv!(estim, y0m, d0)
+function correct_estimate!(estim::Luenberger, y0m, d0)
+    return correct_estimate_obsv!(estim, y0m, d0)
 end
 
 

diff --git a/src/sim_model.jl b/src/sim_model.jl
@@ -218,11 +218,13 @@ function initstate!(model::SimModel, u, d=model.buffer.empty)
     return x
 end
 
-"""
+@doc raw"""
     updatestate!(model::SimModel, u, d=[]) -> x
 
 Update `model.x0` states with current inputs `u` and measured disturbances `d`.
 
+The method computes and returns the model state for the next time step ``\mathbf{x}(k+1)``.
+
 # Examples
 ```jldoctest
 julia> model = LinModel(ss(1.0, 1.0, 1.0, 0, 1.0));

diff --git a/src/state_estim.jl b/src/state_estim.jl
@@ -20,6 +20,7 @@ abstract type StateEstimator{NT<:Real} end
 
 struct StateEstimatorBuffer{NT<:Real}
     u ::Vector{NT}
+    û ::Vector{NT}
     x̂ ::Vector{NT}
     ym::Vector{NT}
     ŷ ::Vector{NT}
@@ -38,12 +39,13 @@ function StateEstimatorBuffer{NT}(
     nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int
 ) where NT <: Real
     u  = Vector{NT}(undef, nu)
+    û  = Vector{NT}(undef, nu)
     x̂  = Vector{NT}(undef, nx̂)
     ym = Vector{NT}(undef, nym)
     ŷ  = Vector{NT}(undef, ny)
     d  = Vector{NT}(undef, nd)
     empty = Vector{NT}(undef, 0)
-    return StateEstimatorBuffer{NT}(u, x̂, ym, ŷ, d, empty)
+    return StateEstimatorBuffer{NT}(u, û, x̂, ym, ŷ, d, empty)
 end
 
 const IntVectorOrInt = Union{Int, Vector{Int}}