Merge pull request #60 from JuliaControl/alloc_updatestate

remove one allocation in `updatestate!`

Author: franckgaga

src/controller/execute.jl

@@ -491,7 +491,7 @@ set_objective_linear_coef!(::PredictiveController, _ ) = nothing
 Call [`updatestate!`](@ref) on `mpc.estim` [`StateEstimator`](@ref).
 """
 function updatestate!(mpc::PredictiveController, u, ym, d=empty(mpc.estim.x̂0))
-    return updatestate!(mpc.estim,u,ym,d)
+    return updatestate!(mpc.estim, u, ym, d)
 end
 updatestate!(::PredictiveController, _ ) = throw(ArgumentError("missing measured outputs ym"))
 

src/estimator/execute.jl

@@ -209,10 +209,11 @@ julia> x̂ = updatestate!(kf, [1], [0]) # x̂[2] is the integrator state (nint_y
 """
 function updatestate!(estim::StateEstimator, u, ym, d=empty(estim.x̂0))
     validate_args(estim, u, ym, d)
-    u0, ym0, d0 = remove_op!(estim, u, ym, d) 
-    update_estimate!(estim, u0, ym0, d0)
-    estim.x̂0 .+= estim.f̂op .- estim.x̂op
-    return estim.x̂0 + estim.x̂op
+    u0, ym0, d0 = remove_op!(estim, u, ym, d)
+    x̂0next = update_estimate!(estim, u0, ym0, d0)
+    x̂next   = x̂0next
+    x̂next .+= estim.x̂op
+    return x̂next
 end
 updatestate!(::StateEstimator, _ ) = throw(ArgumentError("missing measured outputs ym"))
 

src/estimator/internal_model.jl

@@ -222,7 +222,7 @@ function setmodel_estimator!(estim::InternalModel, model::LinModel, _ , _ , _)
 end
 
 @doc raw"""
-    update_estimate!(estim::InternalModel, u0, y0m, d0=[])
+    update_estimate!(estim::InternalModel, u0, y0m, d0) -> x̂0next
 
 Update `estim.x̂0`/`x̂d`/`x̂s` with current inputs `u0`, measured outputs `y0m` and dist. `d0`.
 
@@ -236,9 +236,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{NT, SM}, u0, y0m, d0=empty(estim.x̂0)
-) where {NT<:Real, SM}
+function update_estimate!(estim::InternalModel{NT, SM}, u0, y0m, d0) where {NT<:Real, SM}
     model = estim.model
     x̂d, x̂s = estim.x̂d, estim.x̂s
     # -------------- deterministic model ---------------------
@@ -253,7 +251,10 @@ function update_estimate!(
     mul!(x̂snext, estim.Âs, x̂s)
     mul!(x̂snext, estim.B̂s, ŷs, 1, 1)
     x̂s .= x̂snext
-    return nothing
+    x̂0next    = x̂dnext
+    x̂0next  .+= estim.f̂op .- estim.x̂op
+    estim.x̂0 .= x̂0next
+    return x̂0next
 end
 
 @doc raw"""

src/estimator/kalman.jl

@@ -171,7 +171,7 @@ end
 
 
 @doc raw"""
-    update_estimate!(estim::SteadyKalmanFilter, u0, y0m, d0=[])
+    update_estimate!(estim::SteadyKalmanFilter, u0, y0m, d0) -> x̂0next
 
 Update `estim.x̂0` estimate with current inputs `u0`, measured outputs `y0m` and dist. `d0`.
 
@@ -181,7 +181,7 @@ The [`SteadyKalmanFilter`](@ref) updates it with the precomputed Kalman gain ``\
                + \mathbf{K̂}[\mathbf{y^m}(k) - \mathbf{Ĉ^m x̂}_{k-1}(k) - \mathbf{D̂_d^m d}(k)]
 ```
 """
-function update_estimate!(estim::SteadyKalmanFilter, u0, y0m, d0=empty(estim.x̂0))
+function update_estimate!(estim::SteadyKalmanFilter, u0, y0m, d0)
     Â, B̂u, B̂d = estim.Â, estim.B̂u, estim.B̂d
     x̂0, K̂ = estim.x̂0, estim.K̂
     ŷ0m, x̂0next = similar(y0m), similar(x̂0)
@@ -195,8 +195,9 @@ function update_estimate!(estim::SteadyKalmanFilter, u0, y0m, d0=empty(estim.x̂
     mul!(x̂0next, B̂u, u0, 1, 1)
     mul!(x̂0next, B̂d, d0, 1, 1)
     mul!(x̂0next, K̂, v̂, 1, 1)
+    x̂0next  .+= estim.f̂op .- estim.x̂op
     estim.x̂0 .= x̂0next
-    return nothing
+    return x̂0next
 end
 
 struct KalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT}
@@ -320,7 +321,7 @@ function KalmanFilter(model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂) where {
 end
 
 @doc raw"""
-    update_estimate!(estim::KalmanFilter, u0, y0m, d0=[])
+    update_estimate!(estim::KalmanFilter, u0, y0m, d0) -> x̂0next
 
 Update [`KalmanFilter`](@ref) state `estim.x̂0` and estimation error covariance `estim.P̂`.
 
@@ -344,7 +345,7 @@ control period ``k-1``. See [^2] for details.
 [^2]: Boyd S., "Lecture 8 : The Kalman Filter" (Winter 2008-09) [course slides], *EE363: 
      Linear Dynamical Systems*, <https://web.stanford.edu/class/ee363/lectures/kf.pdf>.
 """
-function update_estimate!(estim::KalmanFilter, u0, y0m, d0=empty(estim.x̂0))
+function update_estimate!(estim::KalmanFilter, u0, y0m, d0)
     Ĉm = @views estim.Ĉ[estim.i_ym, :]
     return update_estimate_kf!(estim, u0, y0m, d0, estim.Â, Ĉm)
 end
@@ -537,7 +538,7 @@ function init_ukf(::SimModel{NT}, nx̂, α, β, κ) where {NT<:Real}
 end
 
 @doc raw"""
-    update_estimate!(estim::UnscentedKalmanFilter, u0, y0m, d0=[])
+    update_estimate!(estim::UnscentedKalmanFilter, u0, y0m, d0) -> x̂0next
     
 Update [`UnscentedKalmanFilter`](@ref) state `estim.x̂0` and covariance estimate `estim.P̂`.
 
@@ -576,9 +577,7 @@ noise, respectively.
      Kalman, H∞, and Nonlinear Approaches", John Wiley & Sons, p. 433–459, <https://doi.org/10.1002/0470045345.ch14>, 
      ISBN9780470045343.
 """
-function update_estimate!(
-    estim::UnscentedKalmanFilter{NT}, u0, y0m, d0=empty(estim.x̂0)
-) where NT<:Real
+function update_estimate!(estim::UnscentedKalmanFilter{NT}, u0, y0m, d0) where NT<:Real
     x̂0, P̂, Q̂, R̂, K̂, M̂ = estim.x̂0, estim.P̂, estim.Q̂, estim.R̂, estim.K̂, estim.M̂
     nym, nx̂ = estim.nym, estim.nx̂
     γ, m̂, Ŝ = estim.γ, estim.m̂, estim.Ŝ
@@ -633,9 +632,10 @@ function update_estimate!(
     X̄next .= X̂0next .- x̂0next
     P̂next  = P̂cor
     P̂next.data .= X̄next * Ŝ * X̄next' .+ Q̂
+    x̂0next  .+= estim.f̂op .- estim.x̂op
     estim.x̂0 .= x̂0next
     estim.P̂  .= P̂next
-    return nothing
+    return x̂0next
 end
 
 struct ExtendedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
@@ -763,7 +763,7 @@ end
 
 
 @doc raw"""
-    update_estimate!(estim::ExtendedKalmanFilter, u0, y0m, d0=[])
+    update_estimate!(estim::ExtendedKalmanFilter, u0, y0m, d0) -> x̂0next
 
 Update [`ExtendedKalmanFilter`](@ref) state `estim.x̂0` and covariance `estim.P̂`.
 
@@ -792,9 +792,7 @@ automatically computes the Jacobians:
 ```
 The matrix ``\mathbf{Ĥ^m}`` is the rows of ``\mathbf{Ĥ}`` that are measured outputs.
 """
-function update_estimate!(
-    estim::ExtendedKalmanFilter{NT}, u0, y0m, d0=empty(estim.x̂0)
-) where NT<:Real
+function update_estimate!(estim::ExtendedKalmanFilter{NT}, u0, y0m, d0) where NT<:Real
     model = estim.model
     nx̂, nu, ny = estim.nx̂, model.nu, model.ny
     x̂0 = estim.x̂0
@@ -838,7 +836,7 @@ function validate_kfcov(nym, nx̂, Q̂, R̂, P̂_0=nothing)
 end
 
 """
-    update_estimate_kf!(estim::StateEstimator, u0, y0m, d0, Â, Ĉm)
+    update_estimate_kf!(estim::StateEstimator, u0, y0m, d0, Â, Ĉm) -> x̂0next
 
 Update time-varying/extended Kalman Filter estimates with augmented `Â` and `Ĉm` matrices.
 
@@ -848,9 +846,7 @@ substitutes the augmented model matrices with its Jacobians (`Â = 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}, u0, y0m, d0, Â, Ĉm
-) where NT<:Real
+function update_estimate_kf!(estim::StateEstimator{NT}, u0, y0m, d0, Â, Ĉm) where NT<:Real
     Q̂, R̂, M̂, K̂ = estim.Q̂, estim.R̂, estim.M̂, estim.K̂
     x̂0, P̂ = estim.x̂0, estim.P̂
     nx̂, nu, ny = estim.nx̂, estim.model.nu, estim.model.ny
@@ -865,7 +861,8 @@ function update_estimate_kf!(
     f̂!(x̂0next, û0, estim, estim.model, x̂0, u0, d0)
     mul!(x̂0next, K̂, v̂, 1, 1)
     P̂next = Hermitian(Â * (P̂ .- M̂ * Ĉm * P̂) * Â' .+ Q̂, :L)
+    x̂0next  .+= estim.f̂op .- estim.x̂op
     estim.x̂0 .= x̂0next
     estim.P̂  .= P̂next
-    return nothing
+    return x̂0next
 end

src/estimator/luenberger.jl

@@ -99,11 +99,11 @@ end
 
 
 """
-    update_estimate!(estim::Luenberger, u0, y0m, d0=[])
+    update_estimate!(estim::Luenberger, u0, y0m, d0) -> x̂0next
 
 Same than [`update_estimate!(::SteadyKalmanFilter)`](@ref) but using [`Luenberger`](@ref).
 """
-function update_estimate!(estim::Luenberger, u0, y0m, d0=empty(estim.x̂0))
+function update_estimate!(estim::Luenberger, u0, y0m, d0)
     Â, B̂u, B̂d = estim.Â, estim.B̂u, estim.B̂d
     x̂0, K̂ = estim.x̂0, estim.K̂
     Ĉm, D̂dm = @views estim.Ĉ[estim.i_ym, :], estim.D̂d[estim.i_ym, :]
@@ -117,8 +117,9 @@ function update_estimate!(estim::Luenberger, u0, y0m, d0=empty(estim.x̂0))
     mul!(x̂0next, B̂u, u0, 1, 1)
     mul!(x̂0next, B̂d, d0, 1, 1)
     mul!(x̂0next, K̂, v̂, 1, 1)
+    x̂0next  .+= estim.f̂op .- estim.x̂op
     estim.x̂0 .= x̂0next
-    return nothing
+    return x̂0next
 end
 
 "Throw an error if `setmodel!` is called on `Luenberger` observer."

src/estimator/mhe/execute.jl

@@ -17,7 +17,7 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, _ , _ , _ )
 end
 
 @doc raw"""
-    update_estimate!(estim::MovingHorizonEstimator, u0, y0m, d0=[])
+    update_estimate!(estim::MovingHorizonEstimator, u0, y0m, d0) -> x̂0next
     
 Update [`MovingHorizonEstimator`](@ref) state `estim.x̂0`.
 
@@ -28,9 +28,7 @@ augmented model from ``j = N_k-1`` to ``0`` inclusively. Afterward, if ``k ≥ H
 arrival covariance for the next time step ``\mathbf{P̂}_{k-N_k+1}(k-N_k+2)`` is estimated
 using `estim.covestim` object.
 """
-function update_estimate!(
-    estim::MovingHorizonEstimator{NT}, u0, y0m, d0=empty(estim.x̂0)
-) where NT<:Real
+function update_estimate!(estim::MovingHorizonEstimator{NT}, u0, y0m, d0) where NT<:Real
     model, optim, x̂0 = estim.model, estim.optim, estim.x̂0
     add_data_windows!(estim::MovingHorizonEstimator, u0, d0, y0m)
     initpred!(estim, model)
@@ -39,9 +37,9 @@ function update_estimate!(
     nx̃ = !isinf(estim.C) + nx̂
     Z̃var::Vector{JuMP.VariableRef} = optim[:Z̃var]
     V̂  = Vector{NT}(undef, nym*Nk)
-    X̂0  = Vector{NT}(undef, nx̂*Nk)
-    û0  = Vector{NT}(undef, nu)
-    ŷ0  = Vector{NT}(undef, ny)
+    X̂0 = Vector{NT}(undef, nx̂*Nk)
+    û0 = Vector{NT}(undef, nu)
+    ŷ0 = Vector{NT}(undef, ny)
     x̄  = Vector{NT}(undef, nx̂)
     ϵ_0 = isinf(estim.C) ? empty(estim.Z̃) : estim.Z̃[begin]
     Z̃_0 = [ϵ_0; estim.x̂0arr_old; estim.Ŵ]
@@ -82,10 +80,10 @@ function update_estimate!(
     # --------- update estimate -----------------------
     estim.Ŵ[1:nŵ*Nk] .= @views estim.Z̃[nx̃+1:nx̃+nŵ*Nk] # update Ŵ with optimum for warm-start
     V̂, X̂0 = predict!(V̂, X̂0, û0, ŷ0, estim, model, estim.Z̃)
-    # remove constant (f̂op - x̂op), since it is added in updatestate! method:
-    estim.x̂0 .= @views X̂0[end-nx̂+1:end] .- (estim.f̂op .- estim.x̂op)
     Nk == estim.He && update_cov!(estim::MovingHorizonEstimator)
-    return nothing
+    x̂0next    = X̂0[end-nx̂+1:end] 
+    estim.x̂0 .= x̂0next
+    return x̂0next
 end