diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index c8d97ff10..e3383e926 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -273,10 +273,16 @@ NonLinMPC controller with a sample time Ts = 10.0 s: `model` is a [`LinModel`](@ref)). The economic cost ``J_E`` and custom constraint ``\mathbf{g_c}`` functions receive the - extended vectors ``\mathbf{U_e}`` (`nu*Hp+nu` elements), ``\mathbf{Ŷ_e}`` (`ny+ny*Hp` - elements) and ``\mathbf{D̂_e}`` (`nd+nd*Hp` elements) as arguments. They all include the - values from ``k`` to ``k + H_p`` (inclusively). They also receives the slack ``ϵ`` - (scalar), which is always zero if `Cwt=Inf`. + extended vectors ``\mathbf{U_e}``, ``\mathbf{Ŷ_e}`` and ``\mathbf{D̂_e}`` as arguments. + They also receives the slack ``ϵ`` (scalar), which is always zero if `Cwt=Inf`. The + following table details the vector sizes and the time steps of the first and last data + point in them: + + | VECTOR | SIZE | FIRST TIME STEP | LAST TIME STEP | + | :--------------- | :------------- | :-------------- | :------------- | + | ``\mathbf{U_e}`` | `(nu*(Hp+1),)` | ``k + 0`` | ``k + H_p`` | + | ``\mathbf{Ŷ_e}`` | `(ny*(Hp+1),)` | ``k + 0`` | ``k + H_p`` | + | ``\mathbf{D̂_e}`` | `(nd*(Hp+1),)` | ``k + 0`` | ``k + H_p`` | More precisely, the last two time steps in ``\mathbf{U_e}`` are forced to be equal, i.e. ``\mathbf{u}(k+H_p) = \mathbf{u}(k+H_p-1)``, since ``H_c ≤ H_p`` implies that diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 3408b5524..fe77ab8fc 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -157,8 +157,7 @@ struct MovingHorizonEstimator{ Z̃ = zeros(NT, nZ̃) X̂op = repeat(x̂op, He) X̂0, Y0m = zeros(NT, nx̂*He), zeros(NT, nym*He) - nD0 = direct ? nd*(He+1) : nd*He - U0, D0 = zeros(NT, nu*He), zeros(NT, nD0) + U0, D0 = zeros(NT, nu*He), zeros(NT, nd*(He+1)) Ŵ = zeros(NT, nx̂*He) buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk, He, nε) x̂0arr_old = zeros(NT, nx̂) @@ -1038,14 +1037,14 @@ produces an estimator in the current form, while the prediction form is obtained &= \mathbf{E Z + F} \end{aligned} ``` -in which ``\mathbf{U_0}`` and ``\mathbf{Y_0^m}`` respectively include the deviation values of -the manipulated inputs ``\mathbf{u_0}(k-j+p)`` from ``j=N_k`` to ``1`` and measured outputs -``\mathbf{y_0^m}(k-j+1)`` from ``j=N_k`` to ``1``. The vector ``\mathbf{D_0}`` comprises one -additional measured disturbance if ``p=0``, that is, it includes the deviation vectors -``\mathbf{d_0}(k-j+1)`` from ``j=N_k+1-p`` to ``1``. The constant ``\mathbf{B}`` is the -contribution for non-zero state ``\mathbf{x̂_{op}}`` and state update ``\mathbf{f̂_{op}}`` -operating points (for linearization, see [`augment_model`](@ref) and [`linearize`](@ref)). -The method also returns the matrices for the estimation error at arrival: +in which ``\mathbf{U_0}`` and ``\mathbf{Y_0^m}`` respectively include the deviation values +of the manipulated inputs ``\mathbf{u_0}(k-j+p)`` from ``j=N_k`` to ``1`` and measured +outputs ``\mathbf{y_0^m}(k-j+1)`` from ``j=N_k`` to ``1``. The vector ``\mathbf{D_0}`` +includes the the measured disturbance deviation values ``\mathbf{d_0}(k-j)`` from from +``j=N_k`` to ``0``. The constant ``\mathbf{B}`` is the contribution for non-zero state +``\mathbf{x̂_{op}}`` and state update ``\mathbf{f̂_{op}}`` operating points (for linearization, +see [`augment_model`](@ref) and [`linearize`](@ref)). The method also returns the matrices +for the estimation error at arrival: ```math \mathbf{x̄} = \mathbf{x̂_0^†}(k-N_k+p) - \mathbf{x̂_0}(k-N_k+p) = \mathbf{e_x̄ Z + f_x̄} ``` @@ -1108,10 +1107,10 @@ see [`initpred!(::MovingHorizonEstimator, ::LinModel)`](@ref) and [`linconstrain \vdots & \vdots & \ddots & \vdots \\ \mathbf{Ĉ^m}\mathbf{Â}^{H_e-2}\mathbf{B̂_u} & \mathbf{Ĉ^m}\mathbf{Â}^{H_e-3}\mathbf{B̂_u} & \cdots & \mathbf{0} \end{bmatrix} \\ \mathbf{J} &= - \begin{bmatrix} - \mathbf{D̂_d^m} & \mathbf{0} & \cdots & \mathbf{0} \\ - \mathbf{Ĉ^m}\mathbf{Â}^{0}\mathbf{B̂_d} & \mathbf{D̂_d^m} & \cdots & \mathbf{0} \\ - \vdots & \vdots & \ddots & \vdots \\ - \mathbf{Ĉ^m}\mathbf{Â}^{H_e-2}\mathbf{B̂_d} & \mathbf{Ĉ^m}\mathbf{Â}^{H_e-3}\mathbf{B̂_d} & \cdots & \mathbf{D̂_d^m} \end{bmatrix} \\ + \mathbf{0} & \mathbf{D̂_d^m} & \mathbf{0} & \cdots & \mathbf{0} \\ + \mathbf{0} & \mathbf{Ĉ^m}\mathbf{Â}^{0}\mathbf{B̂_d} & \mathbf{D̂_d^m} & \cdots & \mathbf{0} \\ + \vdots & \vdots & \vdots & \ddots & \vdots \\ + \mathbf{0} & \mathbf{Ĉ^m}\mathbf{Â}^{H_e-2}\mathbf{B̂_d} & \mathbf{Ĉ^m}\mathbf{Â}^{H_e-3}\mathbf{B̂_d} & \cdots & \mathbf{D̂_d^m} \end{bmatrix} \\ \mathbf{B} &= - \begin{bmatrix} \mathbf{0} \\ \mathbf{Ĉ^m S}(0) \\ @@ -1138,8 +1137,8 @@ see [`initpred!(::MovingHorizonEstimator, ::LinModel)`](@ref) and [`linconstrain \vdots & \vdots & \ddots & \vdots \\ \mathbf{Â}^{H_e-1}\mathbf{B̂_d} & \mathbf{Â}^{H_e-2}\mathbf{B̂_d} & \cdots & \mathbf{Â}^{0}\mathbf{B̂_d} \end{bmatrix} \ , \quad \mathbf{J_x̂} = \begin{cases} - [\begin{smallmatrix} \mathbf{J_x̂^†} & \mathbf{0} \end{smallmatrix}] & p=0 \\ - \mathbf{J_x̂^†} & p=1 \end{cases} \\ + [\begin{smallmatrix} \mathbf{J_x̂^†} & \mathbf{0} \end{smallmatrix}] & p=0 \\ + [\begin{smallmatrix} \mathbf{0} & \mathbf{J_x̂^†} \end{smallmatrix}] & p=1 \end{cases} \\ \mathbf{B_x̂} &= \begin{bmatrix} \mathbf{S}(0) \\ \mathbf{S}(1) \\ @@ -1218,11 +1217,9 @@ function init_predmat_mhe( # --- measured disturbances D --- nĈm_Âpow_B̂d = reduce(vcat, getpower(nĈm_Âpow3D, i)*B̂d for i=0:He-1) nĈm_Âpow_B̂d = [-D̂dm; nĈm_Âpow_B̂d] - J = zeros(NT, nym*He, nd*(He+1-p)) - col_begin = iszero(p) ? 1 : 0 - col_end = iszero(p) ? He+1 : He - i=0 - for j=col_begin:col_end-1 + J = zeros(NT, nym*He, nd*(He+1)) + i = 0 + for j=1:He iRow = (1 + i*nym):(nym*He) iCol = (1:nd) .+ j*nd J[iRow, iCol] = nĈm_Âpow_B̂d[1:length(iRow) ,:] @@ -1230,10 +1227,10 @@ function init_predmat_mhe( end iszero(p) && @views (J[:, 1:nd] = nĈm_Âpow_B̂d[nym+1:end, :]) Âpow_B̂d = reduce(vcat, getpower(Âpow3D, i)*B̂d for i=0:He-1) - Jx̂ = zeros(NT, nx̂*He, nd*(He+1-p)) + Jx̂ = zeros(NT, nx̂*He, nd*(He+1)) for j=0:He-1 iRow = (1 + j*nx̂):(nx̂*He) - iCol = (1:nd) .+ j*nd + iCol = (1:nd) .+ j*nd .+ p Jx̂[iRow, iCol] = Âpow_B̂d[1:length(iRow) ,:] end # --- state x̂op and state update f̂op operating points --- diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index d55696dce..3a761c878 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -10,13 +10,10 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, _ , d0, u0) estim.H̃ .= 0 estim.q̃ .= 0 estim.r .= 0 - if estim.direct - # add u0(-1) and d0(-1) to the data windows: - estim.U0[1:estim.model.nu] .= u0 - estim.D0[1:estim.model.nd] .= d0 - end + estim.direct && (estim.U0[1:estim.model.nu] .= u0) # add u0(-1) to the data windows + estim.D0[1:estim.model.nd] .= d0 # add d0(-1) to the data windows estim.lastu0 .= u0 - # estim.cov.P̂_0 is in fact P̂(-1|-1) is estim.direct==false, else P̂(-1|0) + # estim.cov.P̂_0 is P̂(-1|-1) if estim.direct==false, else P̂(-1|0) invert_cov!(estim, estim.cov.P̂_0) estim.P̂arr_old .= estim.cov.P̂_0 estim.x̂0arr_old .= 0 @@ -323,10 +320,7 @@ function add_data_windows!(estim::MovingHorizonEstimator, y0m, d0, u0=estim.last estim.Nk .= estim.He else estim.Y0m[(1 + nym*(Nk-1)):(nym*Nk)] .= y0m - if nd > 0 - # D0 include 1 additional measured disturbance if direct==true (p==0): - estim.D0[(1 + nd*(Nk-p)):(nd*Nk+1-p)] .= d0 - end + nd > 0 && (estim.D0[(1 + nd*Nk):(nd*(Nk+1))] .= d0) estim.U0[(1 + nu*(Nk-1)):(nu*Nk)] .= u0 estim.X̂0[(1 + nx̂*(Nk-1)):(nx̂*Nk)] .= x̂0 estim.Ŵ[(1 + nŵ*(Nk-1)):(nŵ*Nk)] .= ŵ @@ -757,7 +751,7 @@ function predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim::MovingHorizonEstimator, m nu, nd, nx̂, nŵ, nym = model.nu, model.nd, estim.nx̂, estim.nx̂, estim.nym nx̃ = nε + nx̂ x̂0 = @views Z̃[nx̃-nx̂+1:nx̃] - if estim.direct + if estim.direct # p = 0 ŷ0next = ŷ0 d0 = @views estim.D0[1:nd] for j=1:Nk @@ -773,11 +767,11 @@ function predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim::MovingHorizonEstimator, m V̂[(1 + nym*(j-1)):(nym*j)] .= y0nextm .- ŷ0nextm x̂0, d0 = x̂0next, d0next end - else + else # p = 1 for j=1:Nk y0m = @views estim.Y0m[(1 + nym * (j-1)):(nym*j)] - d0 = @views estim.D0[ (1 + nd * (j-1)):(nd*j)] u0 = @views estim.U0[ (1 + nu * (j-1)):(nu*j)] + d0 = @views estim.D0[ (1 + nd*j):(nd*(j+1))] # 1st one is d(k-Nk), not used ŵ = @views Z̃[(1 + nx̃ + nŵ*(j-1)):(nx̃ + nŵ*j)] ĥ!(ŷ0, estim, model, x̂0, d0) ŷ0m = @views ŷ0[estim.i_ym]