Merge pull request #348 from JuliaControl/mhe_allocs

added : reduce allocations in `MovingHorizonEstimator` and `initstate!`

Author: franckgaga

src/estimator/construct.jl

@@ -1,8 +1,11 @@
 struct StateEstimatorBuffer{NT<:Real}
     u ::Vector{NT}
     û ::Vector{NT}
-    k::Vector{NT}
+    k ::Vector{NT}
     x̂ ::Vector{NT}
+    Z̃ ::Vector{NT}
+    V̂ ::Vector{NT}
+    X̂ ::Vector{NT}
     P̂ ::Matrix{NT}
     Q̂ ::Matrix{NT}
     R̂ ::Matrix{NT}
@@ -21,12 +24,19 @@ Create a buffer for `StateEstimator` objects for estimated states and measured o
 The buffer is used to store intermediate results during estimation without allocating.
 """
 function StateEstimatorBuffer{NT}(
-    nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int, nk::Int=0
+    nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int, nk::Int=0, He::Int=0, nε::Int=0
 ) where NT <: Real
+    nŵ = nx̂
+    nZ̃ = nx̂ + nŵ*He + nε
+    nV̂ = nym*He
+    nX̂ = nx̂*He
     u  = Vector{NT}(undef, nu)
     û  = Vector{NT}(undef, nu)
     k  = Vector{NT}(undef, nk)
     x̂  = Vector{NT}(undef, nx̂)
+    Z̃  = Vector{NT}(undef, nZ̃)
+    V̂  = Vector{NT}(undef, nV̂)
+    X̂  = Vector{NT}(undef, nX̂)
     P̂  = Matrix{NT}(undef, nx̂, nx̂)
     Q̂  = Matrix{NT}(undef, nx̂, nx̂)
     R̂  = Matrix{NT}(undef, nym, nym)
@@ -35,7 +45,7 @@ function StateEstimatorBuffer{NT}(
     ŷ  = Vector{NT}(undef, ny)
     d  = Vector{NT}(undef, nd)
     empty = Vector{NT}(undef, 0)
-    return StateEstimatorBuffer{NT}(u, û, k, x̂, P̂, Q̂, R̂, K̂, ym, ŷ, d, empty)
+    return StateEstimatorBuffer{NT}(u, û, k, x̂, Z̃, V̂, X̂, P̂, Q̂, R̂, K̂, ym, ŷ, d, empty)
 end
 
 "Include all the covariance matrices for the Kalman filters and moving horizon estimator."

src/estimator/execute.jl

@@ -213,8 +213,15 @@ measured outputs ``\mathbf{y^m}``.
 function init_estimate!(estim::StateEstimator, ::LinModel, y0m, d0, u0)
     Â, B̂u, B̂d = estim.Â, estim.B̂u, estim.B̂d
     Ĉm, D̂dm = estim.Ĉm, estim.D̂dm
-    # TODO: use estim.buffer.x̂ to reduce allocations
-    estim.x̂0 .= [I - Â; Ĉm]\[B̂u*u0 + B̂d*d0 + estim.f̂op - estim.x̂op; y0m - D̂dm*d0]
+    x̂_tmp, ŷ_tmp = estim.buffer.x̂, estim.buffer.ŷ
+    x̂_tmp .= estim.f̂op .- estim.x̂op
+    mul!(x̂_tmp, B̂u, u0, 1, 1)
+    mul!(x̂_tmp, B̂d, d0, 1, 1)
+    ŷm_tmp = @views ŷ_tmp[estim.i_ym]
+    mul!(ŷm_tmp, D̂dm, d0)
+    ŷm_tmp .= y0m .- ŷm_tmp
+    M = [I - Â; Ĉm]
+    estim.x̂0 .= M\[x̂_tmp; ŷm_tmp]
     return nothing
 end
 """

src/estimator/internal_model.jl

@@ -326,9 +326,12 @@ 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)
+    x̂_tmp = estim.buffer.x̂
+    x̂_tmp .= estim.f̂op .- estim.x̂op
+    mul!(x̂_tmp, model.Bu, u0, 1, 1)
+    mul!(x̂_tmp, model.Bd, d0, 1, 1)
+    M = I - model.A
+    x̂d .= M\x̂_tmp # also updates estim.x̂0 (they are the same object)
     ŷ0d = estim.buffer.ŷ
     h!(ŷ0d, model, x̂d, d0, model.p)
     ŷs = ŷ0d

src/estimator/mhe/construct.jl

@@ -160,7 +160,7 @@ struct MovingHorizonEstimator{
         nD0 = direct ? nd*(He+1) : nd*He
         U0, D0  = zeros(NT, nu*He), zeros(NT, nD0) 
         Ŵ = zeros(NT, nx̂*He)
-        buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk)
+        buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk, He, nε)
         x̂0arr_old = zeros(NT, nx̂)
         P̂arr_old = copy(cov.P̂_0)
         Nk = [0]

src/estimator/mhe/execute.jl

@@ -490,12 +490,10 @@ If first warm-starts the solver with [`set_warmstart_mhe!`](@ref). It then calls
 """
 function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real
     model, optim, buffer = estim.model, estim.optim, estim.buffer
-    nym, nx̂, nŵ, nε, Nk = estim.nym, estim.nx̂, estim.nx̂, estim.nε, estim.Nk[]
-    nx̃ = nε + nx̂
+    nŵ, nx̂, Nk =  estim.nx̂, estim.nx̂, estim.Nk[]
+    nx̃ = estim.nε + nx̂
     Z̃var::Vector{JuMP.VariableRef} = optim[:Z̃var]
-    V̂   = Vector{NT}(undef, nym*Nk)     # TODO: remove this allocation
-    X̂0  = Vector{NT}(undef, nx̂*Nk)      # TODO: remove this allocation
-    Z̃s = set_warmstart_mhe!(V̂, X̂0, estim, Z̃var)
+    Z̃s = set_warmstart_mhe!(estim, Z̃var)
     # ------- solve optimization problem --------------
     try
         JuMP.optimize!(optim)
@@ -534,14 +532,15 @@ function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real
     # --------- update estimate -----------------------
     û0, ŷ0, k = buffer.û, buffer.ŷ, buffer.k
     estim.Ŵ[1:nŵ*Nk] .= @views estim.Z̃[nx̃+1:nx̃+nŵ*Nk] # update Ŵ with optimum for warm-start
-    V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, estim.Z̃)
-    x̂0next    = @views X̂0[end-nx̂+1:end] 
+    V̂, X̂0 = estim.buffer.V̂, estim.buffer.X̂
+    predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, estim.Z̃)
+    x̂0next    = @views X̂0[Nk*nx̂-nx̂+1:Nk*nx̂]
     estim.x̂0 .= x̂0next
     return estim.Z̃
 end
 
 @doc raw"""
-    set_warmstart_mhe!(V̂, X̂0, estim::MovingHorizonEstimator, Z̃var) -> Z̃s
+    set_warmstart_mhe!(estim::MovingHorizonEstimator, Z̃var) -> Z̃s
 
 Set and return the warm-start value of `Z̃var` for [`MovingHorizonEstimator`](@ref).
 
@@ -564,11 +563,11 @@ computed at the last time step ``k-1``. If the objective function is not finite
 point, all the process noises ``\mathbf{ŵ}_{k-1}(k-j)`` are warm-started at zeros. The
 method mutates all the arguments.
 """
-function set_warmstart_mhe!(V̂, X̂0, estim::MovingHorizonEstimator{NT}, Z̃var) where NT<:Real
+function set_warmstart_mhe!(estim::MovingHorizonEstimator{NT}, Z̃var) where NT<:Real
     model, buffer = estim.model, estim.buffer
-    nε, nx̂, nŵ, nZ̃, Nk = estim.nε, estim.nx̂, estim.nx̂, length(estim.Z̃), estim.Nk[]
+    nε, nx̂, nŵ, Nk = estim.nε, estim.nx̂, estim.nx̂, estim.Nk[]
     nx̃ = nε + nx̂
-    Z̃s  = Vector{NT}(undef, nZ̃)  # TODO: remove this allocation
+    Z̃s = estim.buffer.Z̃
     û0, ŷ0, x̄, k = buffer.û, buffer.ŷ, buffer.x̂, buffer.k
     # --- slack variable ε ---
     estim.nε == 1 && (Z̃s[begin] = estim.Z̃[begin])
@@ -577,7 +576,8 @@ function set_warmstart_mhe!(V̂, X̂0, estim::MovingHorizonEstimator{NT}, Z̃var
     # --- process noise estimates Ŵ ---
     Z̃s[nx̃+1:end] = estim.Ŵ
     # verify definiteness of objective function:
-    V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, Z̃s)
+    V̂, X̂0 = estim.buffer.V̂, estim.buffer.X̂
+    predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim, model, Z̃s)
     Js = obj_nonlinprog!(x̄, estim, model, V̂, Z̃s)
     if !isfinite(Js)
         Z̃s[nx̃+1:end] = 0

src/model/linmodel.jl

@@ -266,10 +266,13 @@ disturbances ``\mathbf{d_0 = d - d_{op}}``. The Moore-Penrose pseudo-inverse com
 ``\mathbf{(I - A)^{-1}}`` to support integrating `model` (integrator states will be 0).
 """
 function steadystate!(model::LinModel, u0, d0)
+    x_tmp = model.buffer.x
+    x_tmp .= model.fop .- model.xop
+    mul!(x_tmp, model.Bu, u0, 1, 1)
+    mul!(x_tmp, model.Bd, d0, 1, 1)
     M = I - model.A
     rtol = sqrt(eps(real(float(oneunit(eltype(M)))))) # pinv docstring recommendation
-    #TODO: use model.buffer.x to reduce allocations
-    model.x0 .= pinv(M; rtol)*(model.Bu*u0 + model.Bd*d0 + model.fop - model.xop)
+    model.x0 .= pinv(M; rtol)*x_tmp
     return nothing
 end