diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index c6e0d2219..0bc8878a6 100644 --- a/src/estimator/construct.jl +++ b/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." diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 4fb761fab..3ad4bb5c1 100644 --- a/src/estimator/execute.jl +++ b/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 """ diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index 3e67db6f9..7233a59d0 100644 --- a/src/estimator/internal_model.jl +++ b/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 diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 2419af99d..3408b5524 100644 --- a/src/estimator/mhe/construct.jl +++ b/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] diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index f140533e0..d55696dce 100644 --- a/src/estimator/mhe/execute.jl +++ b/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 diff --git a/src/model/linmodel.jl b/src/model/linmodel.jl index a1f86e76d..cfc515010 100644 --- a/src/model/linmodel.jl +++ b/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