diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 8f7f602b8..4c3fbadf9 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1159,7 +1159,6 @@ function predict!( k0 = @views K0[(1 + nk*(j-1)):(nk*j)] x̂0next = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)] f̂!(x̂0next, û0, k0, mpc.estim, model, x̂0, u0, d0) - x̂0next .+= mpc.estim.f̂op .- mpc.estim.x̂op x̂0 = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)] d0 = @views D̂0[(1 + nd*(j-1)):(nd*j)] ŷ0 = @views Ŷ0[(1 + ny*(j-1)):(ny*j)] @@ -1318,8 +1317,6 @@ function con_nonlinprogeq!( x̂0next_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*j)] ŝnext = @views geq[(1 + nx̂*(j-1)):(nx̂*j)] f̂!(x̂0next, û0, k0, mpc.estim, model, x̂0, u0, d0) - # handle operating points (but should be zeros for NonLinModel): - x̂0next .+= mpc.estim.f̂op .- mpc.estim.x̂op ŝnext .= x̂0next .- x̂0next_Z̃ x̂0 = x̂0next_Z̃ # using states in Z̃ for next iteration (allow parallel for) d0 = d0next diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 0022b52d6..0bcf08c9a 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -19,18 +19,19 @@ end Mutating state function ``\mathbf{f̂}`` of the augmented model. By introducing an augmented state vector ``\mathbf{x̂_0}`` like in [`augment_model`](@ref), -the function returns the next state of the augmented model, defined as: +the function returns the next state of the augmented model, as deviation vectors: ```math \begin{aligned} - \mathbf{x̂_0}(k+1) &= \mathbf{f̂}\Big(\mathbf{x̂_0}(k), \mathbf{u_0}(k), \mathbf{d_0}(k)\Big) \\ + \mathbf{x̂_0}(k+1) &= \mathbf{f̂}\Big(\mathbf{x̂_0}(k), \mathbf{u_0}(k), \mathbf{d_0}(k)\Big) \mathbf{ŷ_0}(k) &= \mathbf{ĥ}\Big(\mathbf{x̂_0}(k), \mathbf{d_0}(k)\Big) \end{aligned} ``` where ``\mathbf{x̂_0}(k+1)`` is stored in `x̂0next` argument. The method mutates `x̂0next`, `û0` and `k0` in place. The argument `û0` stores the disturbed input of the augmented model ``\mathbf{û_0}``, and `k0`, the intermediate stage values of `model.solver`, when applicable. -The model parameter `model.p` is not included in the function signature for conciseness. See -Extended Help for details on ``\mathbf{û_0, f̂}`` and ``\mathbf{ĥ}`` implementations. +The model parameter `model.p` is not included in the function signature for conciseness. +The operating points are handled inside ``\mathbf{f̂}``. See Extended Help for details on +``\mathbf{û_0, f̂}`` and ``\mathbf{ĥ}`` implementations. # Extended Help !!! details "Extended Help" @@ -41,7 +42,8 @@ Extended Help for details on ``\mathbf{û_0, f̂}`` and ``\mathbf{ĥ}`` implem \begin{aligned} \mathbf{f̂}\Big(\mathbf{x̂_0}(k), \mathbf{u_0}(k), \mathbf{d_0}(k)\Big) &= \begin{bmatrix} \mathbf{f}\Big(\mathbf{x_0}(k), \mathbf{û_0}(k), \mathbf{d_0}(k), \mathbf{p}\Big) \\ - \mathbf{A_s} \mathbf{x_s}(k) \end{bmatrix} \\ + \mathbf{A_s} \mathbf{x_s}(k) \end{bmatrix} + + \mathbf{f̂_{op}} - \mathbf{x̂_{op}} \\ \mathbf{ĥ}\Big(\mathbf{x̂_0}(k), \mathbf{d_0}(k)\Big) &= \mathbf{h}\Big(\mathbf{x_0}(k), \mathbf{d_0}(k), \mathbf{p}\Big) + \mathbf{y_{s_y}}(k) \end{aligned} @@ -55,30 +57,33 @@ Extended Help for details on ``\mathbf{û_0, f̂}`` and ``\mathbf{ĥ}`` implem \end{aligned} ``` The ``\mathbf{f}`` and ``\mathbf{h}`` functions above are in fact the [`f!`](@ref) and - [`h!`](@ref) methods, respectively. + [`h!`](@ref) methods, respectively. The operating points ``\mathbf{x̂_{op}, f̂_{op}}`` + are computed by [`augment_model`](@ref) (almost always zeros in practice for + [`NonLinModel`](@ref)). """ function f̂!(x̂0next, û0, k0, estim::StateEstimator, model::SimModel, x̂0, u0, d0) - return f̂!(x̂0next, û0, k0, model, estim.As, estim.Cs_u, x̂0, u0, d0) + return f̂!(x̂0next, û0, k0, model, estim.As, estim.Cs_u, estim.f̂op, estim.x̂op, x̂0, u0, d0) end """ f̂!(x̂0next, _ , _ , estim::StateEstimator, model::LinModel, x̂0, u0, d0) -> nothing -Use the augmented model matrices if `model` is a [`LinModel`](@ref). +Use the augmented model matrices and operating points if `model` is a [`LinModel`](@ref). """ function f̂!(x̂0next, _ , _ , estim::StateEstimator, ::LinModel, x̂0, u0, d0) mul!(x̂0next, estim.Â, x̂0) mul!(x̂0next, estim.B̂u, u0, 1, 1) mul!(x̂0next, estim.B̂d, d0, 1, 1) + x̂0next .+= estim.f̂op .- estim.x̂op return nothing end """ - f̂!(x̂0next, û0, k0, model::SimModel, As, Cs_u, x̂0, u0, d0) + f̂!(x̂0next, û0, k0, model::SimModel, As, Cs_u, f̂op, x̂op, x̂0, u0, d0) Same than [`f̂!`](@ref) for [`SimModel`](@ref) but without the `estim` argument. """ -function f̂!(x̂0next, û0, k0, model::SimModel, As, Cs_u, x̂0, u0, d0) +function f̂!(x̂0next, û0, k0, model::SimModel, As, Cs_u, f̂op, x̂op, x̂0, u0, d0) # `@views` macro avoid copies with matrix slice operator e.g. [a:b] @views xd, xs = x̂0[1:model.nx], x̂0[model.nx+1:end] @views xdnext, xsnext = x̂0next[1:model.nx], x̂0next[model.nx+1:end] @@ -86,6 +91,7 @@ function f̂!(x̂0next, û0, k0, model::SimModel, As, Cs_u, x̂0, u0, d0) û0 .+= u0 # û0 = u0 + ys_u f!(xdnext, k0, model, xd, û0, d0, model.p) mul!(xsnext, As, xs) + x̂0next .+= f̂op .- x̂op return nothing end diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index b1c6b0976..c5963f2d2 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -172,8 +172,10 @@ State function ``\mathbf{f̂}`` of [`InternalModel`](@ref) for [`NonLinModel`](@ It calls [`f!`](@ref) directly since this estimator does not augment the states. """ -function f̂!(x̂0next, _ , k0, ::InternalModel, model::NonLinModel, x̂0, u0, d0) - return f!(x̂0next, k0, model, x̂0, u0, d0, model.p) +function f̂!(x̂0next, _ , k0, estim::InternalModel, model::NonLinModel, x̂0, u0, d0) + f!(x̂0next, k0, model, x̂0, u0, d0, model.p) + x̂0next .+= estim.f̂op .- estim.x̂op + return x̂0next end @doc raw""" diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index da01b769e..91ca91013 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -867,7 +867,6 @@ function update_estimate!(estim::UnscentedKalmanFilter, y0m, d0, u0) P̂next = estim.buffer.P̂ mul!(P̂next, X̄0next, Ŝ_X̄0nextᵀ) P̂next .+= Q̂ - x̂0next .+= estim.f̂op .- estim.x̂op estim.x̂0 .= x̂0next estim.cov.P̂ .= Hermitian(P̂next, :L) return nothing @@ -1068,11 +1067,17 @@ objects with the linearization points. """ function get_ekf_linfunc(NT, model, i_ym, nint_u, nint_ym, jacobian) As, Cs_u, Cs_y = init_estimstoch(model, i_ym, nint_u, nint_ym) - f̂_ekf!(x̂0next, x̂0, û0, k0, u0, d0) = f̂!(x̂0next, û0, k0, model, As, Cs_u, x̂0, u0, d0) - ĥ_ekf!(ŷ0, x̂0, d0) = ĥ!(ŷ0, model, Cs_y, x̂0, d0) + nxs = size(As, 1) + x̂op, f̂op = [model.xop; zeros(nxs)], [model.fop; zeros(nxs)] + f̂_ekf!(x̂0next, x̂0, û0, k0, u0, d0) = f̂!( + x̂0next, û0, k0, model, As, Cs_u, f̂op, x̂op, x̂0, u0, d0 + ) + ĥ_ekf!(ŷ0, x̂0, d0) = ĥ!( + ŷ0, model, Cs_y, x̂0, d0 + ) strict = Val(true) nu, ny, nd, nk = model.nu, model.ny, model.nd, model.nk - nx̂ = model.nx + size(As, 1) + nx̂ = model.nx + nxs x̂0next = zeros(NT, nx̂) ŷ0 = zeros(NT, ny) x̂0 = zeros(NT, nx̂) @@ -1232,7 +1237,6 @@ function predict_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter}, mul!(Â_P̂corr_Âᵀ, Â, P̂corr_Âᵀ) P̂next = estim.buffer.P̂ P̂next .= Â_P̂corr_Âᵀ .+ Q̂ - x̂0next .+= estim.f̂op .- estim.x̂op estim.x̂0 .= x̂0next estim.cov.P̂ .= Hermitian(P̂next, :L) return nothing diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 9d1551421..c54a056ff 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -610,7 +610,7 @@ function predict!(V̂, X̂0, û0, k0, ŷ0, estim::MovingHorizonEstimator, mode ŵ = @views Z̃[(1 + nx̃ + nŵ*(j-1)):(nx̃ + nŵ*j)] x̂0next = @views X̂0[(1 + nx̂ *(j-1)):(nx̂ *j)] f̂!(x̂0next, û0, k0, estim, model, x̂0, u0, d0) - x̂0next .+= ŵ .+ estim.f̂op .- estim.x̂op + x̂0next .+= ŵ y0nextm = @views estim.Y0m[(1 + nym * (j-1)):(nym*j)] d0next = @views estim.D0[(1 + nd*j):(nd*(j+1))] ĥ!(ŷ0next, estim, model, x̂0next, d0next) @@ -629,7 +629,7 @@ function predict!(V̂, X̂0, û0, k0, ŷ0, estim::MovingHorizonEstimator, mode V̂[(1 + nym*(j-1)):(nym*j)] .= y0m .- ŷ0m x̂0next = @views X̂0[(1 + nx̂ *(j-1)):(nx̂ *j)] f̂!(x̂0next, û0, k0, estim, model, x̂0, u0, d0) - x̂0next .+= ŵ .+ estim.f̂op .- estim.x̂op + x̂0next .+= ŵ x̂0 = x̂0next end end